source: flair-src/trunk/lib/FlairMeta/src/PlaneStateMachine.cpp@ 469

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

remove executable property
File size: 14.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: 2022/01/05
6// filename: PlaneStateMachine.cpp
7//
8// author: Gildas Bayard, Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: state machine class for plane
14//
15//
16/*********************************************************************/
17
18#include "PlaneStateMachine.h"
19#include "Plane.h"
20#include <GridLayout.h>
21#include <DoubleSpinBox.h>
22#include <Tab.h>
23#include <TabWidget.h>
24#include <PushButton.h>
25#include <UavMultiplex.h>
26#include <Bldc.h>
27#include <Servos.h>
28#include <Ahrs.h>
29#include <ControlLaw.h>
30#include <Pid.h>
31#include <PidThrust.h>
32#include <NestedSat.h>
33#include <MetaDualShock3.h>
34#include <AhrsData.h>
35#include <BatteryMonitor.h>
36#include <FrameworkManager.h>
37#include <math.h>
38
39using namespace std;
40using namespace flair::core;
41using namespace flair::gui;
42using namespace flair::sensor;
43using namespace flair::actuator;
44using namespace flair::filter;
45using namespace flair::meta;
46
47PlaneStateMachine::PlaneStateMachine(TargetController *controller):
48 Thread(getFrameworkManager(),"PlaneStateMachine",50),
49 uav(GetPlane()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),safeToFly(true){
50
51 uav->UseDefaultPlot();
52
53 Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
54 buttonslayout = new GridLayout(uavTab->NewRow(), "buttons");
55 button_kill = new PushButton(buttonslayout->NewRow(), "kill");
56 button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
57 button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
58 button_start = new PushButton(buttonslayout->NewRow(), "start");
59 button_stop = new PushButton(buttonslayout->LastRowLastCol(), "stop");
60 thrust = new DoubleSpinBox(buttonslayout->NewRow(), "fixed thrust",0, 50, 0.1, 2);
61
62
63 Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
64 TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
65 setupLawTab = new Tab(tabWidget, "Setup");
66 graphLawTab = new Tab(tabWidget, "Graphes");
67
68 uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
69 uRoll->ConvertSatFromDegToRad();
70 uRoll->UseDefaultPlot(graphLawTab->NewRow());
71
72 uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
73 uPitch->ConvertSatFromDegToRad();
74 uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
75
76 uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
77 uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
78
79 getFrameworkManager()->AddDeviceToLog(uRoll);
80 uRoll->AddDeviceToLog(uPitch);
81 uRoll->AddDeviceToLog(uYaw);
82
83 joy=new MetaDualShock3("uav high level controller",controller);
84 uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
85
86 orientationMode = OrientationMode_t::Manual;
87 torqueMode = TorqueMode_t::Default;
88}
89
90PlaneStateMachine::~PlaneStateMachine() {}
91
92void PlaneStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
93 uRoll->AddDeviceToLog(device);
94}
95
96void PlaneStateMachine::AddDataToControlLawLog(const core::io_data *data) {
97 uRoll->AddDataToLog(data);
98}
99
100const TargetController *PlaneStateMachine::GetTargetController(void) const {
101 return controller;
102}
103
104MetaDualShock3 *PlaneStateMachine::GetJoystick(void) const {
105 return joy;
106}
107
108const Quaternion &PlaneStateMachine::GetCurrentQuaternion(void) const {
109 return currentQuaternion;
110}
111
112const Vector3Df &PlaneStateMachine::GetCurrentAngularSpeed(void) const {
113 return currentAngularSpeed;
114}
115
116
117void PlaneStateMachine::Run() {
118 WarnUponSwitches(true);
119 uav->StartSensors();
120//uav->GetBldc()->SetEnabled(true);
121//uav->GetServos()->SetEnabled(true);
122 if (getFrameworkManager()->ErrorOccured() == true) {
123 SafeStop();
124 }
125
126 while (!ToBeStopped()) {
127 SecurityCheck();
128
129 // get controller inputs
130 CheckJoystick();
131 CheckPushButton();
132
133 if (IsPeriodSet()) {
134 WaitPeriod();
135 } else {
136 WaitUpdate(uav->GetAhrs());
137 }
138 needToComputeDefaultTorques = true;
139
140 SignalEvent(Event_t::EnteringControlLoop);
141
142 ComputeOrientation();
143
144 // compute torques to apply
145 ComputeTorques();
146
147 //check nan/inf problems
148 bool isValuePossible=true;
149 //test it separately to warn for all problems
150 if(!IsValuePossible(currentTorques.roll,"roll torque")) isValuePossible=false;
151 if(!IsValuePossible(currentTorques.pitch,"pitch torque")) isValuePossible=false;
152 if(!IsValuePossible(currentTorques.yaw,"yaw torque")) isValuePossible=false;
153
154 if(!isValuePossible) {
155 if(failSafeMode) {
156 Warn("We are already in safe mode, the uav is going to crash!\n");
157 } else {
158 Thread::Warn("switching back to safe mode\n");
159 EnterFailSafeMode();
160 needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
161
162 ComputeTorques();
163 }
164
165 }
166
167 // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
168 // thrust (value between 0 and 1)
169 uav->GetUavMultiplex()->SetRoll(currentTorques.roll);
170 uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
171 uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
172 uav->GetUavMultiplex()->SetThrust(thrust->Value());
173
174 uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
175 uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
176 uav->GetUavMultiplex()->SetYawTrim(0);
177 uav->GetUavMultiplex()->Update(GetTime());
178 }
179
180 WarnUponSwitches(false);
181}
182
183bool PlaneStateMachine::IsValuePossible(float value,std::string desc) {
184 if(isnan(value)) {
185 Warn("%s is not an number\n",desc.c_str());
186 return false;
187 } else if(isinf(value)) {
188 Warn("%s is infinite\n",desc.c_str());
189 return false;
190 } else {
191 return true;
192 }
193}
194
195
196void PlaneStateMachine::ComputeOrientation(void) {
197 if (failSafeMode) {
198 GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
199 currentAngularSpeed);
200 } else {
201 GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
202 currentAngularSpeed);
203 }
204}
205
206const AhrsData *PlaneStateMachine::GetOrientation(void) const {
207 return GetDefaultOrientation();
208}
209
210const AhrsData *PlaneStateMachine::GetDefaultOrientation(void) const {
211 return uav->GetAhrs()->GetDatas();
212}
213
214
215const AhrsData *PlaneStateMachine::ComputeReferenceOrientation(void) {
216 if (orientationMode == OrientationMode_t::Manual) {
217 return GetDefaultReferenceOrientation();
218 } else {
219 return GetReferenceOrientation();
220 }
221}
222
223const AhrsData *PlaneStateMachine::GetDefaultReferenceOrientation(void) const {
224 // We directly control yaw, pitch, roll angles
225 return joy->GetReferenceOrientation();
226}
227
228const AhrsData *PlaneStateMachine::GetReferenceOrientation(void) {
229 Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
230 "switching back to safe mode\n");
231 EnterFailSafeMode();
232 return GetDefaultReferenceOrientation();
233}
234
235void PlaneStateMachine::ComputeTorques(void) {
236 if (torqueMode == TorqueMode_t::Default) {
237 ComputeDefaultTorques(currentTorques);
238 } else {
239 ComputeCustomTorques(currentTorques);
240 }
241}
242
243void PlaneStateMachine::ComputeDefaultTorques(Euler &torques) {
244 if (needToComputeDefaultTorques) {
245 const AhrsData *refOrientation = ComputeReferenceOrientation();
246 Quaternion refQuaternion;
247 Vector3Df refAngularRates;
248 refOrientation->GetQuaternionAndAngularRates(refQuaternion,
249 refAngularRates);
250 Euler refAngles = refQuaternion.ToEuler();
251 Euler currentAngles = currentQuaternion.ToEuler();
252
253 uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
254 currentAngularSpeed.z - refAngularRates.z);
255 uYaw->Update(GetTime());
256 torques.yaw = uYaw->Output();
257
258 uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
259 currentAngularSpeed.y);
260 uPitch->Update(GetTime());
261 torques.pitch = uPitch->Output();
262//Printf("%f %f %f %f\n",refAngles.pitch, currentAngles.pitch, currentAngularSpeed.y,torques.pitch);
263 uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
264 uRoll->Update(GetTime());
265 torques.roll = uRoll->Output();
266
267 savedDefaultTorques = torques;
268 needToComputeDefaultTorques = false;
269 } else {
270 torques = savedDefaultTorques;
271 }
272}
273
274void PlaneStateMachine::ComputeCustomTorques(Euler &torques) {
275 Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
276 "switching back to safe mode\n");
277 EnterFailSafeMode();
278 ComputeDefaultTorques(torques);
279}
280
281
282void PlaneStateMachine::SignalEvent(Event_t event) {
283 switch (event) {
284 case Event_t::Started:
285 Thread::Info("entering 'Started' state\n");
286 break;
287 case Event_t::Stopped:
288 Thread::Info("entering 'Stopped' state\n");
289 break;
290 case Event_t::EmergencyStop:
291 Thread::Info("Emergency stop!\n");
292 break;
293 case Event_t::EnteringFailSafeMode:
294 Thread::Info("Entering fail safe mode\n");
295 break;
296 }
297}
298
299void PlaneStateMachine::EmergencyStop(void) {
300
301 StopActuators();
302 EnterFailSafeMode();
303 joy->Rumble(0x70);
304 SignalEvent(Event_t::EmergencyStop);
305
306 //safeToFly=false;
307 //Warn("Emergency stop, UAV will not take off again until program is rerunned\n");
308}
309
310void PlaneStateMachine::StartActuators(void) {
311 //The uav always takes off in fail safe mode
312 flagBatteryLow=false;
313 EnterFailSafeMode();
314 joy->SetLedOFF(4);//DualShock3::led4
315 joy->SetLedOFF(1);//DualShock3::led1
316 joy->Rumble(0x70);
317 joy->SetZRef(0);
318
319 uRoll->Reset();
320 uPitch->Reset();
321 uYaw->Reset();
322
323 uav->GetUavMultiplex()->LockUserInterface();
324 // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
325 // consignes moteurs
326 // sans les faire tourner effectivement (en déplaçant à la main le drone)
327 uav->GetBldc()->SetEnabled(true);
328 uav->GetServos()->SetEnabled(true);
329 SignalEvent(Event_t::Started);
330}
331
332void PlaneStateMachine::StopActuators(void) {
333 joy->FlashLed(1, 10, 10); // DualShock3::led1
334 uav->GetBldc()->SetEnabled(false);
335 uav->GetServos()->SetEnabled(false);
336 uav->GetUavMultiplex()->UnlockUserInterface();
337 SignalEvent(Event_t::Stopped);
338 uav->GetAhrs()->UnlockUserInterface();
339
340 uRoll->Reset();
341 uPitch->Reset();
342 uYaw->Reset();
343}
344
345GridLayout *PlaneStateMachine::GetButtonsLayout(void) const {
346 return buttonslayout;
347}
348
349void PlaneStateMachine::SecurityCheck(void) {
350 MandatorySecurityCheck();
351 ExtraSecurityCheck();
352}
353
354void PlaneStateMachine::MandatorySecurityCheck(void) {
355 if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
356 flagConnectionLost = true;
357
358 EnterFailSafeMode();
359 }
360 if(uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
361 flagBatteryLow=true;
362 Thread::Err("Low Battery\n");
363 EnterFailSafeMode();
364 }
365}
366
367void PlaneStateMachine::CheckJoystick(void) {
368 GenericCheckJoystick();
369 ExtraCheckJoystick();
370}
371
372void PlaneStateMachine::GenericCheckJoystick(void) {
373 static bool isEmergencyStopButtonPressed = false;
374 static bool isTakeOffButtonPressed = false;
375 static bool isSafeModeButtonPressed = false;
376
377 if (controller->IsButtonPressed(1)) { // select
378 if (!isEmergencyStopButtonPressed) {
379 isEmergencyStopButtonPressed = true;
380 Thread::Info("Emergency stop from joystick\n");
381 EmergencyStop();
382 }
383 } else
384 isEmergencyStopButtonPressed = false;
385
386 if (controller->IsButtonPressed(0)) { // start
387 if (!isTakeOffButtonPressed) {
388 isTakeOffButtonPressed = true;
389 StartActuators();
390 }
391 } else
392 isTakeOffButtonPressed = false;
393
394 // cross
395 // gsanahuj:conflict with Majd programs.
396 // check if l1,l2,r1 and r2 are not pressed
397 // to allow a combination in user program
398 if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
399 !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
400 !controller->IsButtonPressed(10)) {
401 if (!isSafeModeButtonPressed) {
402 isSafeModeButtonPressed = true;
403 EnterFailSafeMode();
404 }
405 } else
406 isSafeModeButtonPressed = false;
407}
408
409void PlaneStateMachine::CheckPushButton(void) {
410 GenericCheckPushButton();
411 ExtraCheckPushButton();
412}
413
414void PlaneStateMachine::GenericCheckPushButton(void) {
415 if (button_kill->Clicked() == true)
416 SafeStop();
417 if (button_start_log->Clicked() == true)
418 getFrameworkManager()->StartLog();
419 if (button_stop_log->Clicked() == true)
420 getFrameworkManager()->StopLog();
421 if (button_start->Clicked() == true)
422 StartActuators();
423 if (button_stop->Clicked() == true)
424 StopActuators();
425}
426
427void PlaneStateMachine::EnterFailSafeMode(void) {
428 SetOrientationMode(OrientationMode_t::Manual);;
429 SetTorqueMode(TorqueMode_t::Default);
430
431 GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
432 currentAngularSpeed);
433 joy->SetYawRef(currentQuaternion);
434 uYaw->Reset();
435 uPitch->Reset();
436 uRoll->Reset();
437
438 failSafeMode = true;
439 SignalEvent(Event_t::EnteringFailSafeMode);
440}
441
442bool PlaneStateMachine::ExitFailSafeMode(void) {
443 // only exit fail safe mode if in Stabilized altitude state
444 // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
445 // le ruban perturbe l'us
446 /*
447 if (altitudeState!=AltitudeState_t::Stabilized) {
448 return false;
449 } else*/ {
450 failSafeMode = false;
451 return true;
452 }
453}
454
455bool PlaneStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
456 if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
457 if (!ExitFailSafeMode())
458 return false;
459 }
460 // When transitionning from Custom to Default torque mode, we should reset the
461 // default control laws
462 if ((torqueMode == TorqueMode_t::Custom) &&
463 (newTorqueMode == TorqueMode_t::Default)) {
464 uYaw->Reset();
465 uPitch->Reset();
466 uRoll->Reset();
467 }
468 torqueMode = newTorqueMode;
469 return true;
470}
471
472bool PlaneStateMachine::SetOrientationMode(
473 OrientationMode_t const &newOrientationMode) {
474 if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
475 if (!ExitFailSafeMode())
476 return false;
477 }
478 // When transitionning from Custom to Manual mode we must reset to yaw
479 // reference to the current absolute yaw angle,
480 // overwise the Uav will abruptly change orientation
481 if ((orientationMode == OrientationMode_t::Custom) &&
482 (newOrientationMode == OrientationMode_t::Manual)) {
483 joy->SetYawRef(currentQuaternion);
484 }
485 orientationMode = newOrientationMode;
486 return true;
487}
488
489
490NestedSat* PlaneStateMachine::GetURoll(void) { return uRoll;};
491NestedSat* PlaneStateMachine::GetUPitch(void) { return uPitch;};
492Pid* PlaneStateMachine::GetUYaw(void) { return uYaw;};
Note: See TracBrowser for help on using the repository browser.