add debug values for testing

src/main/blackbox/blackbox.c

@@ -1702,10 +1702,14 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d",   autopilotConfig()->altitude_I);;
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d",   autopilotConfig()->altitude_D);;
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d",   autopilotConfig()->altitude_F);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_P, "%d",   autopilotConfig()->position_P);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d",   autopilotConfig()->position_I);;
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_D, "%d",   autopilotConfig()->position_D);;
 
 #ifdef USE_MAG
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d",           compassConfig()->mag_hardware);
 #endif
+
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, "%d",  armingConfig()->gyro_cal_on_first_arm);
         BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d",       rxConfig()->airModeActivateThreshold);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SERIAL_RX_PROVIDER, "%d",     rxConfig()->serialrx_provider);

src/main/build/debug.c

@@ -123,4 +123,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     [DEBUG_GIMBAL] = "GIMBAL",
     [DEBUG_WING_SETPOINT] = "WING_SETPOINT",
     [DEBUG_POS_HOLD] = "POSITION_HOLD",
+    [DEBUG_AUTOPILOT_POSITION] = "AUTOPILOT_POSITION",
 };

src/main/build/debug.h

@@ -125,6 +125,7 @@ typedef enum {
     DEBUG_GIMBAL,
     DEBUG_WING_SETPOINT,
     DEBUG_POS_HOLD,
+    DEBUG_AUTOPILOT_POSITION,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/settings.c

@@ -1857,6 +1857,9 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_ALTITUDE_I,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_I) },
     { PARAM_NAME_ALTITUDE_D,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_D) },
     { PARAM_NAME_ALTITUDE_F,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, altitude_F) },
+    { PARAM_NAME_POSITION_P,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, position_P) },
+    { PARAM_NAME_POSITION_I,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, position_I) },
+    { PARAM_NAME_POSITION_D,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, position_D) },
 
 // PG_MODE_ACTIVATION_CONFIG
 #if defined(USE_CUSTOM_BOX_NAMES)

src/main/fc/parameter_names.h

@@ -168,6 +168,9 @@
 #define PARAM_NAME_ALTITUDE_I "autopilot_altitude_I"
 #define PARAM_NAME_ALTITUDE_D "autopilot_altitude_D"
 #define PARAM_NAME_ALTITUDE_F "autopilot_altitude_F"
+#define PARAM_NAME_POSITION_P "autopilot_position_P"
+#define PARAM_NAME_POSITION_I "autopilot_position_I"
+#define PARAM_NAME_POSITION_D "autopilot_position_D"
 
 #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
 #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"

src/main/flight/autopilot.c

@@ -152,26 +152,33 @@ void positionControl(gpsLocation_t targetLocation) {
         previousVelocity = velocity;
         // ** THIS WILL NEED A FILTER LIKE FOR GPS RESCUE**
 
-        // use a PID function to control velocity to target
-        // P proportional to velocity, positive means moving away from target
-        // I or integral of velocity =  distance away from target
-        // D is acceleration of position error, or away from target
-        // F would be a value proportional to target change (tricky, so not done yet)
-
-        float velocityP = velocity * positionPidCoeffs.Kp; // position D
-        float velocityI = distanceCm * positionPidCoeffs.Ki; // position P
-        float velocityD = acceleration * positionPidCoeffs.Kd; // position D
+        float velocityP = velocity * positionPidCoeffs.Kp; // proportional to velocity
+        float velocityI = distanceCm * positionPidCoeffs.Ki; // proportional to distance error
+        float velocityD = acceleration * positionPidCoeffs.Kd; // proportional to acceleration *needs filtering*
+        // F would be a value proportional to target change (not done yet)
 
         float pidSum = velocityP + velocityI + velocityD; // greater when position error is bad and getting worse
 
+        // value sent to 
         rollSetpoint = rollCorrection * pidSum; // with some gain adjustment to give reasonable number
         pitchSetpoint = pitchCorrection * pidSum;
 
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(normalisedErrorAngle));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(rollCorrection));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pitchCorrection));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(distanceCm));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(velocityP));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(velocityI));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(velocityD));
+
         newGPSData = false;
     }
     // send setpoints to pid.c using the same method as for gpsRescueAngle
-    posHoldAngle[AI_PITCH] = pitchSetpoint;
+    // value sent should be in degrees * 100 (why??)
     posHoldAngle[AI_ROLL] = rollSetpoint;
+    posHoldAngle[AI_PITCH] = pitchSetpoint;
+
+    DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(rollSetpoint));
 
     // but for now let's not really do that until we get the PIDs sorted out :-)
     posHoldAngle[AI_PITCH] = 0.0f;

src/main/flight/autopilot.h

@@ -25,6 +25,7 @@ extern float posHoldAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREE
 
 void autopilotInit(const autopilotConfig_t *config);
 void resetAltitudeControl(void);
+void resetPositionControl(void);
 
 void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
 void positionControl(gpsLocation_t targetLocation);

src/main/flight/pos_hold.c

@@ -37,7 +37,7 @@ posHoldState_t posHoldState;
 
 void posHoldReset(void)
 {
-//    resetPositionControl(); - need to add this in position_control.c
+    resetPositionControl();
     posHoldState.targetLocation = gpsSol.llh;
     posHoldState.targetAdjustRate = 0.0f;
 }
@@ -53,9 +53,11 @@ void posHoldProcessTransitions(void) {
     if (FLIGHT_MODE(POS_HOLD_MODE)) {
         if (!posHoldState.isPosHoldActive) {
             posHoldReset();
+            DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, 22);
             posHoldState.isPosHoldActive = true;
         }
     } else {
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, -22);
         posHoldState.isPosHoldActive = false;
     }
 }