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

Last change on this file since 402 was 400, checked in by Sanahuja Guillaume, 4 years ago

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