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

Last change on this file since 248 was 216, checked in by Sanahuja Guillaume, 7 years ago

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