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

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

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