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

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

maj quaternion 3dmgx3

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