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

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

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