source: flair-src/branches/mavlink/lib/FlairMeta/src/UavStateMachine.cpp @ 98

Last change on this file since 98 was 42, checked in by Sanahuja Guillaume, 5 years ago

m

File size: 24.0 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#include <math.h>
45
46using namespace std;
47using namespace flair::core;
48using namespace flair::gui;
49using namespace flair::sensor;
50using namespace flair::actuator;
51using namespace flair::filter;
52using namespace flair::meta;
53
54UavStateMachine::UavStateMachine(Uav* inUav,TargetController *controller):
55        Thread(getFrameworkManager(),"UavStateMachine",50),
56        uav(inUav),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
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(),"uav high level controller",controller);
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 {
127    return controller;
128}
129
130const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
131  return currentQuaternion;
132}
133
134const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
135  return currentAngularSpeed;
136}
137
138const Uav *UavStateMachine::GetUav(void) const { return uav; }
139
140void UavStateMachine::AltitudeValues(float &altitude,
141                                     float &verticalSpeed) const {
142  FailSafeAltitudeValues(altitude, verticalSpeed);
143}
144
145void UavStateMachine::FailSafeAltitudeValues(float &altitude,
146                                             float &verticalSpeed) const {
147  altitude = uav->GetMetaUsRangeFinder()->z();
148  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
149}
150
151void UavStateMachine::Run() {
152  WarnUponSwitches(true);
153  uav->StartSensors();
154
155  if (getFrameworkManager()->ErrorOccured() == true) {
156    SafeStop();
157  }
158
159  while (!ToBeStopped()) {
160    SecurityCheck();
161
162    // get controller inputs
163    CheckJoystick();
164    CheckPushButton();
165
166    if (IsPeriodSet()) {
167      WaitPeriod();
168    } else {
169      WaitUpdate(uav->GetAhrs());
170    }
171    needToComputeDefaultTorques = true;
172    needToComputeDefaultThrust = true;
173
174    SignalEvent(Event_t::EnteringControlLoop);
175
176    ComputeOrientation();
177    ComputeAltitude();
178
179    // compute thrust and torques to apply
180    ComputeTorques();
181    ComputeThrust(); // logs are added to uz, so it must be updated at last
182
183    //check nan problems
184    if(CheckIsNan(currentTorques.roll,"roll torque")
185       || CheckIsNan(currentTorques.pitch,"pitch torque")
186       || CheckIsNan(currentTorques.yaw,"yaw torque")
187       || CheckIsNan(currentThrust,"thrust")) {
188
189      if(failSafeMode) {
190        Warn("We are already in safe mode, the uav is going to crash!\n");
191      } else {
192        Thread::Warn("switching back to safe mode\n");
193        EnterFailSafeMode();
194        needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
195        needToComputeDefaultThrust = true;
196
197        ComputeTorques();
198        ComputeThrust();
199      }
200    }
201
202    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
203    // thrust (value between 0 and 1)
204    uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
205    uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
206    uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
207    uav->GetUavMultiplex()->SetThrust(-currentThrust); // on raisonne en negatif
208                                                       // sur l'altitude, a
209                                                       // revoir avec les
210                                                       // equations
211    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
212    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
213    uav->GetUavMultiplex()->SetYawTrim(0);
214    uav->GetUavMultiplex()->Update(GetTime());
215  }
216
217  WarnUponSwitches(false);
218}
219
220bool UavStateMachine::CheckIsNan(float value,std::string desc) {
221  if(isnan(value)) {
222    Warn("%s is not an number\n",desc.c_str());
223    return true;
224  } else {
225    return false;
226  }
227}
228
229
230void UavStateMachine::ComputeOrientation(void) {
231  if (failSafeMode) {
232    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
233                                                          currentAngularSpeed);
234  } else {
235    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
236                                                   currentAngularSpeed);
237  }
238}
239
240const AhrsData *UavStateMachine::GetOrientation(void) const {
241  return GetDefaultOrientation();
242}
243
244const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
245  return uav->GetAhrs()->GetDatas();
246}
247
248void UavStateMachine::ComputeAltitude(void) {
249  if (failSafeMode) {
250    FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
251  } else {
252    AltitudeValues(currentAltitude, currentVerticalSpeed);
253  }
254}
255
256void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
257                                               float &refVerticalVelocity) {
258  if (altitudeMode == AltitudeMode_t::Manual) {
259    GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
260  } else {
261    GetReferenceAltitude(refAltitude, refVerticalVelocity);
262  }
263}
264
265void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
266                                                  float &refVerticalVelocity) {
267  float zc, dzc;
268
269  switch (altitudeState) {
270  // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
271  // until we detect a take off of 0.03m (hard coded value) above the ground.
272  case AltitudeState_t::TakingOff: {
273    if (currentAltitude > groundAltitude + 0.03) {
274      altitudeTrajectory->StartTraj(currentAltitude,
275                                    desiredTakeoffAltitude->Value());
276      altitudeState = AltitudeState_t::Stabilized;
277      SignalEvent(Event_t::Stabilized);
278    }
279    break;
280  }
281  // landing, only check if we reach desired landing altitude
282  case AltitudeState_t::StartLanding: {
283    if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
284      // The Uav target altitude has reached its landing value (typically 0)
285      // but the real Uav altitude may not have reach this value yet because of
286      // command delay. Moreover, it may never exactly reach this value if the
287      // ground is not perfectly leveled (critical case: there's a
288      // deep and narrow hole right in the sensor line of sight). That's why we
289      // have a 2 phases landing strategy.
290      altitudeState = AltitudeState_t::FinishLanding;
291      SignalEvent(Event_t::FinishLanding);
292      joy->SetLedOFF(1); // DualShock3::led1
293    }
294  }
295  // stabilized: check if z trajectory is finished
296  case AltitudeState_t::Stabilized: {
297    if (!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
298      SignalEvent(Event_t::ZTrajectoryFinished);
299      flagZTrajectoryFinished = true;
300    }
301    if (flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
302      flagZTrajectoryFinished = false;
303      altitudeTrajectory->StartTraj(currentAltitude,
304                                    desiredTakeoffAltitude->Value());
305      joy->SetZRef(0);
306    }
307  }
308  }
309
310  // Récupère les consignes (du joystick dans l'implémentation par défaut). La
311  // consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par
312  // la manette
313  zc = joy->ZRef(); // a revoir, la position offset devrait se calculer dans le
314                    // generator
315  dzc = joy->DzRef();
316
317  // z control law
318  altitudeTrajectory->SetPositionOffset(zc);
319  altitudeTrajectory->SetSpeedOffset(dzc);
320
321  altitudeTrajectory->Update(GetTime());
322  refAltitude = altitudeTrajectory->Position();
323  refVerticalVelocity = altitudeTrajectory->Speed();
324}
325
326void UavStateMachine::GetReferenceAltitude(float &refAltitude,
327                                           float &refVerticalVelocity) {
328  Thread::Warn("Default GetReferenceAltitude method is not overloaded => "
329               "switching back to safe mode\n");
330  EnterFailSafeMode();
331};
332
333void UavStateMachine::ComputeThrust(void) {
334  if (altitudeMode == AltitudeMode_t::Manual) {
335    currentThrust = ComputeDefaultThrust();
336  } else {
337    currentThrust = ComputeCustomThrust();
338  }
339}
340
341float UavStateMachine::ComputeDefaultThrust(void) {
342  if (needToComputeDefaultThrust) {
343    // compute desired altitude
344    float refAltitude, refVerticalVelocity;
345    ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
346
347    switch (altitudeState) {
348    case AltitudeState_t::TakingOff: {
349      // The progressive increase in motor speed is used to evaluate the motor
350      // speed that compensate the uav weight. This value
351      // will be used as an offset for altitude control afterwards
352      uZ->OffsetStepUp();
353      break;
354    }
355    case AltitudeState_t::StartLanding:
356    case AltitudeState_t::Stabilized: {
357      float p_error = currentAltitude - refAltitude;
358      float d_error = currentVerticalSpeed - refVerticalVelocity;
359      uZ->SetValues(p_error, d_error);
360      break;
361    }
362    // decrease motor speed in open loop until value offset_g , uav should have
363    // already landed or be very close to at this point
364    case AltitudeState_t::FinishLanding: {
365      if (uZ->OffsetStepDown() == false) {
366        StopMotors();
367      }
368      break;
369    }
370    }
371    uZ->Update(GetTime());
372
373    savedDefaultThrust = uZ->Output();
374    needToComputeDefaultThrust = false;
375  }
376
377  return savedDefaultThrust;
378}
379
380float UavStateMachine::ComputeCustomThrust(void) {
381  Thread::Warn("Default GetThrust method is not overloaded => switching back "
382               "to safe mode\n");
383  EnterFailSafeMode();
384  return ComputeDefaultThrust();
385}
386
387const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
388  if (orientationMode == OrientationMode_t::Manual) {
389    return GetDefaultReferenceOrientation();
390  } else {
391    return GetReferenceOrientation();
392  }
393}
394
395const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
396  // We directly control yaw, pitch, roll angles
397  return joy->GetReferenceOrientation();
398}
399
400const AhrsData *UavStateMachine::GetReferenceOrientation(void) {
401  Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
402               "switching back to safe mode\n");
403  EnterFailSafeMode();
404  return GetDefaultReferenceOrientation();
405}
406
407void UavStateMachine::ComputeTorques(void) {
408  if (torqueMode == TorqueMode_t::Default) {
409    ComputeDefaultTorques(currentTorques);
410  } else {
411    ComputeCustomTorques(currentTorques);
412  }
413}
414
415void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
416  if (needToComputeDefaultTorques) {
417    const AhrsData *refOrientation = ComputeReferenceOrientation();
418    Quaternion refQuaternion;
419    Vector3D refAngularRates;
420    refOrientation->GetQuaternionAndAngularRates(refQuaternion,
421                                                 refAngularRates);
422    Euler refAngles = refQuaternion.ToEuler();
423    Euler currentAngles = currentQuaternion.ToEuler();
424
425    uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
426                    currentAngularSpeed.z - refAngularRates.z);
427    uYaw->Update(GetTime());
428    torques.yaw = uYaw->Output();
429
430    uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
431                      currentAngularSpeed.y);
432    uPitch->Update(GetTime());
433    torques.pitch = uPitch->Output();
434
435    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
436    uRoll->Update(GetTime());
437    torques.roll = uRoll->Output();
438
439    savedDefaultTorques = torques;
440    needToComputeDefaultTorques = false;
441  } else {
442    torques = savedDefaultTorques;
443  }
444}
445
446void UavStateMachine::ComputeCustomTorques(Euler &torques) {
447  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
448               "switching back to safe mode\n");
449  EnterFailSafeMode();
450  ComputeDefaultTorques(torques);
451}
452
453void UavStateMachine::TakeOff(void) {
454  flagZTrajectoryFinished = false;
455
456    if((altitudeState==AltitudeState_t::Stopped) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
457        //The uav always takes off in fail safe mode
458        EnterFailSafeMode();
459        joy->SetLedOFF(4);//DualShock3::led4
460        joy->SetLedOFF(1);//DualShock3::led1
461        joy->Rumble(0x70);
462        joy->SetZRef(0);
463
464    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
465                     // valeur initiale (station sol)
466
467    uav->GetUavMultiplex()->LockUserInterface();
468    // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
469    // consignes moteurs
470    // sans les faire tourner effectivement (en déplaçant à la main le drone)
471    uav->GetBldc()->SetEnabled(true);
472    groundAltitude = currentAltitude;
473    altitudeState = AltitudeState_t::TakingOff;
474
475    SignalEvent(Event_t::TakingOff);
476  } else {
477    joy->ErrorNotify();
478  }
479}
480
481void UavStateMachine::Land(void) {
482  if (altitudeMode != AltitudeMode_t::Manual) {
483    SetAltitudeMode(AltitudeMode_t::Manual);
484  }
485  if (altitudeState == AltitudeState_t::Stabilized) {
486    joy->SetLedOFF(4); // DualShock3::led4
487    joy->Rumble(0x70);
488
489        altitudeTrajectory->StopTraj();
490        joy->SetZRef(0);
491        altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
492        SignalEvent(Event_t::StartLanding);
493        altitudeState=AltitudeState_t::StartLanding;
494    } else if (altitudeState==AltitudeState_t::TakingOff) {
495        EmergencyLand();
496    } else {
497        joy->ErrorNotify();
498    }
499}
500
501void UavStateMachine::EmergencyLand(void) {
502    //Gradually decrease motor speed
503    //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)
504    altitudeState=AltitudeState_t::FinishLanding;
505    safeToFly=false;
506Printf("Emergency landing!\n");
507}
508
509void UavStateMachine::SignalEvent(Event_t event) {
510  switch (event) {
511  case Event_t::StartLanding:
512    Thread::Info("Altitude: entering 'StartLanding' state\n");
513    break;
514  case Event_t::Stopped:
515    Thread::Info("Altitude: entering 'Stopped' state\n");
516    break;
517  case Event_t::TakingOff:
518    Thread::Info("Altitude: taking off\n");
519    break;
520  case Event_t::Stabilized:
521    Thread::Info("Altitude: entering 'Stabilized' state\n");
522    break;
523  case Event_t::FinishLanding:
524    Thread::Info("Altitude: entering 'FinishLanding' state\n");
525    break;
526  case Event_t::EmergencyStop:
527    Thread::Info("Emergency stop!\n");
528    break;
529  }
530}
531
532void UavStateMachine::EmergencyStop(void) {
533    if(altitudeState!=AltitudeState_t::Stopped) {
534        StopMotors();
535        EnterFailSafeMode();
536        joy->Rumble(0x70);
537        SignalEvent(Event_t::EmergencyStop);
538    }
539    safeToFly=false;
540}
541
542void UavStateMachine::StopMotors(void) {
543  joy->FlashLed(1, 10, 10); // DualShock3::led1
544  uav->GetBldc()->SetEnabled(false);
545  uav->GetUavMultiplex()->UnlockUserInterface();
546  SignalEvent(Event_t::Stopped);
547  altitudeState = AltitudeState_t::Stopped;
548  uav->GetAhrs()->UnlockUserInterface();
549
550  uZ->SetValues(0, 0);
551  uZ->Reset();
552}
553
554GridLayout *UavStateMachine::GetButtonsLayout(void) const {
555  return buttonslayout;
556}
557
558void UavStateMachine::SecurityCheck(void) {
559  MandatorySecurityCheck();
560  ExtraSecurityCheck();
561}
562
563void UavStateMachine::MandatorySecurityCheck(void) {
564  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
565    flagConnectionLost = true;
566    Thread::Err("Connection lost\n");
567    EnterFailSafeMode();
568    if (altitudeState == AltitudeState_t::Stopped) {
569      SafeStop();
570    } else {
571      Land();
572    }
573  }
574    if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
575        flagBatteryLow=true;
576        Thread::Err("Low Battery\n");
577        EnterFailSafeMode();
578        Land();
579    }/*
580    Time now=GetTime();
581    if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
582        flagCriticalSensorLost=true;
583        Thread::Err("Critical sensor lost\n");
584        EnterFailSafeMode();
585        EmergencyLand();
586    }*/
587}
588
589void UavStateMachine::CheckJoystick(void) {
590  GenericCheckJoystick();
591  ExtraCheckJoystick();
592}
593
594void UavStateMachine::GenericCheckJoystick(void) {
595  static bool isEmergencyStopButtonPressed = false;
596  static bool isTakeOffButtonPressed = false;
597  static bool isSafeModeButtonPressed = false;
598
599  if (controller->IsButtonPressed(1)) { // select
600    if (!isEmergencyStopButtonPressed) {
601      isEmergencyStopButtonPressed = true;
602      Thread::Info("Emergency stop from joystick\n");
603      EmergencyStop();
604    }
605  } else
606    isEmergencyStopButtonPressed = false;
607
608  if (controller->IsButtonPressed(0)) { // start
609    if (!isTakeOffButtonPressed) {
610      isTakeOffButtonPressed = true;
611      switch (altitudeState) {
612      case AltitudeState_t::Stopped:
613        TakeOff();
614        break;
615      case AltitudeState_t::Stabilized:
616        Land();
617        break;
618      default:
619        joy->ErrorNotify();
620        break;
621      }
622    }
623  } else
624    isTakeOffButtonPressed = false;
625
626  // cross
627  // gsanahuj:conflict with Majd programs.
628  // check if l1,l2,r1 and r2 are not pressed
629  // to allow a combination in user program
630  if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
631      !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
632      !controller->IsButtonPressed(10)) {
633    if (!isSafeModeButtonPressed) {
634      isSafeModeButtonPressed = true;
635      Thread::Info("Entering fail safe mode\n");
636      EnterFailSafeMode();
637    }
638  } else
639    isSafeModeButtonPressed = false;
640}
641
642void UavStateMachine::CheckPushButton(void) {
643  GenericCheckPushButton();
644  ExtraCheckPushButton();
645}
646
647void UavStateMachine::GenericCheckPushButton(void) {
648  if (button_kill->Clicked() == true)
649    SafeStop();
650  if (button_take_off->Clicked() == true)
651    TakeOff();
652  if (button_land->Clicked() == true)
653    Land();
654  if (button_start_log->Clicked() == true)
655    getFrameworkManager()->StartLog();
656  if (button_stop_log->Clicked() == true)
657    getFrameworkManager()->StopLog();
658}
659
660void UavStateMachine::EnterFailSafeMode(void) {
661  SetAltitudeMode(AltitudeMode_t::Manual);
662  SetOrientationMode(OrientationMode_t::Manual);
663  SetThrustMode(ThrustMode_t::Default);
664  SetTorqueMode(TorqueMode_t::Default);
665
666  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
667                                                        currentAngularSpeed);
668  joy->SetYawRef(currentQuaternion);
669  uYaw->Reset();
670  uPitch->Reset();
671  uRoll->Reset();
672
673  failSafeMode = true;
674  SignalEvent(Event_t::EnteringFailSafeMode);
675}
676
677bool UavStateMachine::ExitFailSafeMode(void) {
678  // only exit fail safe mode if in Stabilized altitude state
679  // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
680  // le ruban perturbe l'us
681  /*
682    if (altitudeState!=AltitudeState_t::Stabilized) {
683        return false;
684    } else*/ {
685    failSafeMode = false;
686    return true;
687  }
688}
689
690bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
691  if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
692    if (!ExitFailSafeMode())
693      return false;
694  }
695  // When transitionning from Custom to Default torque mode, we should reset the
696  // default control laws
697  if ((torqueMode == TorqueMode_t::Custom) &&
698      (newTorqueMode == TorqueMode_t::Default)) {
699    uYaw->Reset();
700    uPitch->Reset();
701    uRoll->Reset();
702  }
703  torqueMode = newTorqueMode;
704  return true;
705}
706
707bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
708  if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
709    if (!ExitFailSafeMode())
710      return false;
711  }
712  altitudeMode = newAltitudeMode;
713  GotoAltitude(desiredTakeoffAltitude->Value());
714
715  return true;
716}
717
718bool UavStateMachine::GotoAltitude(float desiredAltitude) {
719  if (altitudeMode != AltitudeMode_t::Manual) {
720    return false;
721  }
722  altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
723                                desiredAltitude);
724  return true;
725}
726
727bool UavStateMachine::SetOrientationMode(
728    OrientationMode_t const &newOrientationMode) {
729  if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
730    if (!ExitFailSafeMode())
731      return false;
732  }
733  // When transitionning from Custom to Manual mode we must reset to yaw
734  // reference to the current absolute yaw angle,
735  // overwise the Uav will abruptly change orientation
736  if ((orientationMode == OrientationMode_t::Custom) &&
737      (newOrientationMode == OrientationMode_t::Manual)) {
738    joy->SetYawRef(currentQuaternion);
739  }
740  orientationMode = newOrientationMode;
741  return true;
742}
743
744bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
745  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
746    if (!ExitFailSafeMode())
747      return false;
748  }
749  thrustMode = newThrustMode;
750  return true;
751}
Note: See TracBrowser for help on using the repository browser.