Changeset 398 in flair-src for trunk/lib/FlairMeta/src

Timestamp:

Mar 4, 2021, 5:00:53 PM (4 years ago)

Author:

Sanahuja Guillaume

Message:

resolve bug with custom altitude

File:

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

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -293 +293 @@
   }
   // landing, only check if we reach desired landing altitude
+  //no break in this case, continue with Stabilized state
   case AltitudeState_t::StartLanding: {
     if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
@@ -318 +319 @@
       joy->SetZRef(0);
     }
+    break;
   }
   }
@@ -342 +344 @@
                "switching back to safe mode\n");
   EnterFailSafeMode();
+  GetDefaultReferenceAltitude(refAltitude,refVerticalVelocity);
 };
 
 void UavStateMachine::ComputeThrust(void) {
-  if (altitudeMode == AltitudeMode_t::Manual) {
+  if (thrustMode == ThrustMode_t::Default) {
     currentThrust = ComputeDefaultThrust();
   } else {
@@ -509 +512 @@
 
 void UavStateMachine::Land(void) {
+  if (thrustMode != ThrustMode_t::Default) {
+    SetThrustMode(ThrustMode_t::Default);
+  }
   if (altitudeMode != AltitudeMode_t::Manual) {
     SetAltitudeMode(AltitudeMode_t::Manual);