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

Last change on this file since 93 was 45, checked in by Sanahuja Guillaume, 8 years ago

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