source: flair-src/tags/latest/lib/FlairMeta/src/UavStateMachine.cpp

Last change on this file was 400, checked in by Sanahuja Guillaume, 9 months ago

cosmetic changes

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