althold init

rtlopez · rtlopez · commit af7db953b896 · 2025-02-21T00:07:50.000+01:00
diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp
@@ -271,13 +271,14 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
       break;
 
     case MSP_BOXNAMES:
-      r.writeString(F("ARM;ANGLE;AIRMODE;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;"));
+      r.writeString(F("ARM;AIRMODE;ANGLE;ALTHOLD;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;"));
       break;
 
     case MSP_BOXIDS:
       r.writeU8(MODE_ARMED);
-      r.writeU8(MODE_ANGLE);
       r.writeU8(MODE_AIRMODE);
+      r.writeU8(MODE_ANGLE);
+      r.writeU8(MODE_ALTHOLD);
       r.writeU8(MODE_BUZZER);
       r.writeU8(MODE_FAILSAFE);
       r.writeU8(MODE_BLACKBOX);
@@ -1419,7 +1420,12 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
           disableRunawayTakeoff = m.readU8();
         }
         (void)disableRunawayTakeoff;
-        _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd);
+#if defined(ESPFC_DEV_PRESET_UNSAFE_ARMING)
+        (void)cmd;
+#warning "Danger macro used ESPFC_DEV_PRESET_UNSAFE_ARMING"
+#else
+      _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd);
+#endif
         if (_model.isModeActive(MODE_ARMED)) _model.disarm(DISARM_REASON_ARMING_DISABLED);
       }
       break;
diff --git a/lib/Espfc/src/Control/Actuator.cpp b/lib/Espfc/src/Control/Actuator.cpp
@@ -154,6 +154,8 @@ bool Actuator::canActivateMode(FlightMode mode)
       return _model.accelActive();
     case MODE_AIRMODE:
       return _model.state.mode.airmodeAllowed;
+    case MODE_ALTHOLD:
+      return _model.state.baro.dev;
     default:
       return true;
   }
diff --git a/lib/Espfc/src/Control/Altitude.hpp b/lib/Espfc/src/Control/Altitude.hpp
@@ -0,0 +1,35 @@
+#pragma once
+
+#include "Model.h"
+
+namespace Espfc::Control {
+
+class Altitude
+{
+public:
+  Altitude(Model& model): _model(model) {}
+
+  int begin()
+  {
+    _model.state.altitude.height = 0.0f;
+    _model.state.altitude.heightPrev = 0.0f;
+    _model.state.altitude.rate = 0.0f;
+
+    return 1;
+  }
+
+  int update()
+  {
+    // initialy use baro altitude only, in feature mix with other sources according to trust level
+    _model.state.altitude.height = _model.state.altitude.height + _model.state.baro.altitude;
+    _model.state.altitude.rate = (_model.state.altitude.height - _model.state.altitude.heightPrev) * _model.state.loopTimer.rate;
+    _model.state.altitude.heightPrev = _model.state.altitude.height;
+
+    return 1;
+  }
+
+private:
+  Model& _model;
+};
+
+}
diff --git a/lib/Espfc/src/Control/Controller.cpp b/lib/Espfc/src/Control/Controller.cpp
@@ -5,12 +5,13 @@ namespace Espfc {
 
 namespace Control {
 
-Controller::Controller(Model& model): _model(model) {}
+Controller::Controller(Model& model): _model(model), _altitude(model) {}
 
 int Controller::begin()
 {
   _rates.begin(_model.config.input);
   _speedFilter.begin(FilterConfig(FILTER_BIQUAD, 10), _model.state.loopTimer.rate);
+  _altitude.begin();
   return 1;
 }
 
@@ -25,26 +26,31 @@ int FAST_CODE_ATTR Controller::update()
 
   {
     Utils::Stats::Measure(_model.state.stats, COUNTER_OUTER_PID);
+    _altitude.update();
     resetIterm();
-    if(_model.config.mixer.type == FC_MIXER_GIMBAL)
+    switch(_model.config.mixer.type)
     {
-      outerLoopRobot();
-    }
-    else
-    {
-      outerLoop();
+      case FC_MIXER_GIMBAL:
+        outerLoopRobot();
+        break;
+
+      default:
+        outerLoop();
+        break;
     }
   }
 
   {
     Utils::Stats::Measure(_model.state.stats, COUNTER_INNER_PID);
-    if(_model.config.mixer.type == FC_MIXER_GIMBAL)
-    {
-      innerLoopRobot();
-    }
-    else
+    switch(_model.config.mixer.type)
     {
-      innerLoop();
+      case FC_MIXER_GIMBAL:
+        innerLoopRobot();
+        break;
+
+      default:
+        innerLoop();
+        break;
     }
   }
 
@@ -110,27 +116,44 @@ void Controller::innerLoopRobot()
 
 void FAST_CODE_ATTR Controller::outerLoop()
 {
+  // Roll/Pitch rates control
   if(_model.isModeActive(MODE_ANGLE))
   {
-    _model.state.setpoint.angle = VectorFloat(
-      _model.state.input.ch[AXIS_ROLL] * Utils::toRad(_model.config.level.angleLimit),
-      _model.state.input.ch[AXIS_PITCH] * Utils::toRad(_model.config.level.angleLimit),
-      _model.state.attitude.euler[AXIS_YAW]
-    );
-    _model.state.setpoint.rate[AXIS_ROLL]  = _model.state.outerPid[AXIS_ROLL].update(_model.state.setpoint.angle[AXIS_ROLL], _model.state.attitude.euler[AXIS_ROLL]);
-    _model.state.setpoint.rate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.setpoint.angle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]);
-    // disable fterm in angle mode
-    _model.state.innerPid[AXIS_ROLL].fScale = 0.f;
-    _model.state.innerPid[AXIS_PITCH].fScale = 0.f;
+    for(size_t i = 0; i < AXIS_COUNT_RP; i++)
+    {
+      const float angleSetpoint = Utils::toRad(_model.config.level.angleLimit) * _model.state.input.ch[i];
+      _model.state.setpoint.rate[i] = _model.state.outerPid[i].update(angleSetpoint, _model.state.attitude.euler[i]);
+      // disable fterm in angle mode
+      _model.state.innerPid[i].fScale = 0.f;
+    }
   }
   else
   {
-    _model.state.setpoint.rate[AXIS_ROLL] = calculateSetpointRate(AXIS_ROLL, _model.state.input.ch[AXIS_ROLL]);
-    _model.state.setpoint.rate[AXIS_PITCH] = calculateSetpointRate(AXIS_PITCH, _model.state.input.ch[AXIS_PITCH]);
+    for(size_t i = 0; i < AXIS_COUNT_RP; i++)
+    {
+      _model.state.setpoint.rate[i] = calculateSetpointRate(i, _model.state.input.ch[i]);
+    }
   }
+
+  // Yaw rates control
   _model.state.setpoint.rate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input.ch[AXIS_YAW]);
-  _model.state.setpoint.rate[AXIS_THRUST] = _model.state.input.ch[AXIS_THRUST];
 
+  // thrust control
+  if(_model.isModeActive(MODE_ALTHOLD))
+  {
+    _model.state.setpoint.rate[AXIS_THRUST] = calcualteAltHoldSetpoint();
+    if(_model.config.debug.mode == DEBUG_ALTITUDE)
+    {
+      _model.state.debug[0] = lrintf(_model.state.input.ch[AXIS_THRUST] * 1000.0f);
+      _model.state.debug[1] = lrintf(_model.state.setpoint.rate[AXIS_THRUST] * 1000.0f);
+    }
+  }
+  else
+  {
+    _model.state.setpoint.rate[AXIS_THRUST] = _model.state.input.ch[AXIS_THRUST];
+  }
+
+  // debug
   if(_model.config.debug.mode == DEBUG_ANGLERATE)
   {
     for(size_t i = 0; i < AXIS_COUNT_RPY; ++i)
@@ -142,14 +165,29 @@ void FAST_CODE_ATTR Controller::outerLoop()
 
 void FAST_CODE_ATTR Controller::innerLoop()
 {
+  // Roll/Pitch/Yaw rates control
   const float tpaFactor = getTpaFactor();
   for(size_t i = 0; i < AXIS_COUNT_RPY; ++i)
   {
     _model.state.output.ch[i] = _model.state.innerPid[i].update(_model.state.setpoint.rate[i], _model.state.gyro.adc[i]) * tpaFactor;
-    //_model.state.debug[i] = lrintf(_model.state.innerPid[i].fTerm * 1000);
   }
-  _model.state.output.ch[AXIS_THRUST] = _model.state.setpoint.rate[AXIS_THRUST];
 
+  // thrust control
+  if(_model.isModeActive(MODE_ALTHOLD))
+  {
+    _model.state.output.ch[AXIS_THRUST] = _model.state.innerPid[AXIS_THRUST].update(_model.state.setpoint.rate[AXIS_THRUST], _model.state.altitude.rate);
+    if(_model.config.debug.mode == DEBUG_ALTITUDE)
+    {
+      _model.state.debug[2] = lrintf(_model.state.output.ch[AXIS_THRUST] * 1000.0f);
+      _model.state.debug[3] = lrintf(_model.state.innerPid[AXIS_THRUST].iTerm * 1000.0f);
+    }
+  }
+  else
+  {
+    _model.state.output.ch[AXIS_THRUST] = _model.state.setpoint.rate[AXIS_THRUST];
+  }
+
+  // debug
   if(_model.config.debug.mode == DEBUG_ITERM_RELAX)
   {
     _model.state.debug[0] = lrintf(Utils::toDeg(_model.state.innerPid[0].itermRelaxBase));
@@ -159,6 +197,17 @@ void FAST_CODE_ATTR Controller::innerLoop()
   }
 }
 
+float Controller::calcualteAltHoldSetpoint() const
+{
+  float thrust = _model.state.input.ch[AXIS_THRUST];
+
+  if(_model.isThrottleLow()) thrust = 0.0f; // stick below min check, no command
+
+  thrust = Utils::deadband(thrust, 0.125f); // +/- 12.5% deadband
+
+  return thrust * 0.5f; // climb/descend rate factor 0.5 m/s
+}
+
 float Controller::getTpaFactor() const
 {
   if(_model.config.controller.tpaScale == 0) return 1.f;
@@ -172,15 +221,19 @@ void Controller::resetIterm()
     || (!_model.isAirModeActive() && _model.config.iterm.lowThrottleZeroIterm && _model.isThrottleLow()) // on low throttle (not in air mode)
   )
   {
-    for(size_t i = 0; i < AXIS_COUNT_RPYT; i++)
+    for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
     {
-      _model.state.innerPid[i].iTerm = 0;
-      _model.state.outerPid[i].iTerm = 0;
+      _model.state.innerPid[i].iTerm = 0.0f;
+      _model.state.outerPid[i].iTerm = 0.0f;
     }
   }
+  if(!_model.isModeActive(MODE_ARMED))
+  {
+    _model.state.innerPid[AXIS_THRUST].iTerm = 0.0f;
+  }
 }
 
-float Controller::calculateSetpointRate(int axis, float input)
+float Controller::calculateSetpointRate(int axis, float input) const
 {
   if(axis == AXIS_YAW) input *= -1.f;
   return _rates.getSetpoint(axis, input);
diff --git a/lib/Espfc/src/Control/Controller.h b/lib/Espfc/src/Control/Controller.h
@@ -2,6 +2,7 @@
 
 #include "Model.h"
 #include "Control/Rates.h"
+#include "Control/Altitude.hpp"
 
 namespace Espfc {
 
@@ -21,10 +22,12 @@ class Controller
 
     inline float getTpaFactor() const;
     inline void resetIterm();
-    float calculateSetpointRate(int axis, float input);
+    float calculateSetpointRate(int axis, float input) const;
+    float calcualteAltHoldSetpoint() const;
 
   private:
     Model& _model;
+    Altitude _altitude;
     Rates _rates;
     Utils::Filter _speedFilter;
 };
diff --git a/lib/Espfc/src/Control/Fusion.cpp b/lib/Espfc/src/Control/Fusion.cpp
@@ -72,7 +72,7 @@ int FAST_CODE_ATTR Fusion::update()
     }
     //else madgwickFusion1();
 
-    if(_model.config.debug.mode == DEBUG_ALTITUDE)
+    if(_model.config.debug.mode == DEBUG_AC_CORRECTION)
     {
       _model.state.debug[0] = lrintf(Utils::toDeg(_model.state.attitude.euler[0]) * 10);
       _model.state.debug[1] = lrintf(Utils::toDeg(_model.state.attitude.euler[1]) * 10);
diff --git a/lib/Espfc/src/Control/Pid.h b/lib/Espfc/src/Control/Pid.h
@@ -22,6 +22,10 @@ constexpr float LEVEL_ITERM_SCALE = 0.1f;    // 1/10
 constexpr float LEVEL_DTERM_SCALE = 0.001f;  // 1/1000
 constexpr float LEVEL_FTERM_SCALE = 0.001f;  // 1/1000
 
+constexpr float VEL_PTERM_SCALE = 0.02f;
+constexpr float VEL_ITERM_SCALE = 0.01f;
+constexpr float VEL_DTERM_SCALE = 0.001f;
+
 enum ItermRelaxType {
   ITERM_RELAX_OFF,
   ITERM_RELAX_RP,
diff --git a/lib/Espfc/src/Control/Rates.cpp b/lib/Espfc/src/Control/Rates.cpp
@@ -1,9 +1,7 @@
 #include "Rates.h"
 #include "Utils/MemoryHelper.h"
 
-namespace Espfc {
-
-namespace Control {
+namespace Espfc::Control {
 
 constexpr float SETPOINT_RATE_LIMIT = 1998.0f;
 constexpr float RC_RATE_INCREMENTAL = 14.54f;
@@ -113,5 +111,3 @@ float FAST_CODE_ATTR Rates::quick(const int axis, float rcCommandf, const float
 }
 
 }
-
-}
diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h
@@ -155,6 +155,7 @@ class Model
     {
 #if defined(ESPFC_DEV_PRESET_UNSAFE_ARMING)
       return false;
+#warning "Danger macro used ESPFC_DEV_PRESET_UNSAFE_ARMING"
 #else
       return state.mode.armingDisabledFlags != 0;
 #endif
@@ -549,6 +550,17 @@ class Model
       }
       state.customMixer = MixerConfig(config.customMixerCount, config.customMixes);
 
+      PidConfig& pcav = config.pid[FC_PID_VEL];
+      Control::Pid& pidav = state.innerPid[AXIS_THRUST];
+      pidav.Kp = (float)pcav.P * VEL_PTERM_SCALE;
+      pidav.Ki = (float)pcav.I * VEL_ITERM_SCALE;
+      pidav.Kd = (float)pcav.D * VEL_DTERM_SCALE;
+      pidav.Kf = 0.0f;
+      pidav.iLimit = 0.6f;
+      pidav.oLimit = 1.0f;
+      pidav.rate = state.loopTimer.rate;
+      pidav.begin();
+
       // override temporary
       //state.telemetryTimer.setRate(100);
     }
diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h
@@ -81,8 +81,9 @@ struct FusionConfig
 
 enum FlightMode {
   MODE_ARMED,
-  MODE_ANGLE,
   MODE_AIRMODE,
+  MODE_ANGLE,
+  MODE_ALTHOLD,
   MODE_BUZZER,
   MODE_FAILSAFE,
   MODE_BLACKBOX,
@@ -411,8 +412,8 @@ struct InputConfig
   int8_t ppmMode = PPM_MODE_NORMAL;
   uint8_t serialRxProvider = SERIALRX_SBUS;
 
-  int16_t maxCheck = 1050;
-  int16_t minCheck = 1900;
+  int16_t minCheck = 1050;
+  int16_t maxCheck = 1900;
   int16_t minRc = 885;
   int16_t midRc = 1500;
   int16_t maxRc = 2115;
diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
@@ -320,6 +320,13 @@ struct ModeState
   bool airmodeAllowed;
 };
 
+struct AltitudeState
+{
+  float height;
+  float heightPrev;
+  float rate;
+};
+
 // runtime data
 struct ModelState
 {
@@ -334,6 +341,8 @@ struct ModelState
   AttitudeState attitude;
   RotationMatrixFloat boardAlignment;
 
+  AltitudeState altitude;
+
   SetpointState setpoint;
   Control::Pid innerPid[AXIS_COUNT_RPYT];
   Control::Pid outerPid[AXIS_COUNT_RPYT];

Original file line number	Diff line number	Diff line change
`@@ -154,6 +154,8 @@ bool Actuator::canActivateMode(FlightMode mode)`
`154`	`154`	`return _model.accelActive();`
`155`	`155`	`case MODE_AIRMODE:`
`156`	`156`	`return _model.state.mode.airmodeAllowed;`
	`157`	`+ case MODE_ALTHOLD:`
	`158`	`+ return _model.state.baro.dev;`
`157`	`159`	`default:`
`158`	`160`	`return true;`
`159`	`161`	`}`
Original file line number	Diff line number	Diff line change
`@@ -72,7 +72,7 @@ int FAST_CODE_ATTR Fusion::update()`
`72`	`72`	`}`
`73`	`73`	`//else madgwickFusion1();`
`74`	`74`
`75`		`- if(_model.config.debug.mode == DEBUG_ALTITUDE)`
	`75`	`+ if(_model.config.debug.mode == DEBUG_AC_CORRECTION)`
`76`	`76`	`{`
`77`	`77`	`_model.state.debug[0] = lrintf(Utils::toDeg(_model.state.attitude.euler[0]) * 10);`
`78`	`78`	`_model.state.debug[1] = lrintf(Utils::toDeg(_model.state.attitude.euler[1]) * 10);`
Original file line number	Diff line number	Diff line change
`@@ -1,9 +1,7 @@`
`1`	`1`	`#include "Rates.h"`
`2`	`2`	`#include "Utils/MemoryHelper.h"`
`3`	`3`
`4`		`-namespace Espfc {`
`5`		`-`
`6`		`-namespace Control {`
	`4`	`+namespace Espfc::Control {`
`7`	`5`
`8`	`6`	`constexpr float SETPOINT_RATE_LIMIT = 1998.0f;`
`9`	`7`	`constexpr float RC_RATE_INCREMENTAL = 14.54f;`
`@@ -113,5 +111,3 @@ float FAST_CODE_ATTR Rates::quick(const int axis, float rcCommandf, const float`
`113`	`111`	`}`
`114`	`112`
`115`	`113`	`}`
`116`		`-`
`117`		`-}`