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