source: flair-src/branches/sanscv/lib/FlairMeta/src/UavStateMachine.cpp@ 328

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

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