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

Last change on this file since 38 was 38, checked in by Bayard Gildas, 5 years ago

Modif. pour ajour manette émulée (EmulatedController?)

File size: 23.1 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,TargetController *controller):
54        Thread(getFrameworkManager(),"UavStateMachine",50),
55        uav(inUav),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
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(),"uav high level controller",controller);
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 =
100      new GroupBox(uavTab->NewRow(), "takeoff/landing");
101  desiredTakeoffAltitude =
102      new DoubleSpinBox(reglagesGroupbox->NewRow(), "desired takeoff altitude",
103                        " m", 0, 5, 0.1, 2, 1);
104  desiredLandingAltitude =
105      new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),
106                        "desired landing altitude", " m", 0, 1, 0.1, 1);
107  altitudeTrajectory =
108      new TrajectoryGenerator1D(uavTab->NewRow(), "alt cons", "m");
109  uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(
110      altitudeTrajectory->Matrix()->Element(0), DataPlot::Green);
111  uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
112      altitudeTrajectory->Matrix()->Element(1), DataPlot::Green);
113}
114
115UavStateMachine::~UavStateMachine() {}
116
117void UavStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
118  uZ->AddDeviceToLog(device);
119}
120
121void UavStateMachine::AddDataToControlLawLog(const core::io_data *data) {
122  uZ->AddDataToLog(data);
123}
124
125const TargetController *UavStateMachine::GetJoystick(void) const {
126    return controller;
127}
128
129const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
130  return currentQuaternion;
131}
132
133const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
134  return currentAngularSpeed;
135}
136
137const Uav *UavStateMachine::GetUav(void) const { return uav; }
138
139void UavStateMachine::AltitudeValues(float &altitude,
140                                     float &verticalSpeed) const {
141  FailSafeAltitudeValues(altitude, verticalSpeed);
142}
143
144void UavStateMachine::FailSafeAltitudeValues(float &altitude,
145                                             float &verticalSpeed) const {
146  altitude = uav->GetMetaUsRangeFinder()->z();
147  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
148}
149
150void UavStateMachine::Run() {
151  WarnUponSwitches(true);
152  uav->StartSensors();
153
154  if (getFrameworkManager()->ErrorOccured() == true) {
155    SafeStop();
156  }
157
158  while (!ToBeStopped()) {
159    SecurityCheck();
160
161    // get controller inputs
162    CheckJoystick();
163    CheckPushButton();
164
165    if (IsPeriodSet()) {
166      WaitPeriod();
167    } else {
168      WaitUpdate(uav->GetAhrs());
169    }
170    needToComputeDefaultTorques = true;
171    needToComputeDefaultThrust = true;
172
173    SignalEvent(Event_t::EnteringControlLoop);
174
175    ComputeOrientation();
176    ComputeAltitude();
177
178    // compute thrust and torques to apply
179    ComputeTorques();
180    ComputeThrust(); // logs are added to uz, so it must be updated at last
181
182    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
183    // thrust (value between 0 and 1)
184    uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
185    uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
186    uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
187    uav->GetUavMultiplex()->SetThrust(-currentThrust); // on raisonne en negatif
188                                                       // sur l'altitude, a
189                                                       // revoir avec les
190                                                       // equations
191    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
192    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
193    uav->GetUavMultiplex()->SetYawTrim(0);
194    uav->GetUavMultiplex()->Update(GetTime());
195  }
196
197  WarnUponSwitches(false);
198}
199
200void UavStateMachine::ComputeOrientation(void) {
201  if (failSafeMode) {
202    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
203                                                          currentAngularSpeed);
204  } else {
205    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
206                                                   currentAngularSpeed);
207  }
208}
209
210const AhrsData *UavStateMachine::GetOrientation(void) const {
211  return GetDefaultOrientation();
212}
213
214const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
215  return uav->GetAhrs()->GetDatas();
216}
217
218void UavStateMachine::ComputeAltitude(void) {
219  if (failSafeMode) {
220    FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
221  } else {
222    AltitudeValues(currentAltitude, currentVerticalSpeed);
223  }
224}
225
226void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
227                                               float &refVerticalVelocity) {
228  if (altitudeMode == AltitudeMode_t::Manual) {
229    GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
230  } else {
231    GetReferenceAltitude(refAltitude, refVerticalVelocity);
232  }
233}
234
235void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
236                                                  float &refVerticalVelocity) {
237  float zc, dzc;
238
239  switch (altitudeState) {
240  // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
241  // until we detect a take off of 0.03m (hard coded value) above the ground.
242  case AltitudeState_t::TakingOff: {
243    if (currentAltitude > groundAltitude + 0.03) {
244      altitudeTrajectory->StartTraj(currentAltitude,
245                                    desiredTakeoffAltitude->Value());
246      altitudeState = AltitudeState_t::Stabilized;
247      SignalEvent(Event_t::Stabilized);
248    }
249    break;
250  }
251  // landing, only check if we reach desired landing altitude
252  case AltitudeState_t::StartLanding: {
253    if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
254      // The Uav target altitude has reached its landing value (typically 0)
255      // but the real Uav altitude may not have reach this value yet because of
256      // command delay. Moreover, it may never exactly reach this value if the
257      // ground is not perfectly 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) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
427        //The uav always takes off in fail safe mode
428        EnterFailSafeMode();
429        joy->SetLedOFF(4);//DualShock3::led4
430        joy->SetLedOFF(1);//DualShock3::led1
431        joy->Rumble(0x70);
432        joy->SetZRef(0);
433
434    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
435                     // valeur initiale (station sol)
436
437    uav->GetUavMultiplex()->LockUserInterface();
438    // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
439    // consignes moteurs
440    // sans les faire tourner effectivement (en déplaçant à la main le drone)
441    uav->GetBldc()->SetEnabled(true);
442    groundAltitude = currentAltitude;
443    altitudeState = AltitudeState_t::TakingOff;
444
445    SignalEvent(Event_t::TakingOff);
446  } else {
447    joy->ErrorNotify();
448  }
449}
450
451void UavStateMachine::Land(void) {
452  if (altitudeMode != AltitudeMode_t::Manual) {
453    SetAltitudeMode(AltitudeMode_t::Manual);
454  }
455  if (altitudeState == AltitudeState_t::Stabilized) {
456    joy->SetLedOFF(4); // DualShock3::led4
457    joy->Rumble(0x70);
458
459        altitudeTrajectory->StopTraj();
460        joy->SetZRef(0);
461        altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
462        SignalEvent(Event_t::StartLanding);
463        altitudeState=AltitudeState_t::StartLanding;
464    } else if (altitudeState==AltitudeState_t::TakingOff) {
465        EmergencyLand();
466    } else {
467        joy->ErrorNotify();
468    }
469}
470
471void UavStateMachine::EmergencyLand(void) {
472    //Gradually decrease motor speed
473    //Called if landing is required during take off (motors are accelerating but Uav did not actually left the ground yet), or if critical sensors have been lost (attitude is lost)
474    altitudeState=AltitudeState_t::FinishLanding;
475    safeToFly=false;
476Printf("Emergency landing!\n");
477}
478
479void UavStateMachine::SignalEvent(Event_t event) {
480  switch (event) {
481  case Event_t::StartLanding:
482    Thread::Info("Altitude: entering 'StartLanding' state\n");
483    break;
484  case Event_t::Stopped:
485    Thread::Info("Altitude: entering 'Stopped' state\n");
486    break;
487  case Event_t::TakingOff:
488    Thread::Info("Altitude: taking off\n");
489    break;
490  case Event_t::Stabilized:
491    Thread::Info("Altitude: entering 'Stabilized' state\n");
492    break;
493  case Event_t::FinishLanding:
494    Thread::Info("Altitude: entering 'FinishLanding' state\n");
495    break;
496  case Event_t::EmergencyStop:
497    Thread::Info("Emergency stop!\n");
498    break;
499  }
500}
501
502void UavStateMachine::EmergencyStop(void) {
503    if(altitudeState!=AltitudeState_t::Stopped) {
504        StopMotors();
505        EnterFailSafeMode();
506        joy->Rumble(0x70);
507        SignalEvent(Event_t::EmergencyStop);
508    }
509    safeToFly=false;
510}
511
512void UavStateMachine::StopMotors(void) {
513  joy->FlashLed(1, 10, 10); // DualShock3::led1
514  uav->GetBldc()->SetEnabled(false);
515  uav->GetUavMultiplex()->UnlockUserInterface();
516  SignalEvent(Event_t::Stopped);
517  altitudeState = AltitudeState_t::Stopped;
518  uav->GetAhrs()->UnlockUserInterface();
519
520  uZ->SetValues(0, 0);
521  uZ->Reset();
522}
523
524GridLayout *UavStateMachine::GetButtonsLayout(void) const {
525  return buttonslayout;
526}
527
528void UavStateMachine::SecurityCheck(void) {
529  MandatorySecurityCheck();
530  ExtraSecurityCheck();
531}
532
533void UavStateMachine::MandatorySecurityCheck(void) {
534  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
535    flagConnectionLost = true;
536    Thread::Err("Connection lost\n");
537    EnterFailSafeMode();
538    if (altitudeState == AltitudeState_t::Stopped) {
539      SafeStop();
540    } else {
541      Land();
542    }
543  }
544    if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
545        flagBatteryLow=true;
546        Thread::Err("Low Battery\n");
547        EnterFailSafeMode();
548        Land();
549    }
550    Time now=GetTime();
551    if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
552        flagCriticalSensorLost=true;
553        Thread::Err("Critical sensor lost\n");
554        EnterFailSafeMode();
555        EmergencyLand(); 
556    }
557}
558
559void UavStateMachine::CheckJoystick(void) {
560  GenericCheckJoystick();
561  ExtraCheckJoystick();
562}
563
564void UavStateMachine::GenericCheckJoystick(void) {
565  static bool isEmergencyStopButtonPressed = false;
566  static bool isTakeOffButtonPressed = false;
567  static bool isSafeModeButtonPressed = false;
568
569  if (controller->IsButtonPressed(1)) { // select
570    if (!isEmergencyStopButtonPressed) {
571      isEmergencyStopButtonPressed = true;
572      Thread::Info("Emergency stop from joystick\n");
573      EmergencyStop();
574    }
575  } else
576    isEmergencyStopButtonPressed = false;
577
578  if (controller->IsButtonPressed(0)) { // start
579    if (!isTakeOffButtonPressed) {
580      isTakeOffButtonPressed = true;
581      switch (altitudeState) {
582      case AltitudeState_t::Stopped:
583        TakeOff();
584        break;
585      case AltitudeState_t::Stabilized:
586        Land();
587        break;
588      default:
589        joy->ErrorNotify();
590        break;
591      }
592    }
593  } else
594    isTakeOffButtonPressed = false;
595
596  // cross
597  // gsanahuj:conflict with Majd programs.
598  // check if l1,l2,r1 and r2 are not pressed
599  // to allow a combination in user program
600  if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
601      !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
602      !controller->IsButtonPressed(10)) {
603    if (!isSafeModeButtonPressed) {
604      isSafeModeButtonPressed = true;
605      Thread::Info("Entering fail safe mode\n");
606      EnterFailSafeMode();
607    }
608  } else
609    isSafeModeButtonPressed = false;
610}
611
612void UavStateMachine::CheckPushButton(void) {
613  GenericCheckPushButton();
614  ExtraCheckPushButton();
615}
616
617void UavStateMachine::GenericCheckPushButton(void) {
618  if (button_kill->Clicked() == true)
619    SafeStop();
620  if (button_take_off->Clicked() == true)
621    TakeOff();
622  if (button_land->Clicked() == true)
623    Land();
624  if (button_start_log->Clicked() == true)
625    getFrameworkManager()->StartLog();
626  if (button_stop_log->Clicked() == true)
627    getFrameworkManager()->StopLog();
628}
629
630void UavStateMachine::EnterFailSafeMode(void) {
631  SetAltitudeMode(AltitudeMode_t::Manual);
632  SetOrientationMode(OrientationMode_t::Manual);
633  SetThrustMode(ThrustMode_t::Default);
634  SetTorqueMode(TorqueMode_t::Default);
635
636  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
637                                                        currentAngularSpeed);
638  joy->SetYawRef(currentQuaternion);
639  uYaw->Reset();
640  uPitch->Reset();
641  uRoll->Reset();
642
643  failSafeMode = true;
644  SignalEvent(Event_t::EnteringFailSafeMode);
645}
646
647bool UavStateMachine::ExitFailSafeMode(void) {
648  // only exit fail safe mode if in Stabilized altitude state
649  // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
650  // le ruban perturbe l'us
651  /*
652    if (altitudeState!=AltitudeState_t::Stabilized) {
653        return false;
654    } else*/ {
655    failSafeMode = false;
656    return true;
657  }
658}
659
660bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
661  if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
662    if (!ExitFailSafeMode())
663      return false;
664  }
665  // When transitionning from Custom to Default torque mode, we should reset the
666  // default control laws
667  if ((torqueMode == TorqueMode_t::Custom) &&
668      (newTorqueMode == TorqueMode_t::Default)) {
669    uYaw->Reset();
670    uPitch->Reset();
671    uRoll->Reset();
672  }
673  torqueMode = newTorqueMode;
674  return true;
675}
676
677bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
678  if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
679    if (!ExitFailSafeMode())
680      return false;
681  }
682  altitudeMode = newAltitudeMode;
683  GotoAltitude(desiredTakeoffAltitude->Value());
684
685  return true;
686}
687
688bool UavStateMachine::GotoAltitude(float desiredAltitude) {
689  if (altitudeMode != AltitudeMode_t::Manual) {
690    return false;
691  }
692  altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
693                                desiredAltitude);
694  return true;
695}
696
697bool UavStateMachine::SetOrientationMode(
698    OrientationMode_t const &newOrientationMode) {
699  if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
700    if (!ExitFailSafeMode())
701      return false;
702  }
703  // When transitionning from Custom to Manual mode we must reset to yaw
704  // reference to the current absolute yaw angle,
705  // overwise the Uav will abruptly change orientation
706  if ((orientationMode == OrientationMode_t::Custom) &&
707      (newOrientationMode == OrientationMode_t::Manual)) {
708    joy->SetYawRef(currentQuaternion);
709  }
710  orientationMode = newOrientationMode;
711  return true;
712}
713
714bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
715  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
716    if (!ExitFailSafeMode())
717      return false;
718  }
719  thrustMode = newThrustMode;
720  return true;
721}
Note: See TracBrowser for help on using the repository browser.