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


Ignore:
Timestamp:
04/08/16 15:40:57 (8 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairMeta/src/UavStateMachine.cpp

    r10 r15  
    5151using namespace flair::meta;
    5252
    53 UavStateMachine::UavStateMachine(Uav* inUav,uint16_t ds3Port):
    54         Thread(getFrameworkManager(),"UavStateMachine",50),
    55         uav(inUav),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagZTrajectoryFinished(false) {
    56     altitudeState=AltitudeState_t::Stopped;
    57     uav->UseDefaultPlot();
    58 
    59     Tab *uavTab=new Tab(getFrameworkManager()->GetTabWidget(),"uav",0);
    60     buttonslayout=new GridLayout(uavTab->NewRow(),"buttons");
    61     button_kill=new PushButton(buttonslayout->NewRow(),"kill");
    62     button_start_log=new PushButton(buttonslayout->NewRow(),"start_log");
    63     button_stop_log=new PushButton(buttonslayout->LastRowLastCol(),"stop_log");
    64     button_take_off=new PushButton(buttonslayout->NewRow(),"take_off");
    65     button_land=new PushButton(buttonslayout->LastRowLastCol(),"land");
    66 
    67     Tab *lawTab=new Tab(getFrameworkManager()->GetTabWidget(),"control laws");
    68     TabWidget *tabWidget=new TabWidget(lawTab->NewRow(),"laws");
    69     setupLawTab=new Tab(tabWidget,"Setup");
    70     graphLawTab=new Tab(tabWidget,"Graphes");
    71 
    72     uRoll=new NestedSat(setupLawTab->At(0,0),"u_roll");
    73     uRoll->ConvertSatFromDegToRad();
    74     uRoll->UseDefaultPlot(graphLawTab->NewRow());
    75 
    76     uPitch=new NestedSat(setupLawTab->At(0,1),"u_pitch");
    77     uPitch->ConvertSatFromDegToRad();
    78     uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
    79 
    80     uYaw=new Pid(setupLawTab->At(0,2),"u_yaw");
    81     uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
    82 
    83     uZ=new PidThrust(setupLawTab->At(1,2),"u_z");
    84     uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
    85 
    86     getFrameworkManager()->AddDeviceToLog(uZ);
    87     uZ->AddDeviceToLog(uRoll);
    88     uZ->AddDeviceToLog(uPitch);
    89     uZ->AddDeviceToLog(uYaw);
    90 
    91     joy=new MetaDualShock3(getFrameworkManager(),"dualshock3",ds3Port,30);
    92     uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
    93 
    94     altitudeMode=AltitudeMode_t::Manual;
    95     orientationMode=OrientationMode_t::Manual;
    96     thrustMode=ThrustMode_t::Default;
    97     torqueMode=TorqueMode_t::Default;
    98 
    99     GroupBox* reglagesGroupbox=new GroupBox(uavTab->NewRow(),"takeoff/landing");
    100     desiredTakeoffAltitude=new DoubleSpinBox(reglagesGroupbox->NewRow(),"desired takeoff altitude"," m",0,5,0.1,2,1);
    101     desiredLandingAltitude=new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),"desired landing altitude"," m",0,1,0.1,1);
    102     altitudeTrajectory=new TrajectoryGenerator1D(uavTab->NewRow(),"alt cons","m");
    103     uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(altitudeTrajectory->Matrix()->Element(0),DataPlot::Green);
    104     uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(altitudeTrajectory->Matrix()->Element(1),DataPlot::Green);
    105 }
    106 
    107 UavStateMachine::~UavStateMachine() {
    108 }
    109 
    110 void UavStateMachine::AddDeviceToControlLawLog(const IODevice* device) {
    111     uZ->AddDeviceToLog(device);
    112 }
    113 
    114 void UavStateMachine::AddDataToControlLawLog(const core::io_data* data) {
    115     uZ->AddDataToLog(data);
    116 }
    117 
    118 const TargetController *UavStateMachine::GetJoystick(void) const {
    119     return joy;
    120 }
     53UavStateMachine::UavStateMachine(Uav *inUav, uint16_t ds3Port)
     54    : Thread(getFrameworkManager(), "UavStateMachine", 50), uav(inUav),
     55      failSafeMode(true), flagConnectionLost(false), flagBatteryLow(false),
     56      flagZTrajectoryFinished(false) {
     57  altitudeState = AltitudeState_t::Stopped;
     58  uav->UseDefaultPlot();
     59
     60  Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
     61  buttonslayout = new GridLayout(uavTab->NewRow(), "buttons");
     62  button_kill = new PushButton(buttonslayout->NewRow(), "kill");
     63  button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
     64  button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
     65  button_take_off = new PushButton(buttonslayout->NewRow(), "take_off");
     66  button_land = new PushButton(buttonslayout->LastRowLastCol(), "land");
     67
     68  Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
     69  TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
     70  setupLawTab = new Tab(tabWidget, "Setup");
     71  graphLawTab = new Tab(tabWidget, "Graphes");
     72
     73  uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
     74  uRoll->ConvertSatFromDegToRad();
     75  uRoll->UseDefaultPlot(graphLawTab->NewRow());
     76
     77  uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
     78  uPitch->ConvertSatFromDegToRad();
     79  uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
     80
     81  uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
     82  uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
     83
     84  uZ = new PidThrust(setupLawTab->At(1, 2), "u_z");
     85  uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
     86
     87  getFrameworkManager()->AddDeviceToLog(uZ);
     88  uZ->AddDeviceToLog(uRoll);
     89  uZ->AddDeviceToLog(uPitch);
     90  uZ->AddDeviceToLog(uYaw);
     91
     92  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
     93  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
     94
     95  altitudeMode = AltitudeMode_t::Manual;
     96  orientationMode = OrientationMode_t::Manual;
     97  thrustMode = ThrustMode_t::Default;
     98  torqueMode = TorqueMode_t::Default;
     99
     100  GroupBox *reglagesGroupbox =
     101      new GroupBox(uavTab->NewRow(), "takeoff/landing");
     102  desiredTakeoffAltitude =
     103      new DoubleSpinBox(reglagesGroupbox->NewRow(), "desired takeoff altitude",
     104                        " m", 0, 5, 0.1, 2, 1);
     105  desiredLandingAltitude =
     106      new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),
     107                        "desired landing altitude", " m", 0, 1, 0.1, 1);
     108  altitudeTrajectory =
     109      new TrajectoryGenerator1D(uavTab->NewRow(), "alt cons", "m");
     110  uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(
     111      altitudeTrajectory->Matrix()->Element(0), DataPlot::Green);
     112  uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
     113      altitudeTrajectory->Matrix()->Element(1), DataPlot::Green);
     114}
     115
     116UavStateMachine::~UavStateMachine() {}
     117
     118void UavStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
     119  uZ->AddDeviceToLog(device);
     120}
     121
     122void UavStateMachine::AddDataToControlLawLog(const core::io_data *data) {
     123  uZ->AddDataToLog(data);
     124}
     125
     126const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
    121127
    122128const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
    123             return currentQuaternion;
     129  return currentQuaternion;
    124130}
    125131
    126132const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
    127     return currentAngularSpeed;
    128 }
    129 
    130 const Uav *UavStateMachine::GetUav(void) const {
    131     return uav;
    132 }
    133 
    134 void UavStateMachine::AltitudeValues(float &altitude,float &verticalSpeed) const{
    135     FailSafeAltitudeValues(altitude, verticalSpeed);
    136 }
    137 
    138 void UavStateMachine::FailSafeAltitudeValues(float &altitude,float &verticalSpeed) const {
    139     altitude=uav->GetMetaUsRangeFinder()->z();
    140     verticalSpeed=uav->GetMetaUsRangeFinder()->Vz();
     133  return currentAngularSpeed;
     134}
     135
     136const Uav *UavStateMachine::GetUav(void) const { return uav; }
     137
     138void UavStateMachine::AltitudeValues(float &altitude,
     139                                     float &verticalSpeed) const {
     140  FailSafeAltitudeValues(altitude, verticalSpeed);
     141}
     142
     143void UavStateMachine::FailSafeAltitudeValues(float &altitude,
     144                                             float &verticalSpeed) const {
     145  altitude = uav->GetMetaUsRangeFinder()->z();
     146  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
    141147}
    142148
    143149void UavStateMachine::Run() {
    144     WarnUponSwitches(true);
    145     uav->StartSensors();
    146 
    147     if(getFrameworkManager()->ErrorOccured()==true) {
    148         SafeStop();
    149     }
    150 
    151     while(!ToBeStopped()) {
    152         SecurityCheck();
    153 
    154         //get controller inputs
    155         CheckJoystick();
    156         CheckPushButton();
    157 
    158         if(IsPeriodSet()) {
    159             WaitPeriod();
    160         } else {
    161             WaitUpdate(uav->GetAhrs());
    162         }
    163         needToComputeDefaultTorques=true;
    164         needToComputeDefaultThrust=true;
    165 
    166         SignalEvent(Event_t::EnteringControlLoop);
    167 
    168         ComputeOrientation();
    169         ComputeAltitude();
    170 
    171         //compute thrust and torques to apply
    172         ComputeTorques();
    173         ComputeThrust();//logs are added to uz, so it must be updated at last
    174 
    175         // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set thrust (value between 0 and 1)
    176         uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
    177         uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
    178         uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
    179         uav->GetUavMultiplex()->SetThrust(-currentThrust);//on raisonne en negatif sur l'altitude, a revoir avec les equations
    180         uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
    181         uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
    182         uav->GetUavMultiplex()->SetYawTrim(0);
    183         uav->GetUavMultiplex()->Update(GetTime());
    184     }
    185 
    186     WarnUponSwitches(false);
     150  WarnUponSwitches(true);
     151  uav->StartSensors();
     152
     153  if (getFrameworkManager()->ErrorOccured() == true) {
     154    SafeStop();
     155  }
     156
     157  while (!ToBeStopped()) {
     158    SecurityCheck();
     159
     160    // get controller inputs
     161    CheckJoystick();
     162    CheckPushButton();
     163
     164    if (IsPeriodSet()) {
     165      WaitPeriod();
     166    } else {
     167      WaitUpdate(uav->GetAhrs());
     168    }
     169    needToComputeDefaultTorques = true;
     170    needToComputeDefaultThrust = true;
     171
     172    SignalEvent(Event_t::EnteringControlLoop);
     173
     174    ComputeOrientation();
     175    ComputeAltitude();
     176
     177    // compute thrust and torques to apply
     178    ComputeTorques();
     179    ComputeThrust(); // logs are added to uz, so it must be updated at last
     180
     181    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
     182    // thrust (value between 0 and 1)
     183    uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
     184    uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
     185    uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
     186    uav->GetUavMultiplex()->SetThrust(-currentThrust); // on raisonne en negatif
     187                                                       // sur l'altitude, a
     188                                                       // revoir avec les
     189                                                       // equations
     190    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
     191    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
     192    uav->GetUavMultiplex()->SetYawTrim(0);
     193    uav->GetUavMultiplex()->Update(GetTime());
     194  }
     195
     196  WarnUponSwitches(false);
    187197}
    188198
    189199void UavStateMachine::ComputeOrientation(void) {
    190     if (failSafeMode) {
    191         GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
     200  if (failSafeMode) {
     201    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     202                                                          currentAngularSpeed);
     203  } else {
     204    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     205                                                   currentAngularSpeed);
     206  }
     207}
     208
     209const AhrsData *UavStateMachine::GetOrientation(void) const {
     210  return GetDefaultOrientation();
     211}
     212
     213const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
     214  return uav->GetAhrs()->GetDatas();
     215}
     216
     217void UavStateMachine::ComputeAltitude(void) {
     218  if (failSafeMode) {
     219    FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
     220  } else {
     221    AltitudeValues(currentAltitude, currentVerticalSpeed);
     222  }
     223}
     224
     225void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
     226                                               float &refVerticalVelocity) {
     227  if (altitudeMode == AltitudeMode_t::Manual) {
     228    GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
     229  } else {
     230    GetReferenceAltitude(refAltitude, refVerticalVelocity);
     231  }
     232}
     233
     234void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
     235                                                  float &refVerticalVelocity) {
     236  float zc, dzc;
     237
     238  switch (altitudeState) {
     239  // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
     240  // until we detect a take off of 0.03m (hard coded value) above the ground.
     241  case AltitudeState_t::TakingOff: {
     242    if (currentAltitude > groundAltitude + 0.03) {
     243      altitudeTrajectory->StartTraj(currentAltitude,
     244                                    desiredTakeoffAltitude->Value());
     245      altitudeState = AltitudeState_t::Stabilized;
     246      SignalEvent(Event_t::Stabilized);
     247    }
     248    break;
     249  }
     250  // landing, only check if we reach desired landing altitude
     251  case AltitudeState_t::StartLanding: {
     252    if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
     253      // The Uav target altitude has reached its landing value (typically 0)
     254      // but the real Uav altitude may not have reach this value yet because of
     255      // command delay. Moreover, it
     256      // may never exactly reach this value if the ground is not perfectly
     257      // leveled (critical case: there's a
     258      // deep and narrow hole right in the sensor line of sight). That's why we
     259      // have a 2 phases landing strategy.
     260      altitudeState = AltitudeState_t::FinishLanding;
     261      SignalEvent(Event_t::FinishLanding);
     262      joy->SetLedOFF(1); // DualShock3::led1
     263    }
     264  }
     265  // stabilized: check if z trajectory is finished
     266  case AltitudeState_t::Stabilized: {
     267    if (!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
     268      SignalEvent(Event_t::ZTrajectoryFinished);
     269      flagZTrajectoryFinished = true;
     270    }
     271    if (flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
     272      flagZTrajectoryFinished = false;
     273      altitudeTrajectory->StartTraj(currentAltitude,
     274                                    desiredTakeoffAltitude->Value());
     275      joy->SetZRef(0);
     276    }
     277  }
     278  }
     279
     280  // Récupère les consignes (du joystick dans l'implémentation par défaut). La
     281  // consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par
     282  // la manette
     283  zc = joy->ZRef(); // a revoir, la position offset devrait se calculer dans le
     284                    // generator
     285  dzc = joy->DzRef();
     286
     287  // z control law
     288  altitudeTrajectory->SetPositionOffset(zc);
     289  altitudeTrajectory->SetSpeedOffset(dzc);
     290
     291  altitudeTrajectory->Update(GetTime());
     292  refAltitude = altitudeTrajectory->Position();
     293  refVerticalVelocity = altitudeTrajectory->Speed();
     294}
     295
     296void UavStateMachine::GetReferenceAltitude(float &refAltitude,
     297                                           float &refVerticalVelocity) {
     298  Thread::Warn("Default GetReferenceAltitude method is not overloaded => "
     299               "switching back to safe mode\n");
     300  EnterFailSafeMode();
     301};
     302
     303void UavStateMachine::ComputeThrust(void) {
     304  if (altitudeMode == AltitudeMode_t::Manual) {
     305    currentThrust = ComputeDefaultThrust();
     306  } else {
     307    currentThrust = ComputeCustomThrust();
     308  }
     309}
     310
     311float UavStateMachine::ComputeDefaultThrust(void) {
     312  if (needToComputeDefaultThrust) {
     313    // compute desired altitude
     314    float refAltitude, refVerticalVelocity;
     315    ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
     316
     317    switch (altitudeState) {
     318    case AltitudeState_t::TakingOff: {
     319      // The progressive increase in motor speed is used to evaluate the motor
     320      // speed that compensate the uav weight. This value
     321      // will be used as an offset for altitude control afterwards
     322      uZ->OffsetStepUp();
     323      break;
     324    }
     325    case AltitudeState_t::StartLanding:
     326    case AltitudeState_t::Stabilized: {
     327      float p_error = currentAltitude - refAltitude;
     328      float d_error = currentVerticalSpeed - refVerticalVelocity;
     329      uZ->SetValues(p_error, d_error);
     330      break;
     331    }
     332    // decrease motor speed in open loop until value offset_g , uav should have
     333    // already landed or be very close to at this point
     334    case AltitudeState_t::FinishLanding: {
     335      if (uZ->OffsetStepDown() == false) {
     336        StopMotors();
     337      }
     338      break;
     339    }
     340    }
     341    uZ->Update(GetTime());
     342
     343    savedDefaultThrust = uZ->Output();
     344    needToComputeDefaultThrust = false;
     345  }
     346
     347  return savedDefaultThrust;
     348}
     349
     350float UavStateMachine::ComputeCustomThrust(void) {
     351  Thread::Warn("Default GetThrust method is not overloaded => switching back "
     352               "to safe mode\n");
     353  EnterFailSafeMode();
     354  return ComputeDefaultThrust();
     355}
     356
     357const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
     358  if (orientationMode == OrientationMode_t::Manual) {
     359    return GetDefaultReferenceOrientation();
     360  } else {
     361    return GetReferenceOrientation();
     362  }
     363}
     364
     365const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
     366  // We directly control yaw, pitch, roll angles
     367  return joy->GetReferenceOrientation();
     368}
     369
     370const AhrsData *UavStateMachine::GetReferenceOrientation(void) {
     371  Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
     372               "switching back to safe mode\n");
     373  EnterFailSafeMode();
     374  return GetDefaultReferenceOrientation();
     375}
     376
     377void UavStateMachine::ComputeTorques(void) {
     378  if (torqueMode == TorqueMode_t::Default) {
     379    ComputeDefaultTorques(currentTorques);
     380  } else {
     381    ComputeCustomTorques(currentTorques);
     382  }
     383}
     384
     385void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
     386  if (needToComputeDefaultTorques) {
     387    const AhrsData *refOrientation = ComputeReferenceOrientation();
     388    Quaternion refQuaternion;
     389    Vector3D refAngularRates;
     390    refOrientation->GetQuaternionAndAngularRates(refQuaternion,
     391                                                 refAngularRates);
     392    Euler refAngles = refQuaternion.ToEuler();
     393    Euler currentAngles = currentQuaternion.ToEuler();
     394
     395    uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
     396                    currentAngularSpeed.z - refAngularRates.z);
     397    uYaw->Update(GetTime());
     398    torques.yaw = uYaw->Output();
     399
     400    uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
     401                      currentAngularSpeed.y);
     402    uPitch->Update(GetTime());
     403    torques.pitch = uPitch->Output();
     404
     405    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
     406    uRoll->Update(GetTime());
     407    torques.roll = uRoll->Output();
     408
     409    savedDefaultTorques = torques;
     410    needToComputeDefaultTorques = false;
     411  } else {
     412    torques = savedDefaultTorques;
     413  }
     414}
     415
     416void UavStateMachine::ComputeCustomTorques(Euler &torques) {
     417  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
     418               "switching back to safe mode\n");
     419  EnterFailSafeMode();
     420  ComputeDefaultTorques(torques);
     421}
     422
     423void UavStateMachine::TakeOff(void) {
     424  flagZTrajectoryFinished = false;
     425
     426  if (altitudeState == AltitudeState_t::Stopped) { // &&
     427    // GetBatteryMonitor()->IsBatteryLow()==false)
     428    // The uav always takes off in fail safe mode
     429    EnterFailSafeMode();
     430    joy->SetLedOFF(4); // DualShock3::led4
     431    joy->SetLedOFF(1); // DualShock3::led1
     432    joy->Rumble(0x70);
     433    joy->SetZRef(0);
     434
     435    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
     436                     // valeur initiale (station sol)
     437
     438    uav->GetUavMultiplex()->LockUserInterface();
     439    // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
     440    // consignes moteurs
     441    // sans les faire tourner effectivement (en déplaçant à la main le drone)
     442    uav->GetBldc()->SetEnabled(true);
     443    groundAltitude = currentAltitude;
     444    altitudeState = AltitudeState_t::TakingOff;
     445
     446    SignalEvent(Event_t::TakingOff);
     447  } else {
     448    joy->ErrorNotify();
     449  }
     450}
     451
     452void UavStateMachine::Land(void) {
     453  if (altitudeMode != AltitudeMode_t::Manual) {
     454    SetAltitudeMode(AltitudeMode_t::Manual);
     455  }
     456  if (altitudeState == AltitudeState_t::Stabilized) {
     457    joy->SetLedOFF(4); // DualShock3::led4
     458    joy->Rumble(0x70);
     459
     460    altitudeTrajectory->StopTraj();
     461    joy->SetZRef(0);
     462    altitudeTrajectory->StartTraj(
     463        currentAltitude,
     464        desiredLandingAltitude->Value()); // shouldn't it be groundAltitude?
     465    SignalEvent(Event_t::StartLanding);
     466    altitudeState = AltitudeState_t::StartLanding;
     467  } else {
     468    joy->ErrorNotify();
     469  }
     470}
     471
     472void UavStateMachine::SignalEvent(Event_t event) {
     473  switch (event) {
     474  case Event_t::StartLanding:
     475    Thread::Info("Altitude: entering 'StartLanding' state\n");
     476    break;
     477  case Event_t::Stopped:
     478    Thread::Info("Altitude: entering 'Stopped' state\n");
     479    break;
     480  case Event_t::TakingOff:
     481    Thread::Info("Altitude: taking off\n");
     482    break;
     483  case Event_t::Stabilized:
     484    Thread::Info("Altitude: entering 'Stabilized' state\n");
     485    break;
     486  case Event_t::FinishLanding:
     487    Thread::Info("Altitude: entering 'FinishLanding' state\n");
     488    break;
     489  case Event_t::EmergencyStop:
     490    Thread::Info("Emergency stop!\n");
     491    break;
     492  }
     493}
     494
     495void UavStateMachine::EmergencyStop(void) {
     496  if (altitudeState != AltitudeState_t::Stopped) {
     497    StopMotors();
     498    EnterFailSafeMode();
     499    joy->Rumble(0x70);
     500    SignalEvent(Event_t::EmergencyStop);
     501  }
     502}
     503
     504void UavStateMachine::StopMotors(void) {
     505  joy->FlashLed(1, 10, 10); // DualShock3::led1
     506  uav->GetBldc()->SetEnabled(false);
     507  uav->GetUavMultiplex()->UnlockUserInterface();
     508  SignalEvent(Event_t::Stopped);
     509  altitudeState = AltitudeState_t::Stopped;
     510  uav->GetAhrs()->UnlockUserInterface();
     511
     512  uZ->SetValues(0, 0);
     513  uZ->Reset();
     514}
     515
     516GridLayout *UavStateMachine::GetButtonsLayout(void) const {
     517  return buttonslayout;
     518}
     519
     520void UavStateMachine::SecurityCheck(void) {
     521  MandatorySecurityCheck();
     522  ExtraSecurityCheck();
     523}
     524
     525void UavStateMachine::MandatorySecurityCheck(void) {
     526  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
     527    flagConnectionLost = true;
     528    Thread::Err("Connection lost\n");
     529    EnterFailSafeMode();
     530    if (altitudeState == AltitudeState_t::Stopped) {
     531      SafeStop();
    192532    } else {
    193         GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
    194     }
    195 }
    196 
    197 const AhrsData *UavStateMachine::GetOrientation(void) const {
    198     return GetDefaultOrientation();
    199 }
    200 
    201 const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
    202     return uav->GetAhrs()->GetDatas();
    203 }
    204 
    205 void UavStateMachine::ComputeAltitude(void) {
    206     if (failSafeMode) {
    207         FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
    208     } else {
    209         AltitudeValues(currentAltitude,currentVerticalSpeed);
    210     }
    211 }
    212 
    213 void UavStateMachine::ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    214     if (altitudeMode==AltitudeMode_t::Manual) {
    215         GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
    216     } else {
    217         GetReferenceAltitude(refAltitude, refVerticalVelocity);
    218     }
    219 }
    220 
    221 void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    222     float zc,dzc;
    223 
    224     switch(altitudeState) {
    225         //initiate a takeoff: increase motor speed in open loop (see ComputeThrust) until we detect a take off of 0.03m (hard coded value) above the ground.
    226         case AltitudeState_t::TakingOff: {
    227             if(currentAltitude>groundAltitude+0.03) {
    228                 altitudeTrajectory->StartTraj(currentAltitude,desiredTakeoffAltitude->Value());
    229                 altitudeState=AltitudeState_t::Stabilized;
    230                 SignalEvent(Event_t::Stabilized);
    231             }
    232             break;
    233         }
    234         //landing, only check if we reach desired landing altitude
    235         case AltitudeState_t::StartLanding: {
    236             if(altitudeTrajectory->Position()==desiredLandingAltitude->Value()) {
    237                 //The Uav target altitude has reached its landing value (typically 0)
    238                 // but the real Uav altitude may not have reach this value yet because of command delay. Moreover, it
    239                 // may never exactly reach this value if the ground is not perfectly leveled (critical case: there's a
    240                 // deep and narrow hole right in the sensor line of sight). That's why we have a 2 phases landing strategy.
    241                 altitudeState=AltitudeState_t::FinishLanding;
    242                 SignalEvent(Event_t::FinishLanding);
    243                 joy->SetLedOFF(1);//DualShock3::led1
    244             }
    245         }
    246         //stabilized: check if z trajectory is finished
    247         case AltitudeState_t::Stabilized: {
    248             if(!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
    249                 SignalEvent(Event_t::ZTrajectoryFinished);
    250                 flagZTrajectoryFinished=true;
    251             }
    252             if(flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
    253                 flagZTrajectoryFinished=false;
    254                 altitudeTrajectory->StartTraj(currentAltitude,desiredTakeoffAltitude->Value());
    255                 joy->SetZRef(0);
    256             }
    257         }
    258     }
    259 
    260     //Récupère les consignes (du joystick dans l'implémentation par défaut). La consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par la manette
    261     zc=joy->ZRef();//a revoir, la position offset devrait se calculer dans le generator
    262     dzc=joy->DzRef();
    263 
    264     //z control law
    265     altitudeTrajectory->SetPositionOffset(zc);
    266     altitudeTrajectory->SetSpeedOffset(dzc);
    267 
    268     altitudeTrajectory->Update(GetTime());
    269     refAltitude=altitudeTrajectory->Position();
    270     refVerticalVelocity=altitudeTrajectory->Speed();
    271 }
    272 
    273 void UavStateMachine::GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    274     Thread::Warn("Default GetReferenceAltitude method is not overloaded => switching back to safe mode\n");
     533      Land();
     534    }
     535  }
     536  if ((altitudeState == AltitudeState_t::TakingOff ||
     537       altitudeState == AltitudeState_t::Stabilized) &&
     538      uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
     539    flagBatteryLow = true;
     540    Printf("Battery Low\n");
    275541    EnterFailSafeMode();
    276 };
    277 
    278 void UavStateMachine::ComputeThrust(void) {
    279     if (altitudeMode==AltitudeMode_t::Manual) {
    280         currentThrust=ComputeDefaultThrust();
    281     } else {
    282         currentThrust=ComputeCustomThrust();
    283     }
    284 }
    285 
    286 float UavStateMachine::ComputeDefaultThrust(void) {
    287     if(needToComputeDefaultThrust) {
    288         //compute desired altitude
    289         float refAltitude, refVerticalVelocity;
    290         ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
    291 
    292         switch(altitudeState) {
    293             case AltitudeState_t::TakingOff: {
    294                 //The progressive increase in motor speed is used to evaluate the motor speed that compensate the uav weight. This value
    295                 //will be used as an offset for altitude control afterwards
    296                 uZ->OffsetStepUp();
    297                 break;
    298             }
    299             case AltitudeState_t::StartLanding:
    300             case AltitudeState_t::Stabilized: {
    301                 float p_error=currentAltitude-refAltitude;
    302                 float d_error=currentVerticalSpeed-refVerticalVelocity;
    303                 uZ->SetValues(p_error,d_error);
    304                 break;
    305             }
    306             //decrease motor speed in open loop until value offset_g , uav should have already landed or be very close to at this point
    307             case AltitudeState_t::FinishLanding: {
    308                 if(uZ->OffsetStepDown()==false) {
    309                     StopMotors();
    310                 }
    311                 break;
    312             }
    313         }
    314         uZ->Update(GetTime());
    315 
    316         savedDefaultThrust=uZ->Output();
    317         needToComputeDefaultThrust=false;
    318     }
    319 
    320     return savedDefaultThrust;
    321 }
    322 
    323 float UavStateMachine::ComputeCustomThrust(void) {
    324     Thread::Warn("Default GetThrust method is not overloaded => switching back to safe mode\n");
    325     EnterFailSafeMode();
    326     return ComputeDefaultThrust();
    327 }
    328 
    329 const AhrsData* UavStateMachine::ComputeReferenceOrientation(void) {
    330     if(orientationMode==OrientationMode_t::Manual) {
    331         return GetDefaultReferenceOrientation();
    332     } else {
    333         return GetReferenceOrientation();
    334     }
    335 }
    336 
    337 const AhrsData* UavStateMachine::GetDefaultReferenceOrientation(void) const {
    338     // We directly control yaw, pitch, roll angles
    339     return joy->GetReferenceOrientation();
    340 }
    341 
    342 const AhrsData* UavStateMachine::GetReferenceOrientation(void) {
    343     Thread::Warn("Default GetReferenceOrientation method is not overloaded => switching back to safe mode\n");
    344     EnterFailSafeMode();
    345     return GetDefaultReferenceOrientation();
    346 }
    347 
    348 void UavStateMachine::ComputeTorques(void) {
    349     if(torqueMode==TorqueMode_t::Default) {
    350         ComputeDefaultTorques(currentTorques);
    351     } else {
    352         ComputeCustomTorques(currentTorques);
    353     }
    354 }
    355 
    356 void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
    357     if(needToComputeDefaultTorques) {
    358         const AhrsData *refOrientation=ComputeReferenceOrientation();
    359         Quaternion refQuaternion;
    360         Vector3D refAngularRates;
    361         refOrientation->GetQuaternionAndAngularRates(refQuaternion,refAngularRates);
    362         Euler refAngles=refQuaternion.ToEuler();
    363         Euler currentAngles=currentQuaternion.ToEuler();
    364 
    365         uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),currentAngularSpeed.z-refAngularRates.z);
    366         uYaw->Update(GetTime());
    367         torques.yaw=uYaw->Output();
    368 
    369         uPitch->SetValues(refAngles.pitch,currentAngles.pitch,currentAngularSpeed.y);
    370         uPitch->Update(GetTime());
    371         torques.pitch=uPitch->Output();
    372 
    373         uRoll->SetValues(refAngles.roll,currentAngles.roll,currentAngularSpeed.x);
    374         uRoll->Update(GetTime());
    375         torques.roll=uRoll->Output();
    376 
    377         savedDefaultTorques=torques;
    378         needToComputeDefaultTorques=false;
    379     } else {
    380         torques=savedDefaultTorques;
    381     }
    382 }
    383 
    384 void UavStateMachine::ComputeCustomTorques(Euler &torques) {
    385     Thread::Warn("Default ComputeCustomTorques method is not overloaded => switching back to safe mode\n");
    386     EnterFailSafeMode();
    387     ComputeDefaultTorques(torques);
    388 }
    389 
    390 void UavStateMachine::TakeOff(void) {
    391     flagZTrajectoryFinished=false;
    392 
    393     if(altitudeState==AltitudeState_t::Stopped) {// && GetBatteryMonitor()->IsBatteryLow()==false)
    394         //The uav always takes off in fail safe mode
    395         EnterFailSafeMode();
    396         joy->SetLedOFF(4);//DualShock3::led4
    397         joy->SetLedOFF(1);//DualShock3::led1
    398         joy->Rumble(0x70);
    399         joy->SetZRef(0);
    400 
    401         uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa valeur initiale (station sol)
    402 
    403         uav->GetUavMultiplex()->LockUserInterface();
    404         //Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les consignes moteurs
    405         // sans les faire tourner effectivement (en déplaçant à la main le drone)
    406         uav->GetBldc()->SetEnabled(true);
    407         groundAltitude=currentAltitude;
    408         altitudeState=AltitudeState_t::TakingOff;
    409 
    410         SignalEvent(Event_t::TakingOff);
    411     } else {
     542    Land();
     543  }
     544}
     545
     546void UavStateMachine::CheckJoystick(void) {
     547  GenericCheckJoystick();
     548  ExtraCheckJoystick();
     549}
     550
     551void UavStateMachine::GenericCheckJoystick(void) {
     552  static bool isEmergencyStopButtonPressed = false;
     553  static bool isTakeOffButtonPressed = false;
     554  static bool isSafeModeButtonPressed = false;
     555
     556  if (joy->IsButtonPressed(1)) { // select
     557    if (!isEmergencyStopButtonPressed) {
     558      isEmergencyStopButtonPressed = true;
     559      Thread::Info("Emergency stop from joystick\n");
     560      EmergencyStop();
     561    }
     562  } else
     563    isEmergencyStopButtonPressed = false;
     564
     565  if (joy->IsButtonPressed(0)) { // start
     566    if (!isTakeOffButtonPressed) {
     567      isTakeOffButtonPressed = true;
     568      switch (altitudeState) {
     569      case AltitudeState_t::Stopped:
     570        TakeOff();
     571        break;
     572      case AltitudeState_t::Stabilized:
     573        Land();
     574        break;
     575      default:
    412576        joy->ErrorNotify();
    413     }
    414 }
    415 
    416 void UavStateMachine::Land(void) {
    417     if (altitudeMode!=AltitudeMode_t::Manual) {
    418         SetAltitudeMode(AltitudeMode_t::Manual);
    419     }
    420     if(altitudeState==AltitudeState_t::Stabilized) {
    421         joy->SetLedOFF(4);//DualShock3::led4
    422         joy->Rumble(0x70);
    423 
    424         altitudeTrajectory->StopTraj();
    425         joy->SetZRef(0);
    426         altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
    427         SignalEvent(Event_t::StartLanding);
    428         altitudeState=AltitudeState_t::StartLanding;
    429     } else {
    430         joy->ErrorNotify();
    431     }
    432 }
    433 
    434 void UavStateMachine::SignalEvent(Event_t event) {
    435     switch(event) {
    436     case Event_t::StartLanding:
    437         Thread::Info("Altitude: entering 'StartLanding' state\n");
    438577        break;
    439     case Event_t::Stopped:
    440         Thread::Info("Altitude: entering 'Stopped' state\n");
    441         break;
    442     case Event_t::TakingOff:
    443         Thread::Info("Altitude: taking off\n");
    444         break;
    445     case Event_t::Stabilized:
    446         Thread::Info("Altitude: entering 'Stabilized' state\n");
    447         break;
    448     case Event_t::FinishLanding:
    449         Thread::Info("Altitude: entering 'FinishLanding' state\n");
    450         break;
    451     case Event_t::EmergencyStop:
    452         Thread::Info("Emergency stop!\n");
    453         break;
    454     }
    455 }
    456 
    457 void UavStateMachine::EmergencyStop(void) {
    458     if(altitudeState!=AltitudeState_t::Stopped) {
    459         StopMotors();
    460         EnterFailSafeMode();
    461         joy->Rumble(0x70);
    462         SignalEvent(Event_t::EmergencyStop);
    463     }
    464 }
    465 
    466 void UavStateMachine::StopMotors(void)
    467 {
    468     joy->FlashLed(1,10,10);//DualShock3::led1
    469     uav->GetBldc()->SetEnabled(false);
    470     uav->GetUavMultiplex()->UnlockUserInterface();
    471     SignalEvent(Event_t::Stopped);
    472     altitudeState=AltitudeState_t::Stopped;
    473     uav->GetAhrs()->UnlockUserInterface();
    474 
    475     uZ->SetValues(0,0);
    476     uZ->Reset();
    477 }
    478 
    479 GridLayout* UavStateMachine::GetButtonsLayout(void) const {
    480     return buttonslayout;
    481 }
    482 
    483 void UavStateMachine::SecurityCheck(void) {
    484     MandatorySecurityCheck();
    485     ExtraSecurityCheck();
    486 }
    487 
    488 void UavStateMachine::MandatorySecurityCheck(void) {
    489     if(getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
    490         flagConnectionLost=true;
    491         Thread::Err("Connection lost\n");
    492         EnterFailSafeMode();
    493         if(altitudeState==AltitudeState_t::Stopped) {
    494             SafeStop();
    495         } else {
    496             Land();
    497         }
    498     }
    499     if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
    500         flagBatteryLow=true;
    501         Printf("Battery Low\n");
    502         EnterFailSafeMode();
    503         Land();
    504     }
    505 }
    506 
    507 void UavStateMachine::CheckJoystick(void) {
    508     GenericCheckJoystick();
    509     ExtraCheckJoystick();
    510 }
    511 
    512 void UavStateMachine::GenericCheckJoystick(void) {
    513     static bool isEmergencyStopButtonPressed=false;
    514     static bool isTakeOffButtonPressed=false;
    515     static bool isSafeModeButtonPressed=false;
    516 
    517    if(joy->IsButtonPressed(1)) { //select
    518         if (!isEmergencyStopButtonPressed) {
    519             isEmergencyStopButtonPressed=true;
    520             Thread::Info("Emergency stop from joystick\n");
    521             EmergencyStop();
    522         }
    523     } else isEmergencyStopButtonPressed=false;
    524 
    525     if(joy->IsButtonPressed(0)) { //start
    526         if (!isTakeOffButtonPressed) {
    527             isTakeOffButtonPressed=true;
    528             switch(altitudeState) {
    529                 case AltitudeState_t::Stopped:
    530                     TakeOff();
    531                     break;
    532                 case AltitudeState_t::Stabilized:
    533                     Land();
    534                     break;
    535                 default:
    536                     joy->ErrorNotify();
    537                     break;
    538             }
    539         }
    540     } else isTakeOffButtonPressed=false;
    541 
    542     //cross
    543     //gsanahuj:conflict with Majd programs.
    544     //check if l1,l2,r1 and r2 are not pressed
    545     //to allow a combination in user program
    546     if(joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) && !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) && !joy->IsButtonPressed(10)) {
    547         if (!isSafeModeButtonPressed) {
    548             isSafeModeButtonPressed=true;
    549             Thread::Info("Entering fail safe mode\n");
    550             EnterFailSafeMode();
    551         }
    552     } else isSafeModeButtonPressed=false;
     578      }
     579    }
     580  } else
     581    isTakeOffButtonPressed = false;
     582
     583  // cross
     584  // gsanahuj:conflict with Majd programs.
     585  // check if l1,l2,r1 and r2 are not pressed
     586  // to allow a combination in user program
     587  if (joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) &&
     588      !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) &&
     589      !joy->IsButtonPressed(10)) {
     590    if (!isSafeModeButtonPressed) {
     591      isSafeModeButtonPressed = true;
     592      Thread::Info("Entering fail safe mode\n");
     593      EnterFailSafeMode();
     594    }
     595  } else
     596    isSafeModeButtonPressed = false;
    553597}
    554598
    555599void UavStateMachine::CheckPushButton(void) {
    556     GenericCheckPushButton();
    557     ExtraCheckPushButton();
     600  GenericCheckPushButton();
     601  ExtraCheckPushButton();
    558602}
    559603
    560604void UavStateMachine::GenericCheckPushButton(void) {
    561     if(button_kill->Clicked()==true) SafeStop();
    562     if(button_take_off->Clicked()==true) TakeOff();
    563     if(button_land->Clicked()==true) Land();
    564     if(button_start_log->Clicked()==true) getFrameworkManager()->StartLog();
    565     if(button_stop_log->Clicked()==true) getFrameworkManager()->StopLog();
     605  if (button_kill->Clicked() == true)
     606    SafeStop();
     607  if (button_take_off->Clicked() == true)
     608    TakeOff();
     609  if (button_land->Clicked() == true)
     610    Land();
     611  if (button_start_log->Clicked() == true)
     612    getFrameworkManager()->StartLog();
     613  if (button_stop_log->Clicked() == true)
     614    getFrameworkManager()->StopLog();
    566615}
    567616
    568617void UavStateMachine::EnterFailSafeMode(void) {
    569     SetAltitudeMode(AltitudeMode_t::Manual);
    570     SetOrientationMode(OrientationMode_t::Manual);
    571     SetThrustMode(ThrustMode_t::Default);
    572     SetTorqueMode(TorqueMode_t::Default);
    573 
    574     GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
    575     joy->SetYawRef(currentQuaternion);
     618  SetAltitudeMode(AltitudeMode_t::Manual);
     619  SetOrientationMode(OrientationMode_t::Manual);
     620  SetThrustMode(ThrustMode_t::Default);
     621  SetTorqueMode(TorqueMode_t::Default);
     622
     623  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     624                                                        currentAngularSpeed);
     625  joy->SetYawRef(currentQuaternion);
     626  uYaw->Reset();
     627  uPitch->Reset();
     628  uRoll->Reset();
     629
     630  failSafeMode = true;
     631  SignalEvent(Event_t::EnteringFailSafeMode);
     632}
     633
     634bool UavStateMachine::ExitFailSafeMode(void) {
     635  // only exit fail safe mode if in Stabilized altitude state
     636  // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
     637  // le ruban perturbe l'us
     638  /*
     639    if (altitudeState!=AltitudeState_t::Stabilized) {
     640        return false;
     641    } else*/ {
     642    failSafeMode = false;
     643    return true;
     644  }
     645}
     646
     647bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
     648  if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
     649    if (!ExitFailSafeMode())
     650      return false;
     651  }
     652  // When transitionning from Custom to Default torque mode, we should reset the
     653  // default control laws
     654  if ((torqueMode == TorqueMode_t::Custom) &&
     655      (newTorqueMode == TorqueMode_t::Default)) {
    576656    uYaw->Reset();
    577657    uPitch->Reset();
    578658    uRoll->Reset();
    579 
    580     failSafeMode=true;
    581     SignalEvent(Event_t::EnteringFailSafeMode);
    582 }
    583 
    584 bool UavStateMachine::ExitFailSafeMode(void) {
    585     // only exit fail safe mode if in Stabilized altitude state
    586     //gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
    587     //le ruban perturbe l'us
    588     /*
    589     if (altitudeState!=AltitudeState_t::Stabilized) {
    590         return false;
    591     } else*/ {
    592         failSafeMode=false;
    593         return true;
    594     }
    595 }
    596 
    597 bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
    598     if ((newTorqueMode==TorqueMode_t::Custom) && (failSafeMode)) {
    599         if (!ExitFailSafeMode()) return false;
    600     }
    601     //When transitionning from Custom to Default torque mode, we should reset the default control laws
    602     if ((torqueMode==TorqueMode_t::Custom) && (newTorqueMode==TorqueMode_t::Default)) {
    603         uYaw->Reset();
    604         uPitch->Reset();
    605         uRoll->Reset();
    606     }
    607     torqueMode=newTorqueMode;
    608     return true;
     659  }
     660  torqueMode = newTorqueMode;
     661  return true;
    609662}
    610663
    611664bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
    612     if ((newAltitudeMode==AltitudeMode_t::Custom) && (failSafeMode)) {
    613         if (!ExitFailSafeMode()) return false;
    614     }
    615     altitudeMode=newAltitudeMode;
    616     GotoAltitude(desiredTakeoffAltitude->Value());
    617 
    618     return true;
     665  if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
     666    if (!ExitFailSafeMode())
     667      return false;
     668  }
     669  altitudeMode = newAltitudeMode;
     670  GotoAltitude(desiredTakeoffAltitude->Value());
     671
     672  return true;
    619673}
    620674
    621675bool UavStateMachine::GotoAltitude(float desiredAltitude) {
    622     if (altitudeMode!=AltitudeMode_t::Manual) {
    623         return false;
    624     }
    625     altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),desiredAltitude);
    626     return true;
    627 }
    628 
    629 bool UavStateMachine::SetOrientationMode(OrientationMode_t const &newOrientationMode) {
    630     if ((newOrientationMode==OrientationMode_t::Custom) && (failSafeMode)) {
    631         if (!ExitFailSafeMode()) return false;
    632     }
    633     //When transitionning from Custom to Manual mode we must reset to yaw reference to the current absolute yaw angle,
    634     // overwise the Uav will abruptly change orientation
    635     if ((orientationMode==OrientationMode_t::Custom) && (newOrientationMode==OrientationMode_t::Manual)) {
    636         joy->SetYawRef(currentQuaternion);
    637     }
    638     orientationMode=newOrientationMode;
    639     return true;
     676  if (altitudeMode != AltitudeMode_t::Manual) {
     677    return false;
     678  }
     679  altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
     680                                desiredAltitude);
     681  return true;
     682}
     683
     684bool UavStateMachine::SetOrientationMode(
     685    OrientationMode_t const &newOrientationMode) {
     686  if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
     687    if (!ExitFailSafeMode())
     688      return false;
     689  }
     690  // When transitionning from Custom to Manual mode we must reset to yaw
     691  // reference to the current absolute yaw angle,
     692  // overwise the Uav will abruptly change orientation
     693  if ((orientationMode == OrientationMode_t::Custom) &&
     694      (newOrientationMode == OrientationMode_t::Manual)) {
     695    joy->SetYawRef(currentQuaternion);
     696  }
     697  orientationMode = newOrientationMode;
     698  return true;
    640699}
    641700
    642701bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
    643     if ((newThrustMode==ThrustMode_t::Custom) && (failSafeMode)) {
    644         if (!ExitFailSafeMode()) return false;
    645     }
    646     thrustMode=newThrustMode;
    647     return true;
    648 }
     702  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
     703    if (!ExitFailSafeMode())
     704      return false;
     705  }
     706  thrustMode = newThrustMode;
     707  return true;
     708}
Note: See TracChangeset for help on using the changeset viewer.