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

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

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