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

Last change on this file since 7 was 7, checked in by Sanahuja Guillaume, 8 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.