diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index df7a622e36..c7560c5f6e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1763,6 +1763,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_MAX_VOLTAGE, "%d", currentPidProfile->tpa_speed_max_voltage); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_PITCH_OFFSET, "%d", currentPidProfile->tpa_speed_pitch_offset); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_TYPE, "%d", currentPidProfile->yaw_type); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_PITCH_OFFSET, "%d", currentPidProfile->angle_pitch_offset); #endif // USE_WING default: diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 7a96425b68..cfff706cb7 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1322,6 +1322,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_SPA_YAW_WIDTH, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_width[FD_YAW]) }, { PARAM_NAME_SPA_YAW_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_mode[FD_YAW]) }, { PARAM_NAME_YAW_TYPE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_YAW_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_type) }, + { PARAM_NAME_ANGLE_PITCH_OFFSET, VAR_INT16 | PROFILE_VALUE, .config.minmaxUnsigned = { -ANGLE_PITCH_OFFSET_MAX, ANGLE_PITCH_OFFSET_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_pitch_offset) }, #endif // PG_TELEMETRY_CONFIG diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index bb99e94a89..04fdad6f0a 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -177,6 +177,7 @@ #define PARAM_NAME_S_YAW "s_yaw" #define PARAM_NAME_ANGLE_P_GAIN "angle_p_gain" #define PARAM_NAME_ANGLE_EARTH_REF "angle_earth_ref" +#define PARAM_NAME_ANGLE_PITCH_OFFSET "angle_pitch_offset" #define PARAM_NAME_HORIZON_LEVEL_STRENGTH "horizon_level_strength" #define PARAM_NAME_HORIZON_LIMIT_DEGREES "horizon_limit_degrees" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 90494e4c99..6b5e0ed78e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -252,6 +252,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .tpa_speed_max_voltage = 2520, .tpa_speed_pitch_offset = 0, .yaw_type = YAW_TYPE_RUDDER, + .angle_pitch_offset = 0, ); } @@ -496,6 +497,12 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_ float angleTarget = angleLimit * currentPidSetpoint * maxSetpointRateInv; // use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate +#ifdef USE_WING + if (axis == FD_PITCH) { + angleTarget += (float)pidProfile->angle_pitch_offset / 10.0f; + } +#endif // USE_WING + #ifdef USE_GPS_RESCUE angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7ed76af28a..67367d4cb2 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -72,6 +72,7 @@ #define TPA_MAX 100 #ifdef USE_WING +#define ANGLE_PITCH_OFFSET_MAX 450 #define TPA_LOW_RATE_MIN INT8_MIN #define TPA_GRAVITY_MAX 5000 #define TPA_CURVE_STALL_THROTTLE_MAX 100 @@ -316,6 +317,7 @@ typedef struct pidProfile_s { uint16_t tpa_speed_max_voltage; // For wings: theoretical max voltage; used for throttle scailing with voltage for air speed estimation int16_t tpa_speed_pitch_offset; // For wings: pitch offset in degrees*10 for craft speed estimation uint8_t yaw_type; // For wings: type of yaw (rudder or differential thrust) + int16_t angle_pitch_offset; // For wings: pitch offset for angle modes; in decidegrees; positive values tilting the wing down } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);