Changeset 42 in flair-src for trunk/lib/FlairMeta/src/UavStateMachine.cpp


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

m

File:
1 edited

Legend:

Unmodified
Added
Removed
  • 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
Note: See TracChangeset for help on using the changeset viewer.