Add OSD items to display nav PIDs outputs

src/main/cms/cms_menu_osd.c

@@ -204,6 +204,12 @@ static const OSD_Entry menuOsdElemsEntries[] =
  OSD_ELEMENT_ENTRY("ROLL PIDS", OSD_ROLL_PIDS),
  OSD_ELEMENT_ENTRY("PITCH PIDS", OSD_PITCH_PIDS),
  OSD_ELEMENT_ENTRY("YAW PIDS", OSD_YAW_PIDS),
+ OSD_ELEMENT_ENTRY("FW ALT PID OUT", OSD_FW_ALT_PID_OUTPUTS),
+ OSD_ELEMENT_ENTRY("FW POS PID OUT", OSD_FW_POS_PID_OUTPUTS),
+ OSD_ELEMENT_ENTRY("MC VELX PID OUT", OSD_MC_VEL_X_PID_OUTPUTS),
+ OSD_ELEMENT_ENTRY("MC VELY PID OUT", OSD_MC_VEL_Y_PID_OUTPUTS),
+ OSD_ELEMENT_ENTRY("MC VELZ PID OUT", OSD_MC_VEL_Z_PID_OUTPUTS),
+ OSD_ELEMENT_ENTRY("MC POS PID OUT", OSD_MC_POS_XYZ_P_OUTPUTS),
 
  OSD_ELEMENT_ENTRY("ATTI PITCH", OSD_ATTITUDE_PITCH),
  OSD_ELEMENT_ENTRY("ATTI ROLL", OSD_ATTITUDE_ROLL),

src/main/io/osd.c

@@ -1017,6 +1017,20 @@ static void osdDrawRadar(uint16_t *drawn)
 
 #endif
 
+void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController) {
+ strcpy(buff, label);
+ uint8_t label_len = strlen(label);
+ for (uint8_t i = label_len; i < 5; ++i) buff[i] = ' ';
+ osdFormatCentiNumber(buff + 5, pidController->proportional, 0, 1, 0, 4);
+ buff[9] = ' ';
+ osdFormatCentiNumber(buff + 10, pidController->integrator, 0, 1, 0, 4);
+ buff[14] = ' ';
+ osdFormatCentiNumber(buff + 15, pidController->derivative, 0, 1, 0, 4);
+ buff[19] = ' ';
+ osdFormatCentiNumber(buff + 20, pidController->output_constrained, 0, 1, 0, 4);
+ buff[24] = '\0';
+}
+
 static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t decimals)
 {
  char buff[6];
@@ -1170,6 +1184,7 @@ static bool osdDrawSingleElement(uint8_t item)
  buff[5] = '\0';
  break;
  }
+
  case OSD_GPS_HDOP:
  {
  buff[0] = SYM_HDP_L;
@@ -1670,6 +1685,54 @@ static bool osdDrawSingleElement(uint8_t item)
  return true;
  }
 
+ case OSD_FW_ALT_PID_OUTPUTS:
+ {
+ const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+ osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt);
+ break;
+ }
+
+ case OSD_FW_POS_PID_OUTPUTS:
+ {
+ const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+ osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav);
+ break;
+ }
+
+ case OSD_MC_VEL_Z_PID_OUTPUTS:
+ {
+ const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+ osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z]);
+ break;
+ }
+
+ case OSD_MC_VEL_X_PID_OUTPUTS:
+ {
+ const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+ osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X]);
+ break;
+ }
+
+ case OSD_MC_VEL_Y_PID_OUTPUTS:
+ {
+ const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+ osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y]);
+ break;
+ }
+
+ case OSD_MC_POS_XYZ_P_OUTPUTS:
+ {
+ const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+ strcpy(buff, "POSO ");
+ osdFormatCentiNumber(buff + 5, nav_pids->pos[X].output_constrained, 0, 1, 0, 4);
+ buff[9] = ' ';
+ osdFormatCentiNumber(buff + 10, nav_pids->pos[Y].output_constrained, 0, 1, 0, 4);
+ buff[14] = ' ';
+ osdFormatCentiNumber(buff + 15, nav_pids->pos[Z].output_constrained, 0, 1, 0, 4);
+ buff[19] = '\0';
+ break;
+ }
+
  case OSD_POWER:
  {
  buff[0] = SYM_WATT;
@@ -2081,6 +2144,13 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
  osdConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10);
  osdConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11);
  osdConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12);
+ osdConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12);
+ osdConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12);
+ osdConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12);
+ osdConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12);
+ osdConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12);
+ osdConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12);
+
  osdConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1);
 
  osdConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5);

src/main/io/osd.h

@@ -87,6 +87,12 @@ typedef enum {
  OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE,
  OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,
  OSD_REMAINING_DISTANCE_BEFORE_RTH,
+ OSD_FW_ALT_PID_OUTPUTS,
+ OSD_FW_POS_PID_OUTPUTS,
+ OSD_MC_VEL_X_PID_OUTPUTS,
+ OSD_MC_VEL_Y_PID_OUTPUTS,
+ OSD_MC_VEL_Z_PID_OUTPUTS,
+ OSD_MC_POS_XYZ_P_OUTPUTS,
  OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/navigation/navigation.c

@@ -1611,6 +1611,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
  const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative;
  const float outValConstrained = constrainf(outVal, outMin, outMax);
 
+ pid->proportional = newProportional;
+ pid->integral = pid->integrator;
+ pid->derivative = newDerivative;
+ pid->output_constrained = outValConstrained;
+
  /* Update I-term */
  if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
  const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
@@ -1638,10 +1643,13 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
 void navPidReset(pidController_t *pid)
 {
  pid->reset = true;
+ pid->proportional = 0.0f;
  pid->integrator = 0.0f;
+ pid->derivative = 0.0f;
  pid->last_input = 0.0f;
  pid->dterm_filter_state.state = 0.0f;
  pid->dterm_filter_state.RC = 0.0f;
+ pid->output_constrained = 0.0f;
 }
 
 void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD)
@@ -3059,6 +3067,10 @@ float calculateAverageSpeed() {
  return (float)getTotalTravelDistance() / (getFlightTime() * 100);
 }
 
+const navigationPIDControllers_t* getNavigationPIDControllers(void) {
+ return &posControl.pids;
+}
+
 #else // NAV
 
 #ifdef USE_GPS

src/main/navigation/navigation.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "common/axis.h"
+#include "common/filter.h"
 #include "common/maths.h"
 #include "common/vector.h"
 
@@ -207,6 +209,46 @@ typedef struct {
  int32_t yaw; // deg * 100
 } navWaypointPosition_t;
 
+typedef struct {
+ float kP;
+ float kI;
+ float kD;
+ float kT; // Tracking gain (anti-windup)
+} pidControllerParam_t;
+
+typedef struct {
+ float kP;
+} pControllerParam_t;
+
+typedef struct {
+ bool reset;
+ pidControllerParam_t param;
+ pt1Filter_t dterm_filter_state; // last derivative for low-pass filter
+ float integrator; // integrator value
+ float last_input; // last input for derivative
+
+ float integral; // used integral value in output
+ float proportional; // used proportional value in output
+ float derivative; // used derivative value in output
+ float output_constrained; // controller output constrained
+} pidController_t;
+
+typedef struct {
+ pControllerParam_t param;
+ float output_constrained;
+} pController_t;
+
+typedef struct navigationPIDControllers_s {
+ /* Multicopter PIDs */
+ pController_t pos[XYZ_AXIS_COUNT];
+ pidController_t vel[XYZ_AXIS_COUNT];
+ pidController_t surface;
+
+ /* Fixed-wing PIDs */
+ pidController_t fw_alt;
+ pidController_t fw_nav;
+} navigationPIDControllers_t;
+
 /* MultiWii-compatible params for telemetry */
 typedef enum {
  MW_GPS_MODE_NONE = 0,
@@ -346,6 +388,8 @@ bool isFixedWingLaunchDetected(void);
 
 float calculateAverageSpeed();
 
+const navigationPIDControllers_t* getNavigationPIDControllers(void);
+
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles
  * are in the [0, 360 * 100) interval.

src/main/navigation/navigation_multicopter.c

@@ -72,6 +72,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
  targetVel = constrainf(targetVel, -navConfig()->general.max_auto_climb_rate, navConfig()->general.max_auto_climb_rate);
  }
 
+ posControl.pids.pos[Z].output_constrained = targetVel;
+
  // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
  // if we are decelerating - don't limit (allow better recovery from falling)
  if (ABS(targetVel) > ABS(posControl.desiredState.vel.z)) {
@@ -352,6 +354,9 @@ static void updatePositionVelocityController_MC(void)
  newVelTotal = maxSpeed;
  }
 
+ posControl.pids.pos[X].output_constrained = newVelX;
+ posControl.pids.pos[Y].output_constrained = newVelY;
+
  // Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
  const float velHeadFactor = getVelocityHeadingAttenuationFactor();
  const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);

src/main/navigation/navigation_private.h

@@ -92,46 +92,12 @@ typedef struct navigationFlags_s {
  bool forcedRTHActivated;
 } navigationFlags_t;
 
-typedef struct {
- float kP;
- float kI;
- float kD;
- float kT; // Tracking gain (anti-windup)
-} pidControllerParam_t;
-
-typedef struct {
- float kP;
-} pControllerParam_t;
-
 typedef enum {
  PID_DTERM_FROM_ERROR = 1 << 0,
  PID_ZERO_INTEGRATOR = 1 << 1,
  PID_SHRINK_INTEGRATOR = 1 << 2,
 } pidControllerFlags_e;
 
-typedef struct {
- bool reset;
- pidControllerParam_t param;
- pt1Filter_t dterm_filter_state; // last derivative for low-pass filter
- float integrator; // integrator value
- float last_input; // last input for derivative
-} pidController_t;
-
-typedef struct {
- pControllerParam_t param;
-} pController_t;
-
-typedef struct navigationPIDControllers_s {
- /* Multicopter PIDs */
- pController_t pos[XYZ_AXIS_COUNT];
- pidController_t vel[XYZ_AXIS_COUNT];
- pidController_t surface;
-
- /* Fixed-wing PIDs */
- pidController_t fw_alt;
- pidController_t fw_nav;
-} navigationPIDControllers_t;
-
 typedef struct {
  fpVector3_t pos;
  fpVector3_t vel;