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

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

resolution bug vrpn client pas demarré

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