source: flair-src/tags/0.2.1/lib/FlairMeta/src/UavStateMachine.cpp@ 447

Last change on this file since 447 was 271, checked in by Sanahuja Guillaume, 5 years ago

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