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

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

maj thread

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