diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ba8db32289..42d274979f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1441,12 +1441,15 @@ STATIC_UNIT_TESTED char *blackboxGetStartDateTime(char *buf) } #ifndef BLACKBOX_PRINT_HEADER_LINE -#define BLACKBOX_PRINT_HEADER_LINE(name, format, ...) case __COUNTER__: \ - blackboxPrintfHeaderLine(name, format, __VA_ARGS__); \ - break; -#define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \ - {__VA_ARGS__}; \ - break; +#define BLACKBOX_PRINT_HEADER_LINE(name, format, ...) \ + case __COUNTER__: { \ + blackboxPrintfHeaderLine(name, format, __VA_ARGS__); \ + } break // absence of semicolon on this line is what enforces the presence of a semicolon at the end of each input line + +#define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) \ + case __COUNTER__: { \ + __VA_ARGS__; \ + } break #endif /** @@ -1707,20 +1710,20 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf); #ifndef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", apConfig()->landing_altitude_m); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", apConfig()->hover_throttle); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", apConfig()->throttle_min); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d", apConfig()->throttle_max); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", apConfig()->altitude_P); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", apConfig()->altitude_I); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", apConfig()->altitude_D); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", apConfig()->altitude_F); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_P, "%d", apConfig()->position_P); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d", apConfig()->position_I); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_D, "%d", apConfig()->position_D); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_A, "%d", apConfig()->position_A); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_CUTOFF, "%d", apConfig()->position_cutoff); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d", apConfig()->max_angle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_LANDING_ALTITUDE_M, "%d", autopilotConfig()->landingAltitudeM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_HOVER_THROTTLE, "%d", autopilotConfig()->hoverThrottle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_THROTTLE_MIN, "%d", autopilotConfig()->throttleMin); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_THROTTLE_MAX, "%d", autopilotConfig()->throttleMax); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_ALTITUDE_P, "%d", autopilotConfig()->altitudeP); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_ALTITUDE_I, "%d", autopilotConfig()->altitudeI); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_ALTITUDE_D, "%d", autopilotConfig()->altitudeD); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_ALTITUDE_F, "%d", autopilotConfig()->altitudeF); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_P, "%d", autopilotConfig()->positionP); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_I, "%d", autopilotConfig()->positionI); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_D, "%d", autopilotConfig()->positionD); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_A, "%d", autopilotConfig()->positionA); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_POSITION_CUTOFF, "%d", autopilotConfig()->positionCutoff); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d", autopilotConfig()->maxAngle); #endif // !USE_WING #ifdef USE_MAG @@ -1793,38 +1796,38 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THRUST_LINEARIZATION, "%d", currentPidProfile->thrustLinearization); #ifdef USE_GPS - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed); #ifdef USE_GPS_RESCUE #ifndef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minStartDistM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->initialClimbM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minStartDistM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->initialClimbM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->returnAltitudeM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->groundSpeedCmS) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_IMU_YAW_GAIN, "%d", gpsRescueConfig()->imuYawGain) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->returnAltitudeM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->groundSpeedCmS); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_IMU_YAW_GAIN, "%d", gpsRescueConfig()->imuYawGain); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP); #ifdef USE_MAG - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag); #endif // USE_MAG #endif // !USE_WING #endif // USE_GPS_RESCUE @@ -1832,15 +1835,15 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_ALTITUDE_HOLD #ifndef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, "%d", altHoldConfig()->alt_hold_adjust_rate); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", altHoldConfig()->alt_hold_deadband); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_CLIMB_RATE, "%d", altHoldConfig()->climbRate); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", altHoldConfig()->deadband); #endif // !USE_WING #endif // USE_ALTITUDE_HOLD #ifdef USE_POSITION_HOLD #ifndef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", posHoldConfig()->pos_hold_deadband); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->posHoldWithoutMag); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", posHoldConfig()->deadband); #endif // !USE_WING #endif diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 82df9227dd..cc5ef8571d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1155,15 +1155,15 @@ const clivalue_t valueTable[] = { #ifdef USE_ALTITUDE_HOLD #ifndef USE_WING - { PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) }, - { PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 70 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_deadband) }, + { PARAM_NAME_ALT_HOLD_CLIMB_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, climbRate) }, + { PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 70 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, deadband) }, #endif // !USE_WING #endif // USE_ALTITUDE_HOLD #ifdef USE_POSITION_HOLD #ifndef USE_WING - { PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_without_mag) }, - { PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_deadband) }, + { PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, posHoldWithoutMag) }, + { PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, deadband) }, #endif // !USE_WING #endif // USE_POSITION_HOLD @@ -1918,20 +1918,20 @@ const clivalue_t valueTable[] = { // PG_AUTOPILOT #ifndef USE_WING - { PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, landing_altitude_m) }, - { PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(apConfig_t, hover_throttle) }, - { PARAM_NAME_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(apConfig_t, throttle_min) }, - { PARAM_NAME_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_AUTOPILOT, offsetof(apConfig_t, throttle_max) }, - { PARAM_NAME_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, altitude_P) }, - { PARAM_NAME_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, altitude_I) }, - { PARAM_NAME_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, altitude_D) }, - { PARAM_NAME_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, altitude_F) }, - { PARAM_NAME_POSITION_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_P) }, - { PARAM_NAME_POSITION_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_I) }, - { PARAM_NAME_POSITION_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_D) }, - { PARAM_NAME_POSITION_A, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_A) }, - { PARAM_NAME_POSITION_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_AUTOPILOT, offsetof(apConfig_t, position_cutoff) }, - { PARAM_NAME_AP_MAX_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 70 }, PG_AUTOPILOT, offsetof(apConfig_t, max_angle) }, + { PARAM_NAME_AP_LANDING_ALTITUDE_M, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, landingAltitudeM) }, + { PARAM_NAME_AP_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, hoverThrottle) }, + { PARAM_NAME_AP_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttleMin) }, + { PARAM_NAME_AP_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1400, 2000 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, throttleMax) }, + { PARAM_NAME_AP_ALTITUDE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitudeP) }, + { PARAM_NAME_AP_ALTITUDE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitudeI) }, + { PARAM_NAME_AP_ALTITUDE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitudeD) }, + { PARAM_NAME_AP_ALTITUDE_F, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, altitudeF) }, + { PARAM_NAME_AP_POSITION_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionP) }, + { PARAM_NAME_AP_POSITION_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionI) }, + { PARAM_NAME_AP_POSITION_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionD) }, + { PARAM_NAME_AP_POSITION_A, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionA) }, + { PARAM_NAME_AP_POSITION_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, positionCutoff) }, + { PARAM_NAME_AP_MAX_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 70 }, PG_AUTOPILOT, offsetof(autopilotConfig_t, maxAngle) }, #endif // !USE_WING // PG_MODE_ACTIVATION_CONFIG diff --git a/src/main/cms/cms_menu_gps_rescue_multirotor.c b/src/main/cms/cms_menu_gps_rescue_multirotor.c index a6520d0734..d4826c3d4d 100644 --- a/src/main/cms/cms_menu_gps_rescue_multirotor.c +++ b/src/main/cms/cms_menu_gps_rescue_multirotor.c @@ -55,16 +55,16 @@ static uint8_t gpsRescueConfig_angle; //degrees static uint16_t gpsRescueConfig_descentDistanceM; //meters static uint16_t gpsRescueConfig_descendRate; -static uint8_t apConfig_landingAltitudeM; +static uint8_t autopilotConfig_landingAltitudeM; -static uint16_t apConfig_throttleMin; -static uint16_t apConfig_throttleMax; -static uint16_t apConfig_hoverThrottle; +static uint16_t autopilotConfig_throttleMin; +static uint16_t autopilotConfig_throttleMax; +static uint16_t autopilotConfig_hoverThrottle; static uint8_t gpsRescueConfig_minSats; static uint8_t gpsRescueConfig_allowArmingWithoutFix; -static uint8_t apConfig_altitude_P, apConfig_altitude_I, apConfig_altitude_D, apConfig_altitude_F; +static uint8_t autopilotConfig_altitude_P, autopilotConfig_altitude_I, autopilotConfig_altitude_D, autopilotConfig_altitude_F; static uint8_t gpsRescueConfig_yawP; static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD; @@ -75,10 +75,10 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp) { UNUSED(pDisp); - apConfig_altitude_P = apConfig()->altitude_P; - apConfig_altitude_I = apConfig()->altitude_I; - apConfig_altitude_D = apConfig()->altitude_D; - apConfig_altitude_F = apConfig()->altitude_F; + autopilotConfig_altitude_P = autopilotConfig()->altitudeP; + autopilotConfig_altitude_I = autopilotConfig()->altitudeI; + autopilotConfig_altitude_D = autopilotConfig()->altitudeD; + autopilotConfig_altitude_F = autopilotConfig()->altitudeF; gpsRescueConfig_yawP = gpsRescueConfig()->yawP; @@ -97,10 +97,10 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En UNUSED(pDisp); UNUSED(self); - apConfigMutable()->altitude_P = apConfig_altitude_P; - apConfigMutable()->altitude_I = apConfig_altitude_I; - apConfigMutable()->altitude_D = apConfig_altitude_D; - apConfigMutable()->altitude_F = apConfig_altitude_F; + autopilotConfigMutable()->altitudeP = autopilotConfig_altitude_P; + autopilotConfigMutable()->altitudeI = autopilotConfig_altitude_I; + autopilotConfigMutable()->altitudeD = autopilotConfig_altitude_D; + autopilotConfigMutable()->altitudeF = autopilotConfig_altitude_F; gpsRescueConfigMutable()->yawP = gpsRescueConfig_yawP; @@ -118,10 +118,10 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] = { {"--- GPS RESCUE PID---", OME_Label, NULL, NULL}, - { "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_P, 0, 200, 1 } }, - { "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_I, 0, 200, 1 } }, - { "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_D, 0, 200, 1 } }, - { "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &apConfig_altitude_F, 0, 200, 1 } }, + { "ALTITUDE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_P, 0, 200, 1 } }, + { "ALTITUDE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_I, 0, 200, 1 } }, + { "ALTITUDE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_D, 0, 200, 1 } }, + { "ALTITUDE F", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &autopilotConfig_altitude_F, 0, 200, 1 } }, { "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 200, 1 } }, @@ -162,11 +162,11 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp) gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM; gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate; - apConfig_landingAltitudeM = apConfig()->landing_altitude_m; + autopilotConfig_landingAltitudeM = autopilotConfig()->landingAltitudeM; - apConfig_throttleMin = apConfig()->throttle_min; - apConfig_throttleMax = apConfig()->throttle_max; - apConfig_hoverThrottle = apConfig()->hover_throttle; + autopilotConfig_throttleMin = autopilotConfig()->throttleMin; + autopilotConfig_throttleMax = autopilotConfig()->throttleMax; + autopilotConfig_hoverThrottle = autopilotConfig()->hoverThrottle; gpsRescueConfig_minSats = gpsRescueConfig()->minSats; gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix; @@ -190,11 +190,11 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM; gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate; - apConfigMutable()->landing_altitude_m = apConfig_landingAltitudeM; + autopilotConfigMutable()->landingAltitudeM = autopilotConfig_landingAltitudeM; - apConfigMutable()->throttle_min = apConfig_throttleMin; - apConfigMutable()->throttle_max = apConfig_throttleMax; - apConfigMutable()->hover_throttle = apConfig_hoverThrottle; + autopilotConfigMutable()->throttleMin = autopilotConfig_throttleMin; + autopilotConfigMutable()->throttleMax = autopilotConfig_throttleMax; + autopilotConfigMutable()->hoverThrottle = autopilotConfig_hoverThrottle; gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats; gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix; @@ -217,11 +217,11 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] = { "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 5, 500, 1 } }, { "DESCENT RATE CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } }, - { "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &apConfig_landingAltitudeM, 1, 15, 1 } }, + { "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &autopilotConfig_landingAltitudeM, 1, 15, 1 } }, - { "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_throttleMin, 1050, 1400, 1 } }, - { "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_throttleMax, 1400, 2000, 1 } }, - { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &apConfig_hoverThrottle, 1100, 1700, 1 } }, + { "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMin, 1050, 1400, 1 } }, + { "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_throttleMax, 1400, 2000, 1 } }, + { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &autopilotConfig_hoverThrottle, 1100, 1700, 1 } }, { "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } }, { "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index e677b6fe26..070a516d6e 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -161,20 +161,20 @@ #define PARAM_NAME_ALTITUDE_LPF "altitude_lpf" #define PARAM_NAME_ALTITUDE_D_LPF "altitude_d_lpf" -#define PARAM_NAME_HOVER_THROTTLE "hover_throttle" -#define PARAM_NAME_LANDING_ALTITUDE "landing_altitude_m" -#define PARAM_NAME_THROTTLE_MIN "autopilot_throttle_min" -#define PARAM_NAME_THROTTLE_MAX "autopilot_throttle_max" -#define PARAM_NAME_ALTITUDE_P "autopilot_altitude_P" -#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_POSITION_A "autopilot_position_A" -#define PARAM_NAME_POSITION_CUTOFF "autopilot_position_cutoff" -#define PARAM_NAME_AP_MAX_ANGLE "autopilot_max_angle" +#define PARAM_NAME_AP_LANDING_ALTITUDE_M "ap_landing_altitude_m" +#define PARAM_NAME_AP_HOVER_THROTTLE "ap_hover_throttle" +#define PARAM_NAME_AP_THROTTLE_MIN "ap_throttle_min" +#define PARAM_NAME_AP_THROTTLE_MAX "ap_throttle_max" +#define PARAM_NAME_AP_ALTITUDE_P "ap_altitude_p" +#define PARAM_NAME_AP_ALTITUDE_I "ap_altitude_i" +#define PARAM_NAME_AP_ALTITUDE_D "ap_altitude_d" +#define PARAM_NAME_AP_ALTITUDE_F "ap_altitude_f" +#define PARAM_NAME_AP_POSITION_P "ap_position_p" +#define PARAM_NAME_AP_POSITION_I "ap_position_i" +#define PARAM_NAME_AP_POSITION_D "ap_position_d" +#define PARAM_NAME_AP_POSITION_A "ap_position_a" +#define PARAM_NAME_AP_POSITION_CUTOFF "ap_position_cutoff" +#define PARAM_NAME_AP_MAX_ANGLE "ap_max_angle" #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward" #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms" @@ -259,7 +259,7 @@ #ifdef USE_ALTITUDE_HOLD #define PARAM_NAME_ALT_HOLD_DEADBAND "alt_hold_deadband" -#define PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE "alt_hold_throttle_response" +#define PARAM_NAME_ALT_HOLD_CLIMB_RATE "alt_hold_climb_rate" #endif #ifdef USE_POSITION_HOLD diff --git a/src/main/flight/alt_hold_multirotor.c b/src/main/flight/alt_hold_multirotor.c index 68db59a12c..ff04c70f9a 100644 --- a/src/main/flight/alt_hold_multirotor.c +++ b/src/main/flight/alt_hold_multirotor.c @@ -62,9 +62,9 @@ static void altHoldReset(void) void altHoldInit(void) { altHold.isActive = false; - altHold.deadband = altHoldConfig()->alt_hold_deadband / 100.0f; - altHold.allowStickAdjustment = altHoldConfig()->alt_hold_deadband; - altHold.maxVelocity = altHoldConfig()->alt_hold_adjust_rate * 10.0f; // 50 in CLI means 500cm/s + altHold.deadband = altHoldConfig()->deadband / 100.0f; + altHold.allowStickAdjustment = altHoldConfig()->deadband; + altHold.maxVelocity = altHoldConfig()->climbRate * 10.0f; // 50 in CLI means 500cm/s altHoldReset(); } @@ -81,7 +81,7 @@ static void altHoldProcessTransitions(void) { // ** the transition out of alt hold (exiting altHold) may be rough. Some notes... ** // The original PR had a gradual transition from hold throttle to pilot control throttle - // using !(altHoldRequested && altHold.isAltHoldActive) to run an exit function + // using !(altHoldRequested && altHold.isActive) to run an exit function // a cross-fade factor was sent to mixer.c based on time since the flight mode request was terminated // it was removed primarily to simplify this PR @@ -101,8 +101,8 @@ static void altHoldUpdateTargetAltitude(void) if (altHold.allowStickAdjustment && calculateThrottleStatus() != THROTTLE_LOW) { const float rcThrottle = rcCommand[THROTTLE]; - const float lowThreshold = apConfig()->hover_throttle - altHold.deadband * (apConfig()->hover_throttle - PWM_RANGE_MIN); - const float highThreshold = apConfig()->hover_throttle + altHold.deadband * (PWM_RANGE_MAX - apConfig()->hover_throttle); + const float lowThreshold = autopilotConfig()->hoverThrottle - altHold.deadband * (autopilotConfig()->hoverThrottle - PWM_RANGE_MIN); + const float highThreshold = autopilotConfig()->hoverThrottle + altHold.deadband * (PWM_RANGE_MAX - autopilotConfig()->hoverThrottle); if (rcThrottle < lowThreshold) { stickFactor = scaleRangef(rcThrottle, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f); @@ -136,7 +136,7 @@ static void altHoldUpdateTargetAltitude(void) static void altHoldUpdate(void) { // check if the user has changed the target altitude using sticks - if (altHoldConfig()->alt_hold_adjust_rate) { + if (altHoldConfig()->climbRate) { altHoldUpdateTargetAltitude(); } altitudeControl(altHold.targetAltitudeCm, taskIntervalSeconds, altHold.targetVelocity); diff --git a/src/main/flight/autopilot_multirotor.c b/src/main/flight/autopilot_multirotor.c index 00eff1555b..cd42049226 100644 --- a/src/main/flight/autopilot_multirotor.c +++ b/src/main/flight/autopilot_multirotor.c @@ -138,23 +138,22 @@ void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned t void autopilotInit(void) { - const apConfig_t *cfg = apConfig(); - + const autopilotConfig_t *cfg = autopilotConfig(); ap.sticksActive = false; - ap.maxAngle = cfg->max_angle; - altitudePidCoeffs.Kp = cfg->altitude_P * ALTITUDE_P_SCALE; - altitudePidCoeffs.Ki = cfg->altitude_I * ALTITUDE_I_SCALE; - altitudePidCoeffs.Kd = cfg->altitude_D * ALTITUDE_D_SCALE; - altitudePidCoeffs.Kf = cfg->altitude_F * ALTITUDE_F_SCALE; - positionPidCoeffs.Kp = cfg->position_P * POSITION_P_SCALE; - positionPidCoeffs.Ki = cfg->position_I * POSITION_I_SCALE; - positionPidCoeffs.Kd = cfg->position_D * POSITION_D_SCALE; - positionPidCoeffs.Kf = cfg->position_A * POSITION_A_SCALE; // Kf used for acceleration + ap.maxAngle = cfg->maxAngle; + altitudePidCoeffs.Kp = cfg->altitudeP * ALTITUDE_P_SCALE; + altitudePidCoeffs.Ki = cfg->altitudeI * ALTITUDE_I_SCALE; + altitudePidCoeffs.Kd = cfg->altitudeD * ALTITUDE_D_SCALE; + altitudePidCoeffs.Kf = cfg->altitudeF * ALTITUDE_F_SCALE; + positionPidCoeffs.Kp = cfg->positionP * POSITION_P_SCALE; + positionPidCoeffs.Ki = cfg->positionI * POSITION_I_SCALE; + positionPidCoeffs.Kd = cfg->positionD * POSITION_D_SCALE; + positionPidCoeffs.Kf = cfg->positionA * POSITION_A_SCALE; // Kf used for acceleration // initialise filters with approximate filter gains; location isn't used at this point. ap.upsampleLpfGain = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate at init resetUpsampleFilters(); // Initialise PT1 filters for velocity and acceleration in earth frame axes - ap.vaLpfCutoff = cfg->position_cutoff * 0.01f; + ap.vaLpfCutoff = cfg->positionCutoff * 0.01f; const float vaGain = pt1FilterGain(ap.vaLpfCutoff, 0.1f); // assume 10Hz GPS connection at start; value is overwritten before first filter use for (unsigned i = 0; i < ARRAYLEN(ap.efAxis); i++) { resetEFAxisFilters(&ap.efAxis[i], vaGain); @@ -193,7 +192,7 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAl const float altitudeF = targetAltitudeStep * altitudePidCoeffs.Kf; - const float hoverOffset = apConfig()->hover_throttle - PWM_RANGE_MIN; + const float hoverOffset = autopilotConfig()->hoverThrottle - PWM_RANGE_MIN; float throttleOffset = altitudeP + altitudeI - altitudeD + altitudeF + hoverOffset; const float tiltMultiplier = 1.0f / fmaxf(getCosTiltAngle(), 0.5f); @@ -203,7 +202,7 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAl throttleOffset *= tiltMultiplier; float newThrottle = PWM_RANGE_MIN + throttleOffset; - newThrottle = constrainf(newThrottle, apConfig()->throttle_min, apConfig()->throttle_max); + newThrottle = constrainf(newThrottle, autopilotConfig()->throttleMin, autopilotConfig()->throttleMax); DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 0, lrintf(newThrottle)); // normal range 1000-2000 but is before constraint newThrottle = scaleRangef(newThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f); @@ -386,7 +385,7 @@ bool positionControl(void) bool isBelowLandingAltitude(void) { - return getAltitudeCm() < 100.0f * apConfig()->landing_altitude_m; + return getAltitudeCm() < 100.0f * autopilotConfig()->landingAltitudeM; } float getAutopilotThrottle(void) diff --git a/src/main/flight/gps_rescue_multirotor.c b/src/main/flight/gps_rescue_multirotor.c index cfb3b83feb..2321629ad0 100644 --- a/src/main/flight/gps_rescue_multirotor.c +++ b/src/main/flight/gps_rescue_multirotor.c @@ -630,7 +630,7 @@ static void descend(bool newGpsData) { if (newGpsData) { // consider landing area to be a circle half landing height around home, to avoid overshooting home point - const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * apConfig()->landing_altitude_m); + const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * autopilotConfig()->landingAltitudeM); const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); // increase the velocity lowpass filter cutoff for more aggressive responses when descending, especially close to home diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index dbc9b08da0..38dfdee169 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -586,7 +586,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_ angleLimit = 85.0f; // allow autopilot to use whatever angle it needs to stop } // limit pilot requested angle to half the autopilot angle to avoid excess speed and chaotic stops - angleLimit = fminf(0.5f * apConfig()->max_angle, angleLimit); + angleLimit = fminf(0.5f * autopilotConfig()->maxAngle, angleLimit); } #endif diff --git a/src/main/flight/pos_hold_multirotor.c b/src/main/flight/pos_hold_multirotor.c index de4147a2df..ba977d8589 100644 --- a/src/main/flight/pos_hold_multirotor.c +++ b/src/main/flight/pos_hold_multirotor.c @@ -51,8 +51,8 @@ static posHoldState_t posHold; void posHoldInit(void) { - posHold.deadband = posHoldConfig()->pos_hold_deadband * 0.01f; - posHold.useStickAdjustment = posHoldConfig()->pos_hold_deadband; + posHold.deadband = posHoldConfig()->deadband * 0.01f; + posHold.useStickAdjustment = posHoldConfig()->deadband; } static void posHoldCheckSticks(void) @@ -73,7 +73,7 @@ static bool sensorsOk(void) #ifdef USE_MAG !compassIsHealthy() && #endif - (!posHoldConfig()->pos_hold_without_mag || !canUseGPSHeading)) { + (!posHoldConfig()->posHoldWithoutMag || !canUseGPSHeading)) { return false; } return true; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 0d39f90533..025df2fc7b 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1530,9 +1530,9 @@ case MSP_NAME: sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM); sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM); sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS); - sbufWriteU16(dst, apConfig()->throttle_min); - sbufWriteU16(dst, apConfig()->throttle_max); - sbufWriteU16(dst, apConfig()->hover_throttle); + sbufWriteU16(dst, autopilotConfig()->throttleMin); + sbufWriteU16(dst, autopilotConfig()->throttleMax); + sbufWriteU16(dst, autopilotConfig()->hoverThrottle); sbufWriteU8(dst, gpsRescueConfig()->sanityChecks); sbufWriteU8(dst, gpsRescueConfig()->minSats); @@ -1548,9 +1548,9 @@ case MSP_NAME: break; case MSP_GPS_RESCUE_PIDS: - sbufWriteU16(dst, apConfig()->altitude_P); - sbufWriteU16(dst, apConfig()->altitude_I); - sbufWriteU16(dst, apConfig()->altitude_D); + sbufWriteU16(dst, autopilotConfig()->altitudeP); + sbufWriteU16(dst, autopilotConfig()->altitudeI); + sbufWriteU16(dst, autopilotConfig()->altitudeD); // altitude_F not implemented yet sbufWriteU16(dst, gpsRescueConfig()->velP); sbufWriteU16(dst, gpsRescueConfig()->velI); @@ -1797,7 +1797,7 @@ case MSP_NAME: sbufWriteU8(dst, rcControlsConfig()->deadband); sbufWriteU8(dst, rcControlsConfig()->yaw_deadband); #if defined(USE_POSITION_HOLD) && !defined(USE_WING) - sbufWriteU8(dst, posHoldConfig()->pos_hold_deadband); + sbufWriteU8(dst, posHoldConfig()->deadband); #else sbufWriteU8(dst, 0); #endif @@ -2901,9 +2901,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src); gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src); gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src); - apConfigMutable()->throttle_min = sbufReadU16(src); - apConfigMutable()->throttle_max = sbufReadU16(src); - apConfigMutable()->hover_throttle = sbufReadU16(src); + autopilotConfigMutable()->throttleMin = sbufReadU16(src); + autopilotConfigMutable()->throttleMax = sbufReadU16(src); + autopilotConfigMutable()->hoverThrottle = sbufReadU16(src); gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src); gpsRescueConfigMutable()->minSats = sbufReadU8(src); if (sbufBytesRemaining(src) >= 6) { @@ -2924,9 +2924,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, break; case MSP_SET_GPS_RESCUE_PIDS: - apConfigMutable()->altitude_P = sbufReadU16(src); - apConfigMutable()->altitude_I = sbufReadU16(src); - apConfigMutable()->altitude_D = sbufReadU16(src); + autopilotConfigMutable()->altitudeP = sbufReadU16(src); + autopilotConfigMutable()->altitudeI = sbufReadU16(src); + autopilotConfigMutable()->altitudeD = sbufReadU16(src); // altitude_F not included in msp yet gpsRescueConfigMutable()->velP = sbufReadU16(src); gpsRescueConfigMutable()->velI = sbufReadU16(src); @@ -2990,7 +2990,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, rcControlsConfigMutable()->deadband = sbufReadU8(src); rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); #if defined(USE_POSITION_HOLD) && !defined(USE_WING) - posHoldConfigMutable()->pos_hold_deadband = sbufReadU8(src); + posHoldConfigMutable()->deadband = sbufReadU8(src); #else sbufReadU8(src); #endif diff --git a/src/main/pg/alt_hold_multirotor.c b/src/main/pg/alt_hold_multirotor.c index c229fcf66a..5c29527219 100644 --- a/src/main/pg/alt_hold_multirotor.c +++ b/src/main/pg/alt_hold_multirotor.c @@ -35,8 +35,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 4); PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, - .alt_hold_adjust_rate = 50, // max vertical velocity change at full/zero throttle. 50 means 5 m/s - .alt_hold_deadband = 20, // throttle deadband in percent of stick travel + .climbRate = 50, // max vertical velocity change at full/zero throttle. 50 means 5 m/s + .deadband = 20, // throttle deadband in percent of stick travel ); #endif diff --git a/src/main/pg/alt_hold_multirotor.h b/src/main/pg/alt_hold_multirotor.h index 88f0933b0c..2310328fe0 100644 --- a/src/main/pg/alt_hold_multirotor.h +++ b/src/main/pg/alt_hold_multirotor.h @@ -28,8 +28,8 @@ #include "pg/pg.h" typedef struct altHoldConfig_s { - uint8_t alt_hold_adjust_rate; - uint8_t alt_hold_deadband; + uint8_t climbRate; + uint8_t deadband; } altHoldConfig_t; PG_DECLARE(altHoldConfig_t, altHoldConfig); diff --git a/src/main/pg/autopilot_multirotor.c b/src/main/pg/autopilot_multirotor.c index 898cfdc205..7854c33c4b 100644 --- a/src/main/pg/autopilot_multirotor.c +++ b/src/main/pg/autopilot_multirotor.c @@ -30,23 +30,23 @@ #include "autopilot.h" -PG_REGISTER_WITH_RESET_TEMPLATE(apConfig_t, apConfig, PG_AUTOPILOT, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 2); -PG_RESET_TEMPLATE(apConfig_t, apConfig, - .landing_altitude_m = 4, - .hover_throttle = 1275, - .throttle_min = 1100, - .throttle_max = 1700, - .altitude_P = 15, - .altitude_I = 15, - .altitude_D = 15, - .altitude_F = 15, - .position_P = 30, - .position_I = 30, - .position_D = 30, - .position_A = 30, - .position_cutoff = 80, - .max_angle = 50, +PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, + .landingAltitudeM = 4, + .hoverThrottle = 1275, + .throttleMin = 1100, + .throttleMax = 1700, + .altitudeP = 15, + .altitudeI = 15, + .altitudeD = 15, + .altitudeF = 15, + .positionP = 30, + .positionI = 30, + .positionD = 30, + .positionA = 30, + .positionCutoff = 80, + .maxAngle = 50, ); #endif // !USE_WING diff --git a/src/main/pg/autopilot_multirotor.h b/src/main/pg/autopilot_multirotor.h index 6e1f6cfecd..978c9a0c92 100644 --- a/src/main/pg/autopilot_multirotor.h +++ b/src/main/pg/autopilot_multirotor.h @@ -27,23 +27,23 @@ #include "pg/pg.h" -typedef struct apConfig_s { - uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres - uint16_t hover_throttle; // value used at the start of a rescue or position hold - uint16_t throttle_min; - uint16_t throttle_max; - uint8_t altitude_P; - uint8_t altitude_I; - uint8_t altitude_D; - uint8_t altitude_F; - uint8_t position_P; - uint8_t position_I; - uint8_t position_D; - uint8_t position_A; - uint8_t position_cutoff; - uint8_t max_angle; -} apConfig_t; +typedef struct autopilotConfig_s { + uint8_t landingAltitudeM; // altitude below which landing behaviours can change, metres + uint16_t hoverThrottle; // value used at the start of a rescue or position hold + uint16_t throttleMin; + uint16_t throttleMax; + uint8_t altitudeP; + uint8_t altitudeI; + uint8_t altitudeD; + uint8_t altitudeF; + uint8_t positionP; + uint8_t positionI; + uint8_t positionD; + uint8_t positionA; + uint8_t positionCutoff; + uint8_t maxAngle; +} autopilotConfig_t; -PG_DECLARE(apConfig_t, apConfig); +PG_DECLARE(autopilotConfig_t, autopilotConfig); #endif // !USE_WING diff --git a/src/main/pg/autopilot_wing.c b/src/main/pg/autopilot_wing.c index 06dc476c1c..73a351f45f 100644 --- a/src/main/pg/autopilot_wing.c +++ b/src/main/pg/autopilot_wing.c @@ -30,9 +30,9 @@ #include "autopilot.h" -PG_REGISTER_WITH_RESET_TEMPLATE(apConfig_t, apConfig, PG_AUTOPILOT, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 2); -PG_RESET_TEMPLATE(apConfig_t, apConfig, +PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, ); #endif // USE_WING diff --git a/src/main/pg/autopilot_wing.h b/src/main/pg/autopilot_wing.h index ee01e0a6f9..567db81cc3 100644 --- a/src/main/pg/autopilot_wing.h +++ b/src/main/pg/autopilot_wing.h @@ -27,10 +27,10 @@ #include "pg/pg.h" -typedef struct apConfig_s { +typedef struct autopilotConfig_s { uint8_t dummy; -} apConfig_t; +} autopilotConfig_t; -PG_DECLARE(apConfig_t, apConfig); +PG_DECLARE(autopilotConfig_t, autopilotConfig); #endif // USE_WING diff --git a/src/main/pg/pos_hold_multirotor.c b/src/main/pg/pos_hold_multirotor.c index 2a7a332241..68e907165f 100644 --- a/src/main/pg/pos_hold_multirotor.c +++ b/src/main/pg/pos_hold_multirotor.c @@ -35,8 +35,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 0); PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, - .pos_hold_without_mag = false, // position hold within this percentage stick deflection - .pos_hold_deadband = 5, // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks + .posHoldWithoutMag = false, // position hold within this percentage stick deflection + .deadband = 5, // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks ); #endif diff --git a/src/main/pg/pos_hold_multirotor.h b/src/main/pg/pos_hold_multirotor.h index 6187d9ba62..64f8f26716 100644 --- a/src/main/pg/pos_hold_multirotor.h +++ b/src/main/pg/pos_hold_multirotor.h @@ -28,8 +28,8 @@ #include "pg/pg.h" typedef struct posHoldConfig_s { - bool pos_hold_without_mag; - uint8_t pos_hold_deadband; + bool posHoldWithoutMag; + uint8_t deadband; } posHoldConfig_t; PG_DECLARE(posHoldConfig_t, posHoldConfig); diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc index 4dbba143bb..98e1d93abc 100644 --- a/src/test/unit/althold_unittest.cc +++ b/src/test/unit/althold_unittest.cc @@ -50,7 +50,7 @@ extern "C" { PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); PG_REGISTER(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 0); - PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0); + PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0); PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0); PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 82b0df4ac1..8b92be036f 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -79,7 +79,7 @@ extern "C" { PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0); - PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0); + PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0); float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint16_t averageSystemLoadPercent = 0; diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index b8c1dda675..af839a3266 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -75,7 +75,7 @@ extern "C" { PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); - PG_REGISTER(apConfig_t, apConfig, PG_AUTOPILOT, 0); + PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = 0