Altitude hold

lib/Espfc/src/Blackbox/Blackbox.cpp

@@ -255,7 +255,7 @@ void FAST_CODE_ATTR Blackbox::updateData()
       mag.magADC[i] = _model.state.mag.adc[i] * 1090;
     }
     if(_model.baroActive()) {
-      baro.altitude = lrintf(_model.state.baro.altitude * 100.f); // cm
+      baro.altitude = lrintf(_model.state.baro.altitudeGround * 100.f); // cm
     }
   }
   rcCommand[AXIS_THRUST] = _model.state.input.buffer[AXIS_THRUST];
@@ -324,6 +324,7 @@ void FAST_CODE_ATTR Blackbox::updateMode()
   updateModeFlag(&rcModeActivationMask, BOXARM, _model.isSwitchActive(MODE_ARMED));
   updateModeFlag(&rcModeActivationMask, BOXANGLE, _model.isSwitchActive(MODE_ANGLE));
   updateModeFlag(&rcModeActivationMask, BOXAIRMODE, _model.isSwitchActive(MODE_AIRMODE));
+  updateModeFlag(&rcModeActivationMask, BOXANTIGRAVITY, _model.isSwitchActive(MODE_ALTHOLD));
   updateModeFlag(&rcModeActivationMask, BOXFAILSAFE, _model.isSwitchActive(MODE_FAILSAFE));
   updateModeFlag(&rcModeActivationMask, BOXBLACKBOX, _model.isSwitchActive(MODE_BLACKBOX));
   updateModeFlag(&rcModeActivationMask, BOXBLACKBOXERASE, _model.isSwitchActive(MODE_BLACKBOX_ERASE));

lib/Espfc/src/Connect/Cli.cpp

@@ -559,12 +559,20 @@ const Cli::Param * Cli::initialize(ModelConfig& c)
     Param(PSTR("pid_level_p"), &c.pid[FC_PID_LEVEL].P),
     Param(PSTR("pid_level_i"), &c.pid[FC_PID_LEVEL].I),
     Param(PSTR("pid_level_d"), &c.pid[FC_PID_LEVEL].D),
+    Param(PSTR("pid_level_f"), &c.pid[FC_PID_LEVEL].F),
 
     Param(PSTR("pid_level_angle_limit"), &c.level.angleLimit),
     Param(PSTR("pid_level_rate_limit"), &c.level.rateLimit),
     Param(PSTR("pid_level_lpf_type"), &c.level.ptermFilter.type, filterTypeChoices),
     Param(PSTR("pid_level_lpf_freq"), &c.level.ptermFilter.freq),
 
+    Param(PSTR("pid_althold_vel_p"), &c.pid[FC_PID_VEL].P),
+    Param(PSTR("pid_althold_vel_i"), &c.pid[FC_PID_VEL].I),
+    Param(PSTR("pid_althold_vel_d"), &c.pid[FC_PID_VEL].D),
+    Param(PSTR("pid_althold_vel_f"), &c.pid[FC_PID_VEL].F),
+    Param(PSTR("pid_althold_iterm_center"), &c.altHold.itermCenter),
+    Param(PSTR("pid_althold_iterm_range"), &c.altHold.itermRange),
+
     Param(PSTR("pid_yaw_lpf_type"), &c.yaw.filter.type, filterTypeChoices),
     Param(PSTR("pid_yaw_lpf_freq"), &c.yaw.filter.freq),
 
@@ -1380,13 +1388,15 @@ void Cli::execute(CliCmd& cmd, Stream& s)
     else if(strcmp_P(cmd.args[1], PSTR("erase")) == 0)
     {
       flashfsEraseCompletely();
+      s.println("OK");
     }
     else if(strcmp_P(cmd.args[1], PSTR("test")) == 0)
     {
       const char * data = "flashfs-test";
       flashfsWrite((const uint8_t*)data, strlen(data), true);
       flashfsFlushAsync(true);
       flashfsClose();
+      s.println("OK");
     }
     else if(strcmp_P(cmd.args[1], PSTR("print")) == 0)
     {

lib/Espfc/src/Connect/MspProcessor.cpp

@@ -275,13 +275,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);
@@ -684,8 +685,8 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
       break;
 
     case MSP_ALTITUDE:
-      r.writeU32(lrintf(_model.state.baro.altitude * 100.f));    // alt [cm]
-      r.writeU16(0); // vario
+      r.writeU32(lrintf(_model.state.altitude.height * 100.f));  // alt [cm]
+      r.writeU16(lrintf(_model.state.altitude.vario * 100.f));   // vario [cm/s]
       break;
 
     case MSP_BEEPER_CONFIG:
@@ -1462,7 +1463,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;
@@ -1491,7 +1497,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
       break;
 
     case MSP_DEBUG:
-      for (int i = 0; i < 4; i++) {
+      for (int i = 0; i < 8; i++) {
         r.writeU16(_model.state.debug[i]);
       }
       break;

lib/Espfc/src/Control/Actuator.cpp

@@ -157,6 +157,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;
   }

lib/Espfc/src/Control/Altitude.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "Model.h"
+#include "Utils/Filter.h"
+
+namespace Espfc::Control {
+
+class Altitude
+{
+public:
+  Altitude(Model& model): _model(model) {}
+
+  int begin()
+  {
+    _model.state.altitude.height = 0.0f;
+    _model.state.altitude.vario = 0.0f;
+
+    _altitudeFilter.begin(FilterConfig(FILTER_PT2, 5), _model.state.accel.timer.rate);
+    _varioFilter.begin(FilterConfig(FILTER_PT2, 5), _model.state.accel.timer.rate);
+
+    return 1;
+  }
+
+  int update()
+  {
+    _model.state.altitude.height = _altitudeFilter.update(_model.state.baro.altitudeGround);
+    _model.state.altitude.vario = _varioFilter.update(_model.state.baro.vario);
+
+    if(_model.config.debug.mode == DEBUG_ALTITUDE)
+    {
+      _model.state.debug[0] = std::clamp(lrintf(_model.state.baro.altitudeGround * 100.0f), -32000l, 32000l);  // gps trust
+      _model.state.debug[1] = std::clamp(lrintf(_model.state.baro.vario * 100.0f), -32000l, 32000l);           // baroAlt cm
+      _model.state.debug[2] = std::clamp(lrintf(_model.state.altitude.height * 100.0f), -32000l, 32000l);      // gpsAlt cm
+      _model.state.debug[3] = std::clamp(lrintf(_model.state.altitude.vario * 100.0f), -32000l, 32000l);       // vario
+    }
+
+    return 1;
+  }
+
+private:
+  Model& _model;
+  Utils::Filter _altitudeFilter;
+  Utils::Filter _varioFilter;
+};
+
+}