Changeset 101 in flair-src for trunk/lib/FlairMeta


Ignore:
Timestamp:
Oct 5, 2016, 5:20:41 PM (5 years ago)
Author:
Sanahuja Guillaume
Message:

maj quaternion 3dmgx3

Location:
trunk/lib/FlairMeta/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairMeta/src/HdsX8.cpp

    r100 r101  
    6262          }
    6363  */
    64   SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
     64  //SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
    6565}
    6666
     
    7070  ((Gx3_25_ahrs *)GetAhrs())->Start();
    7171  ((Srf08 *)GetUsRangeFinder())->Start();
    72   ((Ps3Eye *)GetVerticalCamera())->Start();
     72  //((Ps3Eye *)GetVerticalCamera())->Start();
    7373  Uav::StartSensors();
    7474}
  • trunk/lib/FlairMeta/src/UavStateMachine.cpp

    r45 r101  
    182182
    183183    //check nan/inf problems
    184     if(IsValuePossible(currentTorques.roll,"roll torque")
    185        || IsValuePossible(currentTorques.pitch,"pitch torque")
    186        || IsValuePossible(currentTorques.yaw,"yaw torque")
    187        || IsValuePossible(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       }
     184    if(!IsValuePossible(currentTorques.roll,"roll torque")
     185       || !IsValuePossible(currentTorques.pitch,"pitch torque")
     186       || !IsValuePossible(currentTorques.yaw,"yaw torque")
     187       || !IsValuePossible(currentThrust,"thrust")) {
     188
     189        if(altitudeState==AltitudeState_t::Stopped) {
     190            SafeStop();
     191        } else {
     192
     193          if(failSafeMode) {
     194            Warn("We are already in safe mode, the uav is going to crash!\n");
     195          } else {
     196            Thread::Warn("switching back to safe mode\n");
     197            EnterFailSafeMode();
     198            needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
     199            needToComputeDefaultThrust = true;
     200
     201            ComputeTorques();
     202            ComputeThrust();
     203          }
     204        }
    200205    }
    201206
     
    221226  if(isnan(value)) {
    222227    Warn("%s is not an number\n",desc.c_str());
    223     return true;
     228    return false;
    224229  } else if(isinf(value)) {
    225230    Warn("%s is infinite\n",desc.c_str());
     231    return false;
     232  } else {
    226233    return true;
    227   } else {
    228     return false;
    229234  }
    230235}
Note: See TracChangeset for help on using the changeset viewer.