Changeset 42 in flair-src


Ignore:
Timestamp:
Jul 5, 2016, 8:50:58 AM (4 years ago)
Author:
Sanahuja Guillaume
Message:

m

Location:
trunk
Files:
2 added
9 edited

Legend:

Unmodified
Added
Removed
  • trunk/demos/CircleFollower/simulator/CMakeLists.txt

    r23 r42  
    1111SET(SRC_FILES
    1212        ${CMAKE_SOURCE_DIR}/src/main.cpp
    13 )
    14 
    15 LINK_DIRECTORIES (
    16         ${FLAIR_LIBRARY_DIR}
    1713)
    1814
  • trunk/demos/CircleFollower/uav/CMakeLists.txt

    r23 r42  
    1414        ${CMAKE_SOURCE_DIR}/src/main.cpp
    1515        ${CMAKE_SOURCE_DIR}/src/CircleFollower.cpp
    16 )
    17 
    18 LINK_DIRECTORIES (
    19         ${FLAIR_LIBRARY_DIR}
    2016)
    2117
  • trunk/demos/SimpleFleet/simulator/CMakeLists.txt

    r33 r42  
    1111SET(SRC_FILES
    1212        ${CMAKE_SOURCE_DIR}/src/main.cpp
    13 )
    14 
    15 LINK_DIRECTORIES (
    16         ${FLAIR_LIBRARY_DIR}
    1713)
    1814
  • trunk/demos/SimpleFleet/uav/CMakeLists.txt

    r33 r42  
    1414        ${CMAKE_SOURCE_DIR}/src/main.cpp
    1515        ${CMAKE_SOURCE_DIR}/src/SimpleFleet.cpp
    16 )
    17 
    18 LINK_DIRECTORIES (
    19         ${FLAIR_LIBRARY_DIR}
    2016)
    2117
  • trunk/lib/FlairMeta/src/UavStateMachine.cpp

    r38 r42  
    4242#include <stdio.h>
    4343#include <TrajectoryGenerator1D.h>
     44#include <math.h>
    4445
    4546using namespace std;
     
    180181    ComputeThrust(); // logs are added to uz, so it must be updated at last
    181182
     183    //check nan problems
     184    if(CheckIsNan(currentTorques.roll,"roll torque")
     185       || CheckIsNan(currentTorques.pitch,"pitch torque")
     186       || CheckIsNan(currentTorques.yaw,"yaw torque")
     187       || CheckIsNan(currentThrust,"thrust")) {
     188
     189      if(failSafeMode) {
     190        Warn("We are already in safe mode, the uav is going to crash!\n");
     191      } else {
     192        Thread::Warn("switching back to safe mode\n");
     193        EnterFailSafeMode();
     194        needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
     195        needToComputeDefaultThrust = true;
     196
     197        ComputeTorques();
     198        ComputeThrust();
     199      }
     200    }
     201
    182202    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
    183203    // thrust (value between 0 and 1)
     
    198218}
    199219
     220bool UavStateMachine::CheckIsNan(float value,std::string desc) {
     221  if(isnan(value)) {
     222    Warn("%s is not an number\n",desc.c_str());
     223    return true;
     224  } else {
     225    return false;
     226  }
     227}
     228
     229
    200230void UavStateMachine::ComputeOrientation(void) {
    201231  if (failSafeMode) {
     
    254284      // The Uav target altitude has reached its landing value (typically 0)
    255285      // but the real Uav altitude may not have reach this value yet because of
    256       // command delay. Moreover, it may never exactly reach this value if the 
     286      // command delay. Moreover, it may never exactly reach this value if the
    257287      // ground is not perfectly leveled (critical case: there's a
    258288      // deep and narrow hole right in the sensor line of sight). That's why we
     
    547577        EnterFailSafeMode();
    548578        Land();
    549     }
     579    }/*
    550580    Time now=GetTime();
    551581    if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
     
    553583        Thread::Err("Critical sensor lost\n");
    554584        EnterFailSafeMode();
    555         EmergencyLand(); 
    556     }
     585        EmergencyLand();
     586    }*/
    557587}
    558588
  • trunk/lib/FlairMeta/src/UavStateMachine.h

    r38 r42  
    261261  void Run(void);
    262262  void StopMotors(void);
     263  bool CheckIsNan(float value,std::string desc);
    263264
    264265  meta::Uav *uav;
  • trunk/lib/FlairSensorActuator/src/Gps.cpp

    r15 r42  
    216216  result = nmea_parse(&parser, frame, frame_size, &info);
    217217  if (result != 1) {
    218     Warn("unrecognized nmea sentence\n");
    219     Warn("%s\n", frame);
     218    Warn("unrecognized nmea sentence: %s\n",frame);
    220219  }
    221220
  • trunk/lib/FlairSensorActuator/src/Mb800.cpp

    r41 r42  
    5353      Thread::Err("erreur Write (%s)\n", strerror(-written));
    5454  }
    55 /*
     55
    5656  {
    5757    char to_send[] = "$PASHS,CPD,AFP,95.0\r\n";
     
    6868    }
    6969  }
    70 */
     70
    7171  if ((NMEAFlags & GGA) != 0) {
    7272    char to_send[] = "$PASHS,NME,GGA,A,ON,0.05\r\n";
     
    9999
    100100  while (!ToBeStopped()) {
    101     //Printf("loop\n");
    102101    SleepMS(10);
    103     size = 0;/*
     102    size = 0;
    104103    while (!ToBeStopped()) {
    105104      ssize_t read = serialport->Read(&response[size], 1);
     
    111110        break;
    112111      size++;
    113     }*/
     112    }
    114113    size++;
    115     //parseFrame(response, size);
     114    parseFrame(response, size);
    116115  }
    117116  /** fin running loop **/
     
    122121  char data = 0;
    123122  ssize_t read = 0;
    124 Printf("sync\n");
     123
    125124  // attente fin trame
    126125  while (data != 0x0a && !ToBeStopped()) {
    127126    read = serialport->Read(&data, 1);
    128127    SleepMS(10);
    129     Printf("%i %x\n",read,data);
     128
    130129    if (read < 0) {
    131130      Thread::Err("erreur Read (%s)\n", strerror(-read));
  • trunk/lib/FlairSensorActuator/src/TargetController.cpp

    r38 r42  
    3939    TabWidget* tab=new TabWidget(main_tab->NewRow(),name);
    4040    setup_tab=new Tab(tab,"Reglages");
    41     SetPeriodMS(20);; //50Hz
    4241}
    4342
     
    115114
    116115void TargetController::Run() {
     116  SetPeriodMS(20);; //50Hz
     117
    117118  Message *message;
    118119
Note: See TracChangeset for help on using the changeset viewer.