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

Last change on this file was 422, checked in by Sanahuja Guillaume, 3 weeks ago

test pour recaler l'us en fonction de l'orientation

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