source: flair-src/tags/0.0.1/lib/FlairMeta/src/UavStateMachine.cpp@ 171

Last change on this file since 171 was 15, checked in by Bayard Gildas, 9 years ago

sources reformatted with flair-format-dir script

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