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

Last change on this file since 38 was 38, checked in by Bayard Gildas, 8 years ago

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