Changeset 38 in flair-src for trunk/lib/FlairMeta/src/UavStateMachine.cpp

Timestamp:

Jun 23, 2016, 10:15:30 AM (9 years ago)

Author:

Bayard Gildas

Message:

Modif. pour ajour manette émulée (EmulatedController)

File:

• trunk/lib/FlairMeta/src/UavStateMachine.cpp (modified) (11 diffs)

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -51 +51 @@
 using namespace flair::meta;
 
-UavStateMachine::UavStateMachine(Uav *inUav, uint16_t ds3Port)
-    : Thread(getFrameworkManager(), "UavStateMachine", 50), uav(inUav),
-      failSafeMode(true), flagConnectionLost(false), flagBatteryLow(false),
-      flagZTrajectoryFinished(false) {
-  altitudeState = AltitudeState_t::Stopped;
-  uav->UseDefaultPlot();
+UavStateMachine::UavStateMachine(Uav* inUav,TargetController *controller):
+        Thread(getFrameworkManager(),"UavStateMachine",50),
+        uav(inUav),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
+    altitudeState=AltitudeState_t::Stopped;
+    uav->UseDefaultPlot();
 
   Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
@@ -90 +89 @@
   uZ->AddDeviceToLog(uYaw);
 
-  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
-  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
+    joy=new MetaDualShock3(getFrameworkManager(),"uav high level controller",controller);
+    uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
 
   altitudeMode = AltitudeMode_t::Manual;
@@ -124 +123 @@
 }
 
-const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
+const TargetController *UavStateMachine::GetJoystick(void) const {
+    return controller;
+}
 
 const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
@@ -253 +254 @@
       // The Uav target altitude has reached its landing value (typically 0)
       // but the real Uav altitude may not have reach this value yet because of
-      // command delay. Moreover, it
-      // may never exactly reach this value if the ground is not perfectly
-      // leveled (critical case: there's a
+      // command delay. Moreover, it may never exactly reach this value if the 
+      // ground is not perfectly leveled (critical case: there's a
       // deep and narrow hole right in the sensor line of sight). That's why we
       // have a 2 phases landing strategy.
@@ -424 +424 @@
   flagZTrajectoryFinished = false;
 
-  if (altitudeState == AltitudeState_t::Stopped) { // &&
-    // GetBatteryMonitor()->IsBatteryLow()==false)
-    // The uav always takes off in fail safe mode
-    EnterFailSafeMode();
-    joy->SetLedOFF(4); // DualShock3::led4
-    joy->SetLedOFF(1); // DualShock3::led1
-    joy->Rumble(0x70);
-    joy->SetZRef(0);
+    if((altitudeState==AltitudeState_t::Stopped) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
+        //The uav always takes off in fail safe mode
+        EnterFailSafeMode();
+        joy->SetLedOFF(4);//DualShock3::led4
+        joy->SetLedOFF(1);//DualShock3::led1
+        joy->Rumble(0x70);
+        joy->SetZRef(0);
 
     uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
@@ -458 +457 @@
     joy->Rumble(0x70);
 
-    altitudeTrajectory->StopTraj();
-    joy->SetZRef(0);
-    altitudeTrajectory->StartTraj(
-        currentAltitude,
-        desiredLandingAltitude->Value()); // shouldn't it be groundAltitude?
-    SignalEvent(Event_t::StartLanding);
-    altitudeState = AltitudeState_t::StartLanding;
-  } else {
-    joy->ErrorNotify();
-  }
+        altitudeTrajectory->StopTraj();
+        joy->SetZRef(0);
+        altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
+        SignalEvent(Event_t::StartLanding);
+        altitudeState=AltitudeState_t::StartLanding;
+    } else if (altitudeState==AltitudeState_t::TakingOff) {
+        EmergencyLand();
+    } else {
+        joy->ErrorNotify();
+    }
+}
+
+void UavStateMachine::EmergencyLand(void) {
+    //Gradually decrease motor speed
+    //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)
+    altitudeState=AltitudeState_t::FinishLanding;
+    safeToFly=false;
+Printf("Emergency landing!\n");
 }
 
@@ -494 +501 @@
 
 void UavStateMachine::EmergencyStop(void) {
-  if (altitudeState != AltitudeState_t::Stopped) {
-    StopMotors();
-    EnterFailSafeMode();
-    joy->Rumble(0x70);
-    SignalEvent(Event_t::EmergencyStop);
-  }
+    if(altitudeState!=AltitudeState_t::Stopped) {
+        StopMotors();
+        EnterFailSafeMode();
+        joy->Rumble(0x70);
+        SignalEvent(Event_t::EmergencyStop);
+    }
+    safeToFly=false;
 }
 
@@ -534 +542 @@
     }
   }
-  if ((altitudeState == AltitudeState_t::TakingOff ||
-       altitudeState == AltitudeState_t::Stabilized) &&
-      uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
-    flagBatteryLow = true;
-    Printf("Battery Low\n");
-    EnterFailSafeMode();
-    Land();
-  }
+    if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
+        flagBatteryLow=true;
+        Thread::Err("Low Battery\n");
+        EnterFailSafeMode();
+        Land();
+    }
+    Time now=GetTime();
+    if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
+        flagCriticalSensorLost=true;
+        Thread::Err("Critical sensor lost\n");
+        EnterFailSafeMode();
+        EmergencyLand(); 
+    }
 }
 
@@ -554 +567 @@
   static bool isSafeModeButtonPressed = false;
 
-  if (joy->IsButtonPressed(1)) { // select
+  if (controller->IsButtonPressed(1)) { // select
     if (!isEmergencyStopButtonPressed) {
       isEmergencyStopButtonPressed = true;
@@ -563 +576 @@
     isEmergencyStopButtonPressed = false;
 
-  if (joy->IsButtonPressed(0)) { // start
+  if (controller->IsButtonPressed(0)) { // start
     if (!isTakeOffButtonPressed) {
       isTakeOffButtonPressed = true;
@@ -585 +598 @@
   // check if l1,l2,r1 and r2 are not pressed
   // to allow a combination in user program
-  if (joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) &&
-      !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) &&
-      !joy->IsButtonPressed(10)) {
+  if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
+      !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
+      !controller->IsButtonPressed(10)) {
     if (!isSafeModeButtonPressed) {
       isSafeModeButtonPressed = true;