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

Last change on this file since 460 was 432, checked in by Sanahuja Guillaume, 3 years ago

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