source: flair-src/branches/mavlink/lib/FlairMeta/src/UavStateMachine.cpp@ 112

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

m
File size: 24.0 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 problems
184 if(CheckIsNan(currentTorques.roll,"roll torque")
185 || CheckIsNan(currentTorques.pitch,"pitch torque")
186 || CheckIsNan(currentTorques.yaw,"yaw torque")
187 || CheckIsNan(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::CheckIsNan(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 {
225 return false;
226 }
227}
228
229
230void UavStateMachine::ComputeOrientation(void) {
231 if (failSafeMode) {
232 GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
233 currentAngularSpeed);
234 } else {
235 GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
236 currentAngularSpeed);
237 }
238}
239
240const AhrsData *UavStateMachine::GetOrientation(void) const {
241 return GetDefaultOrientation();
242}
243
244const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
245 return uav->GetAhrs()->GetDatas();
246}
247
248void UavStateMachine::ComputeAltitude(void) {
249 if (failSafeMode) {
250 FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
251 } else {
252 AltitudeValues(currentAltitude, currentVerticalSpeed);
253 }
254}
255
256void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
257 float &refVerticalVelocity) {
258 if (altitudeMode == AltitudeMode_t::Manual) {
259 GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
260 } else {
261 GetReferenceAltitude(refAltitude, refVerticalVelocity);
262 }
263}
264
265void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
266 float &refVerticalVelocity) {
267 float zc, dzc;
268
269 switch (altitudeState) {
270 // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
271 // until we detect a take off of 0.03m (hard coded value) above the ground.
272 case AltitudeState_t::TakingOff: {
273 if (currentAltitude > groundAltitude + 0.03) {
274 altitudeTrajectory->StartTraj(currentAltitude,
275 desiredTakeoffAltitude->Value());
276 altitudeState = AltitudeState_t::Stabilized;
277 SignalEvent(Event_t::Stabilized);
278 }
279 break;
280 }
281 // landing, only check if we reach desired landing altitude
282 case AltitudeState_t::StartLanding: {
283 if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
284 // The Uav target altitude has reached its landing value (typically 0)
285 // but the real Uav altitude may not have reach this value yet because of
286 // command delay. Moreover, it may never exactly reach this value if the
287 // ground is not perfectly leveled (critical case: there's a
288 // deep and narrow hole right in the sensor line of sight). That's why we
289 // have a 2 phases landing strategy.
290 altitudeState = AltitudeState_t::FinishLanding;
291 SignalEvent(Event_t::FinishLanding);
292 joy->SetLedOFF(1); // DualShock3::led1
293 }
294 }
295 // stabilized: check if z trajectory is finished
296 case AltitudeState_t::Stabilized: {
297 if (!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
298 SignalEvent(Event_t::ZTrajectoryFinished);
299 flagZTrajectoryFinished = true;
300 }
301 if (flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
302 flagZTrajectoryFinished = false;
303 altitudeTrajectory->StartTraj(currentAltitude,
304 desiredTakeoffAltitude->Value());
305 joy->SetZRef(0);
306 }
307 }
308 }
309
310 // Récupère les consignes (du joystick dans l'implémentation par défaut). La
311 // consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par
312 // la manette
313 zc = joy->ZRef(); // a revoir, la position offset devrait se calculer dans le
314 // generator
315 dzc = joy->DzRef();
316
317 // z control law
318 altitudeTrajectory->SetPositionOffset(zc);
319 altitudeTrajectory->SetSpeedOffset(dzc);
320
321 altitudeTrajectory->Update(GetTime());
322 refAltitude = altitudeTrajectory->Position();
323 refVerticalVelocity = altitudeTrajectory->Speed();
324}
325
326void UavStateMachine::GetReferenceAltitude(float &refAltitude,
327 float &refVerticalVelocity) {
328 Thread::Warn("Default GetReferenceAltitude method is not overloaded => "
329 "switching back to safe mode\n");
330 EnterFailSafeMode();
331};
332
333void UavStateMachine::ComputeThrust(void) {
334 if (altitudeMode == AltitudeMode_t::Manual) {
335 currentThrust = ComputeDefaultThrust();
336 } else {
337 currentThrust = ComputeCustomThrust();
338 }
339}
340
341float UavStateMachine::ComputeDefaultThrust(void) {
342 if (needToComputeDefaultThrust) {
343 // compute desired altitude
344 float refAltitude, refVerticalVelocity;
345 ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
346
347 switch (altitudeState) {
348 case AltitudeState_t::TakingOff: {
349 // The progressive increase in motor speed is used to evaluate the motor
350 // speed that compensate the uav weight. This value
351 // will be used as an offset for altitude control afterwards
352 uZ->OffsetStepUp();
353 break;
354 }
355 case AltitudeState_t::StartLanding:
356 case AltitudeState_t::Stabilized: {
357 float p_error = currentAltitude - refAltitude;
358 float d_error = currentVerticalSpeed - refVerticalVelocity;
359 uZ->SetValues(p_error, d_error);
360 break;
361 }
362 // decrease motor speed in open loop until value offset_g , uav should have
363 // already landed or be very close to at this point
364 case AltitudeState_t::FinishLanding: {
365 if (uZ->OffsetStepDown() == false) {
366 StopMotors();
367 }
368 break;
369 }
370 }
371 uZ->Update(GetTime());
372
373 savedDefaultThrust = uZ->Output();
374 needToComputeDefaultThrust = false;
375 }
376
377 return savedDefaultThrust;
378}
379
380float UavStateMachine::ComputeCustomThrust(void) {
381 Thread::Warn("Default GetThrust method is not overloaded => switching back "
382 "to safe mode\n");
383 EnterFailSafeMode();
384 return ComputeDefaultThrust();
385}
386
387const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
388 if (orientationMode == OrientationMode_t::Manual) {
389 return GetDefaultReferenceOrientation();
390 } else {
391 return GetReferenceOrientation();
392 }
393}
394
395const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
396 // We directly control yaw, pitch, roll angles
397 return joy->GetReferenceOrientation();
398}
399
400const AhrsData *UavStateMachine::GetReferenceOrientation(void) {
401 Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
402 "switching back to safe mode\n");
403 EnterFailSafeMode();
404 return GetDefaultReferenceOrientation();
405}
406
407void UavStateMachine::ComputeTorques(void) {
408 if (torqueMode == TorqueMode_t::Default) {
409 ComputeDefaultTorques(currentTorques);
410 } else {
411 ComputeCustomTorques(currentTorques);
412 }
413}
414
415void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
416 if (needToComputeDefaultTorques) {
417 const AhrsData *refOrientation = ComputeReferenceOrientation();
418 Quaternion refQuaternion;
419 Vector3D refAngularRates;
420 refOrientation->GetQuaternionAndAngularRates(refQuaternion,
421 refAngularRates);
422 Euler refAngles = refQuaternion.ToEuler();
423 Euler currentAngles = currentQuaternion.ToEuler();
424
425 uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
426 currentAngularSpeed.z - refAngularRates.z);
427 uYaw->Update(GetTime());
428 torques.yaw = uYaw->Output();
429
430 uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
431 currentAngularSpeed.y);
432 uPitch->Update(GetTime());
433 torques.pitch = uPitch->Output();
434
435 uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
436 uRoll->Update(GetTime());
437 torques.roll = uRoll->Output();
438
439 savedDefaultTorques = torques;
440 needToComputeDefaultTorques = false;
441 } else {
442 torques = savedDefaultTorques;
443 }
444}
445
446void UavStateMachine::ComputeCustomTorques(Euler &torques) {
447 Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
448 "switching back to safe mode\n");
449 EnterFailSafeMode();
450 ComputeDefaultTorques(torques);
451}
452
453void UavStateMachine::TakeOff(void) {
454 flagZTrajectoryFinished = false;
455
456 if((altitudeState==AltitudeState_t::Stopped) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
457 //The uav always takes off in fail safe mode
458 EnterFailSafeMode();
459 joy->SetLedOFF(4);//DualShock3::led4
460 joy->SetLedOFF(1);//DualShock3::led1
461 joy->Rumble(0x70);
462 joy->SetZRef(0);
463
464 uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
465 // valeur initiale (station sol)
466
467 uav->GetUavMultiplex()->LockUserInterface();
468 // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
469 // consignes moteurs
470 // sans les faire tourner effectivement (en déplaçant à la main le drone)
471 uav->GetBldc()->SetEnabled(true);
472 groundAltitude = currentAltitude;
473 altitudeState = AltitudeState_t::TakingOff;
474
475 SignalEvent(Event_t::TakingOff);
476 } else {
477 joy->ErrorNotify();
478 }
479}
480
481void UavStateMachine::Land(void) {
482 if (altitudeMode != AltitudeMode_t::Manual) {
483 SetAltitudeMode(AltitudeMode_t::Manual);
484 }
485 if (altitudeState == AltitudeState_t::Stabilized) {
486 joy->SetLedOFF(4); // DualShock3::led4
487 joy->Rumble(0x70);
488
489 altitudeTrajectory->StopTraj();
490 joy->SetZRef(0);
491 altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
492 SignalEvent(Event_t::StartLanding);
493 altitudeState=AltitudeState_t::StartLanding;
494 } else if (altitudeState==AltitudeState_t::TakingOff) {
495 EmergencyLand();
496 } else {
497 joy->ErrorNotify();
498 }
499}
500
501void UavStateMachine::EmergencyLand(void) {
502 //Gradually decrease motor speed
503 //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)
504 altitudeState=AltitudeState_t::FinishLanding;
505 safeToFly=false;
506Printf("Emergency landing!\n");
507}
508
509void UavStateMachine::SignalEvent(Event_t event) {
510 switch (event) {
511 case Event_t::StartLanding:
512 Thread::Info("Altitude: entering 'StartLanding' state\n");
513 break;
514 case Event_t::Stopped:
515 Thread::Info("Altitude: entering 'Stopped' state\n");
516 break;
517 case Event_t::TakingOff:
518 Thread::Info("Altitude: taking off\n");
519 break;
520 case Event_t::Stabilized:
521 Thread::Info("Altitude: entering 'Stabilized' state\n");
522 break;
523 case Event_t::FinishLanding:
524 Thread::Info("Altitude: entering 'FinishLanding' state\n");
525 break;
526 case Event_t::EmergencyStop:
527 Thread::Info("Emergency stop!\n");
528 break;
529 }
530}
531
532void UavStateMachine::EmergencyStop(void) {
533 if(altitudeState!=AltitudeState_t::Stopped) {
534 StopMotors();
535 EnterFailSafeMode();
536 joy->Rumble(0x70);
537 SignalEvent(Event_t::EmergencyStop);
538 }
539 safeToFly=false;
540}
541
542void UavStateMachine::StopMotors(void) {
543 joy->FlashLed(1, 10, 10); // DualShock3::led1
544 uav->GetBldc()->SetEnabled(false);
545 uav->GetUavMultiplex()->UnlockUserInterface();
546 SignalEvent(Event_t::Stopped);
547 altitudeState = AltitudeState_t::Stopped;
548 uav->GetAhrs()->UnlockUserInterface();
549
550 uZ->SetValues(0, 0);
551 uZ->Reset();
552}
553
554GridLayout *UavStateMachine::GetButtonsLayout(void) const {
555 return buttonslayout;
556}
557
558void UavStateMachine::SecurityCheck(void) {
559 MandatorySecurityCheck();
560 ExtraSecurityCheck();
561}
562
563void UavStateMachine::MandatorySecurityCheck(void) {
564 if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
565 flagConnectionLost = true;
566 Thread::Err("Connection lost\n");
567 EnterFailSafeMode();
568 if (altitudeState == AltitudeState_t::Stopped) {
569 SafeStop();
570 } else {
571 Land();
572 }
573 }
574 if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
575 flagBatteryLow=true;
576 Thread::Err("Low Battery\n");
577 EnterFailSafeMode();
578 Land();
579 }/*
580 Time now=GetTime();
581 if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
582 flagCriticalSensorLost=true;
583 Thread::Err("Critical sensor lost\n");
584 EnterFailSafeMode();
585 EmergencyLand();
586 }*/
587}
588
589void UavStateMachine::CheckJoystick(void) {
590 GenericCheckJoystick();
591 ExtraCheckJoystick();
592}
593
594void UavStateMachine::GenericCheckJoystick(void) {
595 static bool isEmergencyStopButtonPressed = false;
596 static bool isTakeOffButtonPressed = false;
597 static bool isSafeModeButtonPressed = false;
598
599 if (controller->IsButtonPressed(1)) { // select
600 if (!isEmergencyStopButtonPressed) {
601 isEmergencyStopButtonPressed = true;
602 Thread::Info("Emergency stop from joystick\n");
603 EmergencyStop();
604 }
605 } else
606 isEmergencyStopButtonPressed = false;
607
608 if (controller->IsButtonPressed(0)) { // start
609 if (!isTakeOffButtonPressed) {
610 isTakeOffButtonPressed = true;
611 switch (altitudeState) {
612 case AltitudeState_t::Stopped:
613 TakeOff();
614 break;
615 case AltitudeState_t::Stabilized:
616 Land();
617 break;
618 default:
619 joy->ErrorNotify();
620 break;
621 }
622 }
623 } else
624 isTakeOffButtonPressed = false;
625
626 // cross
627 // gsanahuj:conflict with Majd programs.
628 // check if l1,l2,r1 and r2 are not pressed
629 // to allow a combination in user program
630 if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
631 !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
632 !controller->IsButtonPressed(10)) {
633 if (!isSafeModeButtonPressed) {
634 isSafeModeButtonPressed = true;
635 Thread::Info("Entering fail safe mode\n");
636 EnterFailSafeMode();
637 }
638 } else
639 isSafeModeButtonPressed = false;
640}
641
642void UavStateMachine::CheckPushButton(void) {
643 GenericCheckPushButton();
644 ExtraCheckPushButton();
645}
646
647void UavStateMachine::GenericCheckPushButton(void) {
648 if (button_kill->Clicked() == true)
649 SafeStop();
650 if (button_take_off->Clicked() == true)
651 TakeOff();
652 if (button_land->Clicked() == true)
653 Land();
654 if (button_start_log->Clicked() == true)
655 getFrameworkManager()->StartLog();
656 if (button_stop_log->Clicked() == true)
657 getFrameworkManager()->StopLog();
658}
659
660void UavStateMachine::EnterFailSafeMode(void) {
661 SetAltitudeMode(AltitudeMode_t::Manual);
662 SetOrientationMode(OrientationMode_t::Manual);
663 SetThrustMode(ThrustMode_t::Default);
664 SetTorqueMode(TorqueMode_t::Default);
665
666 GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
667 currentAngularSpeed);
668 joy->SetYawRef(currentQuaternion);
669 uYaw->Reset();
670 uPitch->Reset();
671 uRoll->Reset();
672
673 failSafeMode = true;
674 SignalEvent(Event_t::EnteringFailSafeMode);
675}
676
677bool UavStateMachine::ExitFailSafeMode(void) {
678 // only exit fail safe mode if in Stabilized altitude state
679 // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
680 // le ruban perturbe l'us
681 /*
682 if (altitudeState!=AltitudeState_t::Stabilized) {
683 return false;
684 } else*/ {
685 failSafeMode = false;
686 return true;
687 }
688}
689
690bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
691 if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
692 if (!ExitFailSafeMode())
693 return false;
694 }
695 // When transitionning from Custom to Default torque mode, we should reset the
696 // default control laws
697 if ((torqueMode == TorqueMode_t::Custom) &&
698 (newTorqueMode == TorqueMode_t::Default)) {
699 uYaw->Reset();
700 uPitch->Reset();
701 uRoll->Reset();
702 }
703 torqueMode = newTorqueMode;
704 return true;
705}
706
707bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
708 if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
709 if (!ExitFailSafeMode())
710 return false;
711 }
712 altitudeMode = newAltitudeMode;
713 GotoAltitude(desiredTakeoffAltitude->Value());
714
715 return true;
716}
717
718bool UavStateMachine::GotoAltitude(float desiredAltitude) {
719 if (altitudeMode != AltitudeMode_t::Manual) {
720 return false;
721 }
722 altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
723 desiredAltitude);
724 return true;
725}
726
727bool UavStateMachine::SetOrientationMode(
728 OrientationMode_t const &newOrientationMode) {
729 if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
730 if (!ExitFailSafeMode())
731 return false;
732 }
733 // When transitionning from Custom to Manual mode we must reset to yaw
734 // reference to the current absolute yaw angle,
735 // overwise the Uav will abruptly change orientation
736 if ((orientationMode == OrientationMode_t::Custom) &&
737 (newOrientationMode == OrientationMode_t::Manual)) {
738 joy->SetYawRef(currentQuaternion);
739 }
740 orientationMode = newOrientationMode;
741 return true;
742}
743
744bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
745 if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
746 if (!ExitFailSafeMode())
747 return false;
748 }
749 thrustMode = newThrustMode;
750 return true;
751}
Note: See TracBrowser for help on using the repository browser.