| 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 | }
|
---|