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

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

lic
File size: 22.2 KB
RevLine 
[10]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}
[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>
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.