source: flair-src/trunk/lib/FlairMeta/src/UavStateMachine.cpp @ 15

Last change on this file since 15 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

File size: 22.2 KB
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[7]5//  created:    2014/04/29
6//  filename:   UavStateMachine.cpp
7//
8//  author:     Gildas Bayard, Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    meta class for UAV
14//
15//
16/*********************************************************************/
17
18#include "UavStateMachine.h"
19#include "Uav.h"
20#include <DataPlot1D.h>
21#include <GridLayout.h>
22#include <Tab.h>
23#include <TabWidget.h>
24#include <PushButton.h>
25#include <SpinBox.h>
26#include <DoubleSpinBox.h>
27#include <X4X8Multiplex.h>
28#include <Bldc.h>
29#include <Ahrs.h>
30#include <MetaUsRangeFinder.h>
31#include <ControlLaw.h>
32#include <Pid.h>
33#include <PidThrust.h>
34#include <NestedSat.h>
35#include <MetaDualShock3.h>
36#include <AhrsData.h>
37#include <BatteryMonitor.h>
38#include <FrameworkManager.h>
39#include <Vector3D.h>
40#include <Vector2D.h>
41#include <cvmatrix.h>
42#include <stdio.h>
43#include <TrajectoryGenerator1D.h>
44
45using namespace std;
46using namespace flair::core;
47using namespace flair::gui;
48using namespace flair::sensor;
49using namespace flair::actuator;
50using namespace flair::filter;
51using namespace flair::meta;
52
[15]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();
[7]59
[15]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");
[7]67
[15]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");
[7]72
[15]73  uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
74  uRoll->ConvertSatFromDegToRad();
75  uRoll->UseDefaultPlot(graphLawTab->NewRow());
[7]76
[15]77  uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
78  uPitch->ConvertSatFromDegToRad();
79  uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
[7]80
[15]81  uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
82  uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
[7]83
[15]84  uZ = new PidThrust(setupLawTab->At(1, 2), "u_z");
85  uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
[7]86
[15]87  getFrameworkManager()->AddDeviceToLog(uZ);
88  uZ->AddDeviceToLog(uRoll);
89  uZ->AddDeviceToLog(uPitch);
90  uZ->AddDeviceToLog(uYaw);
[7]91
[15]92  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
93  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
[7]94
[15]95  altitudeMode = AltitudeMode_t::Manual;
96  orientationMode = OrientationMode_t::Manual;
97  thrustMode = ThrustMode_t::Default;
98  torqueMode = TorqueMode_t::Default;
[7]99
[15]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);
[7]114}
115
[15]116UavStateMachine::~UavStateMachine() {}
[7]117
[15]118void UavStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
119  uZ->AddDeviceToLog(device);
[7]120}
121
[15]122void UavStateMachine::AddDataToControlLawLog(const core::io_data *data) {
123  uZ->AddDataToLog(data);
[7]124}
125
[15]126const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
[7]127
128const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
[15]129  return currentQuaternion;
[7]130}
131
132const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
[15]133  return currentAngularSpeed;
[7]134}
135
[15]136const Uav *UavStateMachine::GetUav(void) const { return uav; }
[7]137
[15]138void UavStateMachine::AltitudeValues(float &altitude,
139                                     float &verticalSpeed) const {
140  FailSafeAltitudeValues(altitude, verticalSpeed);
[7]141}
142
[15]143void UavStateMachine::FailSafeAltitudeValues(float &altitude,
144                                             float &verticalSpeed) const {
145  altitude = uav->GetMetaUsRangeFinder()->z();
146  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
[7]147}
148
149void UavStateMachine::Run() {
[15]150  WarnUponSwitches(true);
151  uav->StartSensors();
[7]152
[15]153  if (getFrameworkManager()->ErrorOccured() == true) {
154    SafeStop();
155  }
[7]156
[15]157  while (!ToBeStopped()) {
158    SecurityCheck();
[7]159
[15]160    // get controller inputs
161    CheckJoystick();
162    CheckPushButton();
[7]163
[15]164    if (IsPeriodSet()) {
165      WaitPeriod();
166    } else {
167      WaitUpdate(uav->GetAhrs());
168    }
169    needToComputeDefaultTorques = true;
170    needToComputeDefaultThrust = true;
[7]171
[15]172    SignalEvent(Event_t::EnteringControlLoop);
[7]173
[15]174    ComputeOrientation();
175    ComputeAltitude();
[7]176
[15]177    // compute thrust and torques to apply
178    ComputeTorques();
179    ComputeThrust(); // logs are added to uz, so it must be updated at last
[7]180
[15]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  }
[7]195
[15]196  WarnUponSwitches(false);
[7]197}
198
199void UavStateMachine::ComputeOrientation(void) {
[15]200  if (failSafeMode) {
201    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
202                                                          currentAngularSpeed);
203  } else {
204    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
205                                                   currentAngularSpeed);
206  }
[7]207}
208
209const AhrsData *UavStateMachine::GetOrientation(void) const {
[15]210  return GetDefaultOrientation();
[7]211}
212
213const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
[15]214  return uav->GetAhrs()->GetDatas();
[7]215}
216
217void UavStateMachine::ComputeAltitude(void) {
[15]218  if (failSafeMode) {
219    FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
220  } else {
221    AltitudeValues(currentAltitude, currentVerticalSpeed);
222  }
[7]223}
224
[15]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  }
[7]232}
233
[15]234void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
235                                                  float &refVerticalVelocity) {
236  float zc, dzc;
[7]237
[15]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);
[7]247    }
[15]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  }
[7]279
[15]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();
[7]286
[15]287  // z control law
288  altitudeTrajectory->SetPositionOffset(zc);
289  altitudeTrajectory->SetSpeedOffset(dzc);
[7]290
[15]291  altitudeTrajectory->Update(GetTime());
292  refAltitude = altitudeTrajectory->Position();
293  refVerticalVelocity = altitudeTrajectory->Speed();
[7]294}
295
[15]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();
[7]301};
302
303void UavStateMachine::ComputeThrust(void) {
[15]304  if (altitudeMode == AltitudeMode_t::Manual) {
305    currentThrust = ComputeDefaultThrust();
306  } else {
307    currentThrust = ComputeCustomThrust();
308  }
[7]309}
310
311float UavStateMachine::ComputeDefaultThrust(void) {
[15]312  if (needToComputeDefaultThrust) {
313    // compute desired altitude
314    float refAltitude, refVerticalVelocity;
315    ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
[7]316
[15]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;
[7]324    }
[15]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());
[7]342
[15]343    savedDefaultThrust = uZ->Output();
344    needToComputeDefaultThrust = false;
345  }
346
347  return savedDefaultThrust;
[7]348}
349
350float UavStateMachine::ComputeCustomThrust(void) {
[15]351  Thread::Warn("Default GetThrust method is not overloaded => switching back "
352               "to safe mode\n");
353  EnterFailSafeMode();
354  return ComputeDefaultThrust();
[7]355}
356
[15]357const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
358  if (orientationMode == OrientationMode_t::Manual) {
359    return GetDefaultReferenceOrientation();
360  } else {
361    return GetReferenceOrientation();
362  }
[7]363}
364
[15]365const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
366  // We directly control yaw, pitch, roll angles
367  return joy->GetReferenceOrientation();
[7]368}
369
[15]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();
[7]375}
376
377void UavStateMachine::ComputeTorques(void) {
[15]378  if (torqueMode == TorqueMode_t::Default) {
379    ComputeDefaultTorques(currentTorques);
380  } else {
381    ComputeCustomTorques(currentTorques);
382  }
[7]383}
384
385void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
[15]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();
[7]394
[15]395    uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
396                    currentAngularSpeed.z - refAngularRates.z);
397    uYaw->Update(GetTime());
398    torques.yaw = uYaw->Output();
[7]399
[15]400    uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
401                      currentAngularSpeed.y);
402    uPitch->Update(GetTime());
403    torques.pitch = uPitch->Output();
[7]404
[15]405    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
406    uRoll->Update(GetTime());
407    torques.roll = uRoll->Output();
[7]408
[15]409    savedDefaultTorques = torques;
410    needToComputeDefaultTorques = false;
411  } else {
412    torques = savedDefaultTorques;
413  }
[7]414}
415
416void UavStateMachine::ComputeCustomTorques(Euler &torques) {
[15]417  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
418               "switching back to safe mode\n");
419  EnterFailSafeMode();
420  ComputeDefaultTorques(torques);
[7]421}
422
423void UavStateMachine::TakeOff(void) {
[15]424  flagZTrajectoryFinished = false;
[7]425
[15]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);
[7]434
[15]435    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
436                     // valeur initiale (station sol)
[7]437
[15]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;
[7]445
[15]446    SignalEvent(Event_t::TakingOff);
447  } else {
448    joy->ErrorNotify();
449  }
[7]450}
451
452void UavStateMachine::Land(void) {
[15]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);
[7]459
[15]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  }
[7]470}
471
472void UavStateMachine::SignalEvent(Event_t event) {
[15]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  }
[7]493}
494
495void UavStateMachine::EmergencyStop(void) {
[15]496  if (altitudeState != AltitudeState_t::Stopped) {
497    StopMotors();
498    EnterFailSafeMode();
499    joy->Rumble(0x70);
500    SignalEvent(Event_t::EmergencyStop);
501  }
[7]502}
503
[15]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();
[7]511
[15]512  uZ->SetValues(0, 0);
513  uZ->Reset();
[7]514}
515
[15]516GridLayout *UavStateMachine::GetButtonsLayout(void) const {
517  return buttonslayout;
[7]518}
519
520void UavStateMachine::SecurityCheck(void) {
[15]521  MandatorySecurityCheck();
522  ExtraSecurityCheck();
[7]523}
524
525void UavStateMachine::MandatorySecurityCheck(void) {
[15]526  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
527    flagConnectionLost = true;
528    Thread::Err("Connection lost\n");
529    EnterFailSafeMode();
530    if (altitudeState == AltitudeState_t::Stopped) {
531      SafeStop();
532    } else {
533      Land();
[7]534    }
[15]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");
541    EnterFailSafeMode();
542    Land();
543  }
[7]544}
545
546void UavStateMachine::CheckJoystick(void) {
[15]547  GenericCheckJoystick();
548  ExtraCheckJoystick();
[7]549}
550
551void UavStateMachine::GenericCheckJoystick(void) {
[15]552  static bool isEmergencyStopButtonPressed = false;
553  static bool isTakeOffButtonPressed = false;
554  static bool isSafeModeButtonPressed = false;
[7]555
[15]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;
[7]564
[15]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:
576        joy->ErrorNotify();
577        break;
578      }
579    }
580  } else
581    isTakeOffButtonPressed = false;
[7]582
[15]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;
[7]597}
598
599void UavStateMachine::CheckPushButton(void) {
[15]600  GenericCheckPushButton();
601  ExtraCheckPushButton();
[7]602}
603
604void UavStateMachine::GenericCheckPushButton(void) {
[15]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();
[7]615}
616
617void UavStateMachine::EnterFailSafeMode(void) {
[15]618  SetAltitudeMode(AltitudeMode_t::Manual);
619  SetOrientationMode(OrientationMode_t::Manual);
620  SetThrustMode(ThrustMode_t::Default);
621  SetTorqueMode(TorqueMode_t::Default);
[7]622
[15]623  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
624                                                        currentAngularSpeed);
625  joy->SetYawRef(currentQuaternion);
626  uYaw->Reset();
627  uPitch->Reset();
628  uRoll->Reset();
[7]629
[15]630  failSafeMode = true;
631  SignalEvent(Event_t::EnteringFailSafeMode);
[7]632}
633
634bool UavStateMachine::ExitFailSafeMode(void) {
[15]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  /*
[7]639    if (altitudeState!=AltitudeState_t::Stabilized) {
640        return false;
641    } else*/ {
[15]642    failSafeMode = false;
643    return true;
644  }
[7]645}
646
647bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
[15]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)) {
656    uYaw->Reset();
657    uPitch->Reset();
658    uRoll->Reset();
659  }
660  torqueMode = newTorqueMode;
661  return true;
[7]662}
663
664bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
[15]665  if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
666    if (!ExitFailSafeMode())
667      return false;
668  }
669  altitudeMode = newAltitudeMode;
670  GotoAltitude(desiredTakeoffAltitude->Value());
[7]671
[15]672  return true;
[7]673}
674
675bool UavStateMachine::GotoAltitude(float desiredAltitude) {
[15]676  if (altitudeMode != AltitudeMode_t::Manual) {
677    return false;
678  }
679  altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
680                                desiredAltitude);
681  return true;
[7]682}
683
[15]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;
[7]699}
700
701bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
[15]702  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
703    if (!ExitFailSafeMode())
704      return false;
705  }
706  thrustMode = newThrustMode;
707  return true;
[7]708}
Note: See TracBrowser for help on using the repository browser.