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

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

small fixes
File size: 25.6 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 ComputeAltitude();//ensure we have good altitude (Land() is not syncronized with Run())
516 altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
517 altitudeState=AltitudeState_t::StartLanding;
518 SignalEvent(Event_t::StartLanding);
519 } else if (altitudeState==AltitudeState_t::TakingOff) {
520 EmergencyLand();
521 } else {
522 joy->ErrorNotify();
523 }
524}
525
526void UavStateMachine::EmergencyLand(void) {
527 //Gradually decrease motor speed
528 //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)
529 altitudeState=AltitudeState_t::FinishLanding;
530 safeToFly=false;
531 Warn("Emergency landing!\n");
532 Warn("You will not be able to take off again\n");
533}
534
535void UavStateMachine::SignalEvent(Event_t event) {
536 switch (event) {
537 case Event_t::StartLanding:
538 Thread::Info("Altitude: entering 'StartLanding' state\n");
539 break;
540 case Event_t::Stopped:
541 Thread::Info("Altitude: entering 'Stopped' state\n");
542 break;
543 case Event_t::TakingOff:
544 Thread::Info("Altitude: taking off\n");
545 break;
546 case Event_t::Stabilized:
547 Thread::Info("Altitude: entering 'Stabilized' state\n");
548 break;
549 case Event_t::FinishLanding:
550 Thread::Info("Altitude: entering 'FinishLanding' state\n");
551 break;
552 case Event_t::EmergencyStop:
553 Thread::Info("Emergency stop!\n");
554 break;
555 case Event_t::EnteringFailSafeMode:
556 Thread::Info("Entering fail safe mode\n");
557 break;
558 }
559}
560
561void UavStateMachine::EmergencyStop(void) {
562 if(altitudeState!=AltitudeState_t::Stopped) {
563 StopMotors();
564 EnterFailSafeMode();
565 joy->Rumble(0x70);
566 SignalEvent(Event_t::EmergencyStop);
567 }
568 //safeToFly=false;
569 //Warn("Emergency stop, UAV will not take off again until program is rerunned\n");
570}
571
572void UavStateMachine::StopMotors(void) {
573 joy->FlashLed(1, 10, 10); // DualShock3::led1
574 uav->GetBldc()->SetEnabled(false);
575 uav->GetUavMultiplex()->UnlockUserInterface();
576 altitudeState = AltitudeState_t::Stopped;
577 SignalEvent(Event_t::Stopped);
578 uav->GetAhrs()->UnlockUserInterface();
579
580 uZ->Reset();
581 uRoll->Reset();
582 uPitch->Reset();
583 uYaw->Reset();
584}
585
586GridLayout *UavStateMachine::GetButtonsLayout(void) const {
587 return buttonslayout;
588}
589
590void UavStateMachine::SecurityCheck(void) {
591 MandatorySecurityCheck();
592 ExtraSecurityCheck();
593}
594
595void UavStateMachine::MandatorySecurityCheck(void) {
596 if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
597 flagConnectionLost = true;
598
599 EnterFailSafeMode();
600 if (altitudeState == AltitudeState_t::Stopped) {
601 Thread::Warn("Connection lost\n");
602 Thread::Warn("UAV won't take off\n");
603 } else {
604 Thread::Err("Connection lost\n");
605 Land();
606 }
607 }
608 if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
609 flagBatteryLow=true;
610 Thread::Err("Low Battery\n");
611 EnterFailSafeMode();
612 Land();
613 }/*
614 Time now=GetTime();
615 if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
616 flagCriticalSensorLost=true;
617 Thread::Err("Critical sensor lost\n");
618 EnterFailSafeMode();
619 EmergencyLand();
620 }*/
621}
622
623void UavStateMachine::CheckJoystick(void) {
624 GenericCheckJoystick();
625 ExtraCheckJoystick();
626}
627
628void UavStateMachine::GenericCheckJoystick(void) {
629 static bool isEmergencyStopButtonPressed = false;
630 static bool isTakeOffButtonPressed = false;
631 static bool isSafeModeButtonPressed = false;
632
633 if (controller->IsButtonPressed(1)) { // select
634 if (!isEmergencyStopButtonPressed) {
635 isEmergencyStopButtonPressed = true;
636 Thread::Info("Emergency stop from joystick\n");
637 EmergencyStop();
638 }
639 } else
640 isEmergencyStopButtonPressed = false;
641
642 if (controller->IsButtonPressed(0)) { // start
643 if (!isTakeOffButtonPressed) {
644 isTakeOffButtonPressed = true;
645 switch (altitudeState) {
646 case AltitudeState_t::Stopped:
647 TakeOff();
648 break;
649 case AltitudeState_t::Stabilized:
650 Land();
651 break;
652 default:
653 joy->ErrorNotify();
654 break;
655 }
656 }
657 } else
658 isTakeOffButtonPressed = false;
659
660 // cross
661 // gsanahuj:conflict with Majd programs.
662 // check if l1,l2,r1 and r2 are not pressed
663 // to allow a combination in user program
664 if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
665 !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
666 !controller->IsButtonPressed(10)) {
667 if (!isSafeModeButtonPressed) {
668 isSafeModeButtonPressed = true;
669 EnterFailSafeMode();
670 }
671 } else
672 isSafeModeButtonPressed = false;
673}
674
675void UavStateMachine::CheckPushButton(void) {
676 GenericCheckPushButton();
677 ExtraCheckPushButton();
678}
679
680void UavStateMachine::GenericCheckPushButton(void) {
681 if (button_kill->Clicked() == true)
682 SafeStop();
683 if (button_take_off->Clicked() == true)
684 TakeOff();
685 if (button_land->Clicked() == true)
686 Land();
687 if (button_start_log->Clicked() == true)
688 getFrameworkManager()->StartLog();
689 if (button_stop_log->Clicked() == true)
690 getFrameworkManager()->StopLog();
691}
692
693void UavStateMachine::EnterFailSafeMode(void) {
694 if(altitudeState!=AltitudeState_t::StartLanding) SetAltitudeMode(AltitudeMode_t::Manual);//
695 SetOrientationMode(OrientationMode_t::Manual);
696 SetThrustMode(ThrustMode_t::Default);
697 SetTorqueMode(TorqueMode_t::Default);
698
699 GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
700 currentAngularSpeed);
701 joy->SetYawRef(currentQuaternion);
702 uYaw->Reset();
703 uPitch->Reset();
704 uRoll->Reset();
705
706 failSafeMode = true;
707 SignalEvent(Event_t::EnteringFailSafeMode);
708}
709
710bool UavStateMachine::ExitFailSafeMode(void) {
711 // only exit fail safe mode if in Stabilized altitude state
712 // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
713 // le ruban perturbe l'us
714 /*
715 if (altitudeState!=AltitudeState_t::Stabilized) {
716 return false;
717 } else*/ {
718 failSafeMode = false;
719 return true;
720 }
721}
722
723bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
724 if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
725 if (!ExitFailSafeMode())
726 return false;
727 }
728 // When transitionning from Custom to Default torque mode, we should reset the
729 // default control laws
730 if ((torqueMode == TorqueMode_t::Custom) &&
731 (newTorqueMode == TorqueMode_t::Default)) {
732 uYaw->Reset();
733 uPitch->Reset();
734 uRoll->Reset();
735 }
736 torqueMode = newTorqueMode;
737 return true;
738}
739
740bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
741 if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
742 if (!ExitFailSafeMode())
743 return false;
744 }
745 altitudeMode = newAltitudeMode;
746 //avoid starting trajectory at take off (it will be started when altitudeState==AltitudeState_t::Stabilized)
747 if(altitudeState!=AltitudeState_t::Stopped) GotoAltitude(desiredTakeoffAltitude->Value());
748
749 return true;
750}
751
752bool UavStateMachine::GotoAltitude(float desiredAltitude) {
753 if (altitudeMode != AltitudeMode_t::Manual) {
754 return false;
755 }
756 altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
757 desiredAltitude);
758 return true;
759}
760
761bool UavStateMachine::SetOrientationMode(
762 OrientationMode_t const &newOrientationMode) {
763 if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
764 if (!ExitFailSafeMode())
765 return false;
766 }
767 // When transitionning from Custom to Manual mode we must reset to yaw
768 // reference to the current absolute yaw angle,
769 // overwise the Uav will abruptly change orientation
770 if ((orientationMode == OrientationMode_t::Custom) &&
771 (newOrientationMode == OrientationMode_t::Manual)) {
772 joy->SetYawRef(currentQuaternion);
773 }
774 orientationMode = newOrientationMode;
775 return true;
776}
777
778bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
779 if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
780 if (!ExitFailSafeMode())
781 return false;
782 }
783 thrustMode = newThrustMode;
784 return true;
785}
786
787NestedSat* UavStateMachine::GetURoll(void) { return uRoll;};
788NestedSat* UavStateMachine::GetUPitch(void) { return uPitch;};
789Pid* UavStateMachine::GetUYaw(void) { return uYaw;};
790PidThrust* UavStateMachine::GetUZ(void) { return uZ;};
791TrajectoryGenerator1D * UavStateMachine::GetAltitudeTrajectory(void) { return altitudeTrajectory;};
Note: See TracBrowser for help on using the repository browser.