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

Timestamp:

Mar 13, 2019, 4:40:36 PM (6 years ago)

Author:

Sanahuja Guillaume

Message:

small fixes

File:

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

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -511 +511 @@
     joy->Rumble(0x70);
 
-        altitudeTrajectory->StopTraj();
-        joy->SetZRef(0);
-        altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
-        altitudeState=AltitudeState_t::StartLanding;
-        SignalEvent(Event_t::StartLanding);
-    } else if (altitudeState==AltitudeState_t::TakingOff) {
-        EmergencyLand();
-    } else {
-        joy->ErrorNotify();
-    }
+    altitudeTrajectory->StopTraj();
+    joy->SetZRef(0);
+    ComputeAltitude();//ensure we have good altitude (Land() is not syncronized with Run())
+    altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
+    altitudeState=AltitudeState_t::StartLanding;
+    SignalEvent(Event_t::StartLanding);
+  } else if (altitudeState==AltitudeState_t::TakingOff) {
+    EmergencyLand();
+  } else {
+    joy->ErrorNotify();
+  }
 }
 
@@ -552 +553 @@
     Thread::Info("Emergency stop!\n");
     break;
+  case Event_t::EnteringFailSafeMode:
+    Thread::Info("Entering fail safe mode\n");
+    break;
   }
 }
@@ -663 +667 @@
     if (!isSafeModeButtonPressed) {
       isSafeModeButtonPressed = true;
-      Thread::Info("Entering fail safe mode\n");
       EnterFailSafeMode();
     }