diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index cb1a833fa3..7d93093dab 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1694,18 +1694,22 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", autopilotConfig()->landing_altitude_m); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", autopilotConfig()->hover_throttle); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", autopilotConfig()->throttle_min); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d", autopilotConfig()->throttle_max); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", autopilotConfig()->altitude_P); - 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_LANDING_ALTITUDE, "%d", autopilotConfig()->landing_altitude_m); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", autopilotConfig()->hover_throttle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", autopilotConfig()->throttle_min); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d", autopilotConfig()->throttle_max); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", autopilotConfig()->altitude_P); + 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); diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 1016d7d986..e5c2a6358d 100644 --- a/src/main/build/debug.c +++ b/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", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 7645d505cb..91369c48a0 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 395cf874e1..ff23833bdc 100644 --- a/src/main/cli/settings.c +++ b/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) diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 04fdad6f0a..0bfa50daa6 100644 --- a/src/main/fc/parameter_names.h +++ b/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" diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index b6887878c1..e6f693f67a 100644 --- a/src/main/flight/autopilot.c +++ b/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; diff --git a/src/main/flight/autopilot.h b/src/main/flight/autopilot.h index b07d8e75f6..7d352b18dc 100644 --- a/src/main/flight/autopilot.h +++ b/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); diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index d7681b7ac2..6295871cb2 100644 --- a/src/main/flight/pos_hold.c +++ b/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; } }