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

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

filter and meta

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