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

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

iadded isready to iodevice:
avoid problem of imu not ready in ardrone2

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