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

Last change on this file since 310 was 310, checked in by Sanahuja Guillaume, 3 years ago

small fixes

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