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

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

added compile info

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