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

Last change on this file since 10 was 10, checked in by Sanahuja Guillaume, 6 years ago

lic

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