[10] | 1 | // %flair:license{
|
---|
[15] | 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
[10] | 4 | // %flair:license}
|
---|
[7] | 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 |
|
---|
| 45 | using namespace std;
|
---|
| 46 | using namespace flair::core;
|
---|
| 47 | using namespace flair::gui;
|
---|
| 48 | using namespace flair::sensor;
|
---|
| 49 | using namespace flair::actuator;
|
---|
| 50 | using namespace flair::filter;
|
---|
| 51 | using namespace flair::meta;
|
---|
| 52 |
|
---|
[38] | 53 | UavStateMachine::UavStateMachine(Uav* inUav,TargetController *controller):
|
---|
| 54 | Thread(getFrameworkManager(),"UavStateMachine",50),
|
---|
| 55 | uav(inUav),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
|
---|
| 56 | altitudeState=AltitudeState_t::Stopped;
|
---|
| 57 | uav->UseDefaultPlot();
|
---|
[7] | 58 |
|
---|
[15] | 59 | Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
|
---|
| 60 | buttonslayout = new GridLayout(uavTab->NewRow(), "buttons");
|
---|
| 61 | button_kill = new PushButton(buttonslayout->NewRow(), "kill");
|
---|
| 62 | button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
|
---|
| 63 | button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
|
---|
| 64 | button_take_off = new PushButton(buttonslayout->NewRow(), "take_off");
|
---|
| 65 | button_land = new PushButton(buttonslayout->LastRowLastCol(), "land");
|
---|
[7] | 66 |
|
---|
[15] | 67 | Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
|
---|
| 68 | TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
|
---|
| 69 | setupLawTab = new Tab(tabWidget, "Setup");
|
---|
| 70 | graphLawTab = new Tab(tabWidget, "Graphes");
|
---|
[7] | 71 |
|
---|
[15] | 72 | uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
|
---|
| 73 | uRoll->ConvertSatFromDegToRad();
|
---|
| 74 | uRoll->UseDefaultPlot(graphLawTab->NewRow());
|
---|
[7] | 75 |
|
---|
[15] | 76 | uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
|
---|
| 77 | uPitch->ConvertSatFromDegToRad();
|
---|
| 78 | uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
|
---|
[7] | 79 |
|
---|
[15] | 80 | uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
|
---|
| 81 | uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
|
---|
[7] | 82 |
|
---|
[15] | 83 | uZ = new PidThrust(setupLawTab->At(1, 2), "u_z");
|
---|
| 84 | uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
|
---|
[7] | 85 |
|
---|
[15] | 86 | getFrameworkManager()->AddDeviceToLog(uZ);
|
---|
| 87 | uZ->AddDeviceToLog(uRoll);
|
---|
| 88 | uZ->AddDeviceToLog(uPitch);
|
---|
| 89 | uZ->AddDeviceToLog(uYaw);
|
---|
[7] | 90 |
|
---|
[38] | 91 | joy=new MetaDualShock3(getFrameworkManager(),"uav high level controller",controller);
|
---|
| 92 | uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
|
---|
[7] | 93 |
|
---|
[15] | 94 | altitudeMode = AltitudeMode_t::Manual;
|
---|
| 95 | orientationMode = OrientationMode_t::Manual;
|
---|
| 96 | thrustMode = ThrustMode_t::Default;
|
---|
| 97 | torqueMode = TorqueMode_t::Default;
|
---|
[7] | 98 |
|
---|
[15] | 99 | GroupBox *reglagesGroupbox =
|
---|
| 100 | new GroupBox(uavTab->NewRow(), "takeoff/landing");
|
---|
| 101 | desiredTakeoffAltitude =
|
---|
| 102 | new DoubleSpinBox(reglagesGroupbox->NewRow(), "desired takeoff altitude",
|
---|
| 103 | " m", 0, 5, 0.1, 2, 1);
|
---|
| 104 | desiredLandingAltitude =
|
---|
| 105 | new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),
|
---|
| 106 | "desired landing altitude", " m", 0, 1, 0.1, 1);
|
---|
| 107 | altitudeTrajectory =
|
---|
| 108 | new TrajectoryGenerator1D(uavTab->NewRow(), "alt cons", "m");
|
---|
| 109 | uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(
|
---|
| 110 | altitudeTrajectory->Matrix()->Element(0), DataPlot::Green);
|
---|
| 111 | uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
|
---|
| 112 | altitudeTrajectory->Matrix()->Element(1), DataPlot::Green);
|
---|
[7] | 113 | }
|
---|
| 114 |
|
---|
[15] | 115 | UavStateMachine::~UavStateMachine() {}
|
---|
[7] | 116 |
|
---|
[15] | 117 | void UavStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
|
---|
| 118 | uZ->AddDeviceToLog(device);
|
---|
[7] | 119 | }
|
---|
| 120 |
|
---|
[15] | 121 | void UavStateMachine::AddDataToControlLawLog(const core::io_data *data) {
|
---|
| 122 | uZ->AddDataToLog(data);
|
---|
[7] | 123 | }
|
---|
| 124 |
|
---|
[38] | 125 | const TargetController *UavStateMachine::GetJoystick(void) const {
|
---|
| 126 | return controller;
|
---|
| 127 | }
|
---|
[7] | 128 |
|
---|
| 129 | const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
|
---|
[15] | 130 | return currentQuaternion;
|
---|
[7] | 131 | }
|
---|
| 132 |
|
---|
| 133 | const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
|
---|
[15] | 134 | return currentAngularSpeed;
|
---|
[7] | 135 | }
|
---|
| 136 |
|
---|
[15] | 137 | const Uav *UavStateMachine::GetUav(void) const { return uav; }
|
---|
[7] | 138 |
|
---|
[15] | 139 | void UavStateMachine::AltitudeValues(float &altitude,
|
---|
| 140 | float &verticalSpeed) const {
|
---|
| 141 | FailSafeAltitudeValues(altitude, verticalSpeed);
|
---|
[7] | 142 | }
|
---|
| 143 |
|
---|
[15] | 144 | void UavStateMachine::FailSafeAltitudeValues(float &altitude,
|
---|
| 145 | float &verticalSpeed) const {
|
---|
| 146 | altitude = uav->GetMetaUsRangeFinder()->z();
|
---|
| 147 | verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
|
---|
[7] | 148 | }
|
---|
| 149 |
|
---|
| 150 | void UavStateMachine::Run() {
|
---|
[15] | 151 | WarnUponSwitches(true);
|
---|
| 152 | uav->StartSensors();
|
---|
[7] | 153 |
|
---|
[15] | 154 | if (getFrameworkManager()->ErrorOccured() == true) {
|
---|
| 155 | SafeStop();
|
---|
| 156 | }
|
---|
[7] | 157 |
|
---|
[15] | 158 | while (!ToBeStopped()) {
|
---|
| 159 | SecurityCheck();
|
---|
[7] | 160 |
|
---|
[15] | 161 | // get controller inputs
|
---|
| 162 | CheckJoystick();
|
---|
| 163 | CheckPushButton();
|
---|
[7] | 164 |
|
---|
[15] | 165 | if (IsPeriodSet()) {
|
---|
| 166 | WaitPeriod();
|
---|
| 167 | } else {
|
---|
| 168 | WaitUpdate(uav->GetAhrs());
|
---|
| 169 | }
|
---|
| 170 | needToComputeDefaultTorques = true;
|
---|
| 171 | needToComputeDefaultThrust = true;
|
---|
[7] | 172 |
|
---|
[15] | 173 | SignalEvent(Event_t::EnteringControlLoop);
|
---|
[7] | 174 |
|
---|
[15] | 175 | ComputeOrientation();
|
---|
| 176 | ComputeAltitude();
|
---|
[7] | 177 |
|
---|
[15] | 178 | // compute thrust and torques to apply
|
---|
| 179 | ComputeTorques();
|
---|
| 180 | ComputeThrust(); // logs are added to uz, so it must be updated at last
|
---|
[7] | 181 |
|
---|
[15] | 182 | // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
|
---|
| 183 | // thrust (value between 0 and 1)
|
---|
| 184 | uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
|
---|
| 185 | uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
|
---|
| 186 | uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
|
---|
| 187 | uav->GetUavMultiplex()->SetThrust(-currentThrust); // on raisonne en negatif
|
---|
| 188 | // sur l'altitude, a
|
---|
| 189 | // revoir avec les
|
---|
| 190 | // equations
|
---|
| 191 | uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
|
---|
| 192 | uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
|
---|
| 193 | uav->GetUavMultiplex()->SetYawTrim(0);
|
---|
| 194 | uav->GetUavMultiplex()->Update(GetTime());
|
---|
| 195 | }
|
---|
[7] | 196 |
|
---|
[15] | 197 | WarnUponSwitches(false);
|
---|
[7] | 198 | }
|
---|
| 199 |
|
---|
| 200 | void UavStateMachine::ComputeOrientation(void) {
|
---|
[15] | 201 | if (failSafeMode) {
|
---|
| 202 | GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
|
---|
| 203 | currentAngularSpeed);
|
---|
| 204 | } else {
|
---|
| 205 | GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
|
---|
| 206 | currentAngularSpeed);
|
---|
| 207 | }
|
---|
[7] | 208 | }
|
---|
| 209 |
|
---|
| 210 | const AhrsData *UavStateMachine::GetOrientation(void) const {
|
---|
[15] | 211 | return GetDefaultOrientation();
|
---|
[7] | 212 | }
|
---|
| 213 |
|
---|
| 214 | const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
|
---|
[15] | 215 | return uav->GetAhrs()->GetDatas();
|
---|
[7] | 216 | }
|
---|
| 217 |
|
---|
| 218 | void UavStateMachine::ComputeAltitude(void) {
|
---|
[15] | 219 | if (failSafeMode) {
|
---|
| 220 | FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
|
---|
| 221 | } else {
|
---|
| 222 | AltitudeValues(currentAltitude, currentVerticalSpeed);
|
---|
| 223 | }
|
---|
[7] | 224 | }
|
---|
| 225 |
|
---|
[15] | 226 | void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
|
---|
| 227 | float &refVerticalVelocity) {
|
---|
| 228 | if (altitudeMode == AltitudeMode_t::Manual) {
|
---|
| 229 | GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
|
---|
| 230 | } else {
|
---|
| 231 | GetReferenceAltitude(refAltitude, refVerticalVelocity);
|
---|
| 232 | }
|
---|
[7] | 233 | }
|
---|
| 234 |
|
---|
[15] | 235 | void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
|
---|
| 236 | float &refVerticalVelocity) {
|
---|
| 237 | float zc, dzc;
|
---|
[7] | 238 |
|
---|
[15] | 239 | switch (altitudeState) {
|
---|
| 240 | // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
|
---|
| 241 | // until we detect a take off of 0.03m (hard coded value) above the ground.
|
---|
| 242 | case AltitudeState_t::TakingOff: {
|
---|
| 243 | if (currentAltitude > groundAltitude + 0.03) {
|
---|
| 244 | altitudeTrajectory->StartTraj(currentAltitude,
|
---|
| 245 | desiredTakeoffAltitude->Value());
|
---|
| 246 | altitudeState = AltitudeState_t::Stabilized;
|
---|
| 247 | SignalEvent(Event_t::Stabilized);
|
---|
[7] | 248 | }
|
---|
[15] | 249 | break;
|
---|
| 250 | }
|
---|
| 251 | // landing, only check if we reach desired landing altitude
|
---|
| 252 | case AltitudeState_t::StartLanding: {
|
---|
| 253 | if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
|
---|
| 254 | // The Uav target altitude has reached its landing value (typically 0)
|
---|
| 255 | // but the real Uav altitude may not have reach this value yet because of
|
---|
[38] | 256 | // command delay. Moreover, it may never exactly reach this value if the
|
---|
| 257 | // ground is not perfectly leveled (critical case: there's a
|
---|
[15] | 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 | }
|
---|
[7] | 279 |
|
---|
[15] | 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();
|
---|
[7] | 286 |
|
---|
[15] | 287 | // z control law
|
---|
| 288 | altitudeTrajectory->SetPositionOffset(zc);
|
---|
| 289 | altitudeTrajectory->SetSpeedOffset(dzc);
|
---|
[7] | 290 |
|
---|
[15] | 291 | altitudeTrajectory->Update(GetTime());
|
---|
| 292 | refAltitude = altitudeTrajectory->Position();
|
---|
| 293 | refVerticalVelocity = altitudeTrajectory->Speed();
|
---|
[7] | 294 | }
|
---|
| 295 |
|
---|
[15] | 296 | void 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();
|
---|
[7] | 301 | };
|
---|
| 302 |
|
---|
| 303 | void UavStateMachine::ComputeThrust(void) {
|
---|
[15] | 304 | if (altitudeMode == AltitudeMode_t::Manual) {
|
---|
| 305 | currentThrust = ComputeDefaultThrust();
|
---|
| 306 | } else {
|
---|
| 307 | currentThrust = ComputeCustomThrust();
|
---|
| 308 | }
|
---|
[7] | 309 | }
|
---|
| 310 |
|
---|
| 311 | float UavStateMachine::ComputeDefaultThrust(void) {
|
---|
[15] | 312 | if (needToComputeDefaultThrust) {
|
---|
| 313 | // compute desired altitude
|
---|
| 314 | float refAltitude, refVerticalVelocity;
|
---|
| 315 | ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
|
---|
[7] | 316 |
|
---|
[15] | 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;
|
---|
[7] | 324 | }
|
---|
[15] | 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());
|
---|
[7] | 342 |
|
---|
[15] | 343 | savedDefaultThrust = uZ->Output();
|
---|
| 344 | needToComputeDefaultThrust = false;
|
---|
| 345 | }
|
---|
| 346 |
|
---|
| 347 | return savedDefaultThrust;
|
---|
[7] | 348 | }
|
---|
| 349 |
|
---|
| 350 | float UavStateMachine::ComputeCustomThrust(void) {
|
---|
[15] | 351 | Thread::Warn("Default GetThrust method is not overloaded => switching back "
|
---|
| 352 | "to safe mode\n");
|
---|
| 353 | EnterFailSafeMode();
|
---|
| 354 | return ComputeDefaultThrust();
|
---|
[7] | 355 | }
|
---|
| 356 |
|
---|
[15] | 357 | const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
|
---|
| 358 | if (orientationMode == OrientationMode_t::Manual) {
|
---|
| 359 | return GetDefaultReferenceOrientation();
|
---|
| 360 | } else {
|
---|
| 361 | return GetReferenceOrientation();
|
---|
| 362 | }
|
---|
[7] | 363 | }
|
---|
| 364 |
|
---|
[15] | 365 | const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
|
---|
| 366 | // We directly control yaw, pitch, roll angles
|
---|
| 367 | return joy->GetReferenceOrientation();
|
---|
[7] | 368 | }
|
---|
| 369 |
|
---|
[15] | 370 | const 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();
|
---|
[7] | 375 | }
|
---|
| 376 |
|
---|
| 377 | void UavStateMachine::ComputeTorques(void) {
|
---|
[15] | 378 | if (torqueMode == TorqueMode_t::Default) {
|
---|
| 379 | ComputeDefaultTorques(currentTorques);
|
---|
| 380 | } else {
|
---|
| 381 | ComputeCustomTorques(currentTorques);
|
---|
| 382 | }
|
---|
[7] | 383 | }
|
---|
| 384 |
|
---|
| 385 | void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
|
---|
[15] | 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();
|
---|
[7] | 394 |
|
---|
[15] | 395 | uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
|
---|
| 396 | currentAngularSpeed.z - refAngularRates.z);
|
---|
| 397 | uYaw->Update(GetTime());
|
---|
| 398 | torques.yaw = uYaw->Output();
|
---|
[7] | 399 |
|
---|
[15] | 400 | uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
|
---|
| 401 | currentAngularSpeed.y);
|
---|
| 402 | uPitch->Update(GetTime());
|
---|
| 403 | torques.pitch = uPitch->Output();
|
---|
[7] | 404 |
|
---|
[15] | 405 | uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
|
---|
| 406 | uRoll->Update(GetTime());
|
---|
| 407 | torques.roll = uRoll->Output();
|
---|
[7] | 408 |
|
---|
[15] | 409 | savedDefaultTorques = torques;
|
---|
| 410 | needToComputeDefaultTorques = false;
|
---|
| 411 | } else {
|
---|
| 412 | torques = savedDefaultTorques;
|
---|
| 413 | }
|
---|
[7] | 414 | }
|
---|
| 415 |
|
---|
| 416 | void UavStateMachine::ComputeCustomTorques(Euler &torques) {
|
---|
[15] | 417 | Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
|
---|
| 418 | "switching back to safe mode\n");
|
---|
| 419 | EnterFailSafeMode();
|
---|
| 420 | ComputeDefaultTorques(torques);
|
---|
[7] | 421 | }
|
---|
| 422 |
|
---|
| 423 | void UavStateMachine::TakeOff(void) {
|
---|
[15] | 424 | flagZTrajectoryFinished = false;
|
---|
[7] | 425 |
|
---|
[38] | 426 | if((altitudeState==AltitudeState_t::Stopped) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
|
---|
| 427 | //The uav always takes off in fail safe mode
|
---|
| 428 | EnterFailSafeMode();
|
---|
| 429 | joy->SetLedOFF(4);//DualShock3::led4
|
---|
| 430 | joy->SetLedOFF(1);//DualShock3::led1
|
---|
| 431 | joy->Rumble(0x70);
|
---|
| 432 | joy->SetZRef(0);
|
---|
[7] | 433 |
|
---|
[15] | 434 | uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
|
---|
| 435 | // valeur initiale (station sol)
|
---|
[7] | 436 |
|
---|
[15] | 437 | uav->GetUavMultiplex()->LockUserInterface();
|
---|
| 438 | // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
|
---|
| 439 | // consignes moteurs
|
---|
| 440 | // sans les faire tourner effectivement (en déplaçant à la main le drone)
|
---|
| 441 | uav->GetBldc()->SetEnabled(true);
|
---|
| 442 | groundAltitude = currentAltitude;
|
---|
| 443 | altitudeState = AltitudeState_t::TakingOff;
|
---|
[7] | 444 |
|
---|
[15] | 445 | SignalEvent(Event_t::TakingOff);
|
---|
| 446 | } else {
|
---|
| 447 | joy->ErrorNotify();
|
---|
| 448 | }
|
---|
[7] | 449 | }
|
---|
| 450 |
|
---|
| 451 | void UavStateMachine::Land(void) {
|
---|
[15] | 452 | if (altitudeMode != AltitudeMode_t::Manual) {
|
---|
| 453 | SetAltitudeMode(AltitudeMode_t::Manual);
|
---|
| 454 | }
|
---|
| 455 | if (altitudeState == AltitudeState_t::Stabilized) {
|
---|
| 456 | joy->SetLedOFF(4); // DualShock3::led4
|
---|
| 457 | joy->Rumble(0x70);
|
---|
[7] | 458 |
|
---|
[38] | 459 | altitudeTrajectory->StopTraj();
|
---|
| 460 | joy->SetZRef(0);
|
---|
| 461 | altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
|
---|
| 462 | SignalEvent(Event_t::StartLanding);
|
---|
| 463 | altitudeState=AltitudeState_t::StartLanding;
|
---|
| 464 | } else if (altitudeState==AltitudeState_t::TakingOff) {
|
---|
| 465 | EmergencyLand();
|
---|
| 466 | } else {
|
---|
| 467 | joy->ErrorNotify();
|
---|
| 468 | }
|
---|
[7] | 469 | }
|
---|
| 470 |
|
---|
[38] | 471 | void UavStateMachine::EmergencyLand(void) {
|
---|
| 472 | //Gradually decrease motor speed
|
---|
| 473 | //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)
|
---|
| 474 | altitudeState=AltitudeState_t::FinishLanding;
|
---|
| 475 | safeToFly=false;
|
---|
| 476 | Printf("Emergency landing!\n");
|
---|
| 477 | }
|
---|
| 478 |
|
---|
[7] | 479 | void UavStateMachine::SignalEvent(Event_t event) {
|
---|
[15] | 480 | switch (event) {
|
---|
| 481 | case Event_t::StartLanding:
|
---|
| 482 | Thread::Info("Altitude: entering 'StartLanding' state\n");
|
---|
| 483 | break;
|
---|
| 484 | case Event_t::Stopped:
|
---|
| 485 | Thread::Info("Altitude: entering 'Stopped' state\n");
|
---|
| 486 | break;
|
---|
| 487 | case Event_t::TakingOff:
|
---|
| 488 | Thread::Info("Altitude: taking off\n");
|
---|
| 489 | break;
|
---|
| 490 | case Event_t::Stabilized:
|
---|
| 491 | Thread::Info("Altitude: entering 'Stabilized' state\n");
|
---|
| 492 | break;
|
---|
| 493 | case Event_t::FinishLanding:
|
---|
| 494 | Thread::Info("Altitude: entering 'FinishLanding' state\n");
|
---|
| 495 | break;
|
---|
| 496 | case Event_t::EmergencyStop:
|
---|
| 497 | Thread::Info("Emergency stop!\n");
|
---|
| 498 | break;
|
---|
| 499 | }
|
---|
[7] | 500 | }
|
---|
| 501 |
|
---|
| 502 | void UavStateMachine::EmergencyStop(void) {
|
---|
[38] | 503 | if(altitudeState!=AltitudeState_t::Stopped) {
|
---|
| 504 | StopMotors();
|
---|
| 505 | EnterFailSafeMode();
|
---|
| 506 | joy->Rumble(0x70);
|
---|
| 507 | SignalEvent(Event_t::EmergencyStop);
|
---|
| 508 | }
|
---|
| 509 | safeToFly=false;
|
---|
[7] | 510 | }
|
---|
| 511 |
|
---|
[15] | 512 | void UavStateMachine::StopMotors(void) {
|
---|
| 513 | joy->FlashLed(1, 10, 10); // DualShock3::led1
|
---|
| 514 | uav->GetBldc()->SetEnabled(false);
|
---|
| 515 | uav->GetUavMultiplex()->UnlockUserInterface();
|
---|
| 516 | SignalEvent(Event_t::Stopped);
|
---|
| 517 | altitudeState = AltitudeState_t::Stopped;
|
---|
| 518 | uav->GetAhrs()->UnlockUserInterface();
|
---|
[7] | 519 |
|
---|
[15] | 520 | uZ->SetValues(0, 0);
|
---|
| 521 | uZ->Reset();
|
---|
[7] | 522 | }
|
---|
| 523 |
|
---|
[15] | 524 | GridLayout *UavStateMachine::GetButtonsLayout(void) const {
|
---|
| 525 | return buttonslayout;
|
---|
[7] | 526 | }
|
---|
| 527 |
|
---|
| 528 | void UavStateMachine::SecurityCheck(void) {
|
---|
[15] | 529 | MandatorySecurityCheck();
|
---|
| 530 | ExtraSecurityCheck();
|
---|
[7] | 531 | }
|
---|
| 532 |
|
---|
| 533 | void UavStateMachine::MandatorySecurityCheck(void) {
|
---|
[15] | 534 | if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
|
---|
| 535 | flagConnectionLost = true;
|
---|
| 536 | Thread::Err("Connection lost\n");
|
---|
| 537 | EnterFailSafeMode();
|
---|
| 538 | if (altitudeState == AltitudeState_t::Stopped) {
|
---|
| 539 | SafeStop();
|
---|
| 540 | } else {
|
---|
| 541 | Land();
|
---|
[7] | 542 | }
|
---|
[15] | 543 | }
|
---|
[38] | 544 | if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
|
---|
| 545 | flagBatteryLow=true;
|
---|
| 546 | Thread::Err("Low Battery\n");
|
---|
| 547 | EnterFailSafeMode();
|
---|
| 548 | Land();
|
---|
| 549 | }
|
---|
| 550 | Time now=GetTime();
|
---|
| 551 | if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
|
---|
| 552 | flagCriticalSensorLost=true;
|
---|
| 553 | Thread::Err("Critical sensor lost\n");
|
---|
| 554 | EnterFailSafeMode();
|
---|
| 555 | EmergencyLand();
|
---|
| 556 | }
|
---|
[7] | 557 | }
|
---|
| 558 |
|
---|
| 559 | void UavStateMachine::CheckJoystick(void) {
|
---|
[15] | 560 | GenericCheckJoystick();
|
---|
| 561 | ExtraCheckJoystick();
|
---|
[7] | 562 | }
|
---|
| 563 |
|
---|
| 564 | void UavStateMachine::GenericCheckJoystick(void) {
|
---|
[15] | 565 | static bool isEmergencyStopButtonPressed = false;
|
---|
| 566 | static bool isTakeOffButtonPressed = false;
|
---|
| 567 | static bool isSafeModeButtonPressed = false;
|
---|
[7] | 568 |
|
---|
[38] | 569 | if (controller->IsButtonPressed(1)) { // select
|
---|
[15] | 570 | if (!isEmergencyStopButtonPressed) {
|
---|
| 571 | isEmergencyStopButtonPressed = true;
|
---|
| 572 | Thread::Info("Emergency stop from joystick\n");
|
---|
| 573 | EmergencyStop();
|
---|
| 574 | }
|
---|
| 575 | } else
|
---|
| 576 | isEmergencyStopButtonPressed = false;
|
---|
[7] | 577 |
|
---|
[38] | 578 | if (controller->IsButtonPressed(0)) { // start
|
---|
[15] | 579 | if (!isTakeOffButtonPressed) {
|
---|
| 580 | isTakeOffButtonPressed = true;
|
---|
| 581 | switch (altitudeState) {
|
---|
| 582 | case AltitudeState_t::Stopped:
|
---|
| 583 | TakeOff();
|
---|
| 584 | break;
|
---|
| 585 | case AltitudeState_t::Stabilized:
|
---|
| 586 | Land();
|
---|
| 587 | break;
|
---|
| 588 | default:
|
---|
| 589 | joy->ErrorNotify();
|
---|
| 590 | break;
|
---|
| 591 | }
|
---|
| 592 | }
|
---|
| 593 | } else
|
---|
| 594 | isTakeOffButtonPressed = false;
|
---|
[7] | 595 |
|
---|
[15] | 596 | // cross
|
---|
| 597 | // gsanahuj:conflict with Majd programs.
|
---|
| 598 | // check if l1,l2,r1 and r2 are not pressed
|
---|
| 599 | // to allow a combination in user program
|
---|
[38] | 600 | if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
|
---|
| 601 | !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
|
---|
| 602 | !controller->IsButtonPressed(10)) {
|
---|
[15] | 603 | if (!isSafeModeButtonPressed) {
|
---|
| 604 | isSafeModeButtonPressed = true;
|
---|
| 605 | Thread::Info("Entering fail safe mode\n");
|
---|
| 606 | EnterFailSafeMode();
|
---|
| 607 | }
|
---|
| 608 | } else
|
---|
| 609 | isSafeModeButtonPressed = false;
|
---|
[7] | 610 | }
|
---|
| 611 |
|
---|
| 612 | void UavStateMachine::CheckPushButton(void) {
|
---|
[15] | 613 | GenericCheckPushButton();
|
---|
| 614 | ExtraCheckPushButton();
|
---|
[7] | 615 | }
|
---|
| 616 |
|
---|
| 617 | void UavStateMachine::GenericCheckPushButton(void) {
|
---|
[15] | 618 | if (button_kill->Clicked() == true)
|
---|
| 619 | SafeStop();
|
---|
| 620 | if (button_take_off->Clicked() == true)
|
---|
| 621 | TakeOff();
|
---|
| 622 | if (button_land->Clicked() == true)
|
---|
| 623 | Land();
|
---|
| 624 | if (button_start_log->Clicked() == true)
|
---|
| 625 | getFrameworkManager()->StartLog();
|
---|
| 626 | if (button_stop_log->Clicked() == true)
|
---|
| 627 | getFrameworkManager()->StopLog();
|
---|
[7] | 628 | }
|
---|
| 629 |
|
---|
| 630 | void UavStateMachine::EnterFailSafeMode(void) {
|
---|
[15] | 631 | SetAltitudeMode(AltitudeMode_t::Manual);
|
---|
| 632 | SetOrientationMode(OrientationMode_t::Manual);
|
---|
| 633 | SetThrustMode(ThrustMode_t::Default);
|
---|
| 634 | SetTorqueMode(TorqueMode_t::Default);
|
---|
[7] | 635 |
|
---|
[15] | 636 | GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
|
---|
| 637 | currentAngularSpeed);
|
---|
| 638 | joy->SetYawRef(currentQuaternion);
|
---|
| 639 | uYaw->Reset();
|
---|
| 640 | uPitch->Reset();
|
---|
| 641 | uRoll->Reset();
|
---|
[7] | 642 |
|
---|
[15] | 643 | failSafeMode = true;
|
---|
| 644 | SignalEvent(Event_t::EnteringFailSafeMode);
|
---|
[7] | 645 | }
|
---|
| 646 |
|
---|
| 647 | bool UavStateMachine::ExitFailSafeMode(void) {
|
---|
[15] | 648 | // only exit fail safe mode if in Stabilized altitude state
|
---|
| 649 | // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
|
---|
| 650 | // le ruban perturbe l'us
|
---|
| 651 | /*
|
---|
[7] | 652 | if (altitudeState!=AltitudeState_t::Stabilized) {
|
---|
| 653 | return false;
|
---|
| 654 | } else*/ {
|
---|
[15] | 655 | failSafeMode = false;
|
---|
| 656 | return true;
|
---|
| 657 | }
|
---|
[7] | 658 | }
|
---|
| 659 |
|
---|
| 660 | bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
|
---|
[15] | 661 | if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
|
---|
| 662 | if (!ExitFailSafeMode())
|
---|
| 663 | return false;
|
---|
| 664 | }
|
---|
| 665 | // When transitionning from Custom to Default torque mode, we should reset the
|
---|
| 666 | // default control laws
|
---|
| 667 | if ((torqueMode == TorqueMode_t::Custom) &&
|
---|
| 668 | (newTorqueMode == TorqueMode_t::Default)) {
|
---|
| 669 | uYaw->Reset();
|
---|
| 670 | uPitch->Reset();
|
---|
| 671 | uRoll->Reset();
|
---|
| 672 | }
|
---|
| 673 | torqueMode = newTorqueMode;
|
---|
| 674 | return true;
|
---|
[7] | 675 | }
|
---|
| 676 |
|
---|
| 677 | bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
|
---|
[15] | 678 | if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
|
---|
| 679 | if (!ExitFailSafeMode())
|
---|
| 680 | return false;
|
---|
| 681 | }
|
---|
| 682 | altitudeMode = newAltitudeMode;
|
---|
| 683 | GotoAltitude(desiredTakeoffAltitude->Value());
|
---|
[7] | 684 |
|
---|
[15] | 685 | return true;
|
---|
[7] | 686 | }
|
---|
| 687 |
|
---|
| 688 | bool UavStateMachine::GotoAltitude(float desiredAltitude) {
|
---|
[15] | 689 | if (altitudeMode != AltitudeMode_t::Manual) {
|
---|
| 690 | return false;
|
---|
| 691 | }
|
---|
| 692 | altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
|
---|
| 693 | desiredAltitude);
|
---|
| 694 | return true;
|
---|
[7] | 695 | }
|
---|
| 696 |
|
---|
[15] | 697 | bool UavStateMachine::SetOrientationMode(
|
---|
| 698 | OrientationMode_t const &newOrientationMode) {
|
---|
| 699 | if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
|
---|
| 700 | if (!ExitFailSafeMode())
|
---|
| 701 | return false;
|
---|
| 702 | }
|
---|
| 703 | // When transitionning from Custom to Manual mode we must reset to yaw
|
---|
| 704 | // reference to the current absolute yaw angle,
|
---|
| 705 | // overwise the Uav will abruptly change orientation
|
---|
| 706 | if ((orientationMode == OrientationMode_t::Custom) &&
|
---|
| 707 | (newOrientationMode == OrientationMode_t::Manual)) {
|
---|
| 708 | joy->SetYawRef(currentQuaternion);
|
---|
| 709 | }
|
---|
| 710 | orientationMode = newOrientationMode;
|
---|
| 711 | return true;
|
---|
[7] | 712 | }
|
---|
| 713 |
|
---|
| 714 | bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
|
---|
[15] | 715 | if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
|
---|
| 716 | if (!ExitFailSafeMode())
|
---|
| 717 | return false;
|
---|
| 718 | }
|
---|
| 719 | thrustMode = newThrustMode;
|
---|
| 720 | return true;
|
---|
[7] | 721 | }
|
---|