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

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

sources reformatted with flair-format-dir script

File size: 22.2 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
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
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; }
127
128const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
129  return currentQuaternion;
130}
131
132const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
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();
147}
148
149void UavStateMachine::Run() {
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);
197}
198
199void UavStateMachine::ComputeOrientation(void) {
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();
532    } else {
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");
541    EnterFailSafeMode();
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:
576        joy->ErrorNotify();
577        break;
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;
597}
598
599void UavStateMachine::CheckPushButton(void) {
600  GenericCheckPushButton();
601  ExtraCheckPushButton();
602}
603
604void UavStateMachine::GenericCheckPushButton(void) {
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();
615}
616
617void UavStateMachine::EnterFailSafeMode(void) {
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)) {
656    uYaw->Reset();
657    uPitch->Reset();
658    uRoll->Reset();
659  }
660  torqueMode = newTorqueMode;
661  return true;
662}
663
664bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
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;
673}
674
675bool UavStateMachine::GotoAltitude(float desiredAltitude) {
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;
699}
700
701bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
702  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
703    if (!ExitFailSafeMode())
704      return false;
705  }
706  thrustMode = newThrustMode;
707  return true;
708}
Note: See TracBrowser for help on using the repository browser.