From 6c9b5227fc9237b906193a8ad38ecbe4c851757d Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Mon, 18 Jun 2018 20:25:15 +0200 Subject: [PATCH] Add adjustments and corresponding OSD items --- src/main/cms/cms_menu_osd.c | 22 +++ src/main/fc/rc_adjustments.c | 166 ++++++++++++++++++++++ src/main/fc/rc_adjustments.h | 19 ++- src/main/flight/mixer.h | 2 + src/main/io/osd.c | 266 +++++++++++++++++++++++++++-------- src/main/io/osd.h | 22 +++ 6 files changed, 439 insertions(+), 58 deletions(-) diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index e62e17d605a..ae10ed83b84 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -201,9 +201,31 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("MAP TAKE OFF", OSD_MAP_TAKEOFF), OSD_ELEMENT_ENTRY("RADAR", OSD_RADAR), #endif + OSD_ELEMENT_ENTRY("EXPO", OSD_RC_EXPO), + OSD_ELEMENT_ENTRY("YAW EXPO", OSD_RC_YAW_EXPO), + OSD_ELEMENT_ENTRY("THR EXPO", OSD_THROTTLE_EXPO), + OSD_ELEMENT_ENTRY("ROLL RATE", OSD_ROLL_RATE), + OSD_ELEMENT_ENTRY("PITCH RATE", OSD_PITCH_RATE), + OSD_ELEMENT_ENTRY("YAW RATE", OSD_YAW_RATE), + OSD_ELEMENT_ENTRY("M EXPO", OSD_MANUAL_RC_EXPO), + OSD_ELEMENT_ENTRY("M YAW EXPO", OSD_MANUAL_RC_YAW_EXPO), + OSD_ELEMENT_ENTRY("M ROLL RATE", OSD_MANUAL_ROLL_RATE), + OSD_ELEMENT_ENTRY("M PITCH RATE", OSD_MANUAL_PITCH_RATE), + OSD_ELEMENT_ENTRY("M YAW RATE", OSD_MANUAL_YAW_RATE), + OSD_ELEMENT_ENTRY("CRUISE THR", OSD_NAV_FW_CRUISE_THR), + OSD_ELEMENT_ENTRY("PITCH TO THR", OSD_NAV_FW_PITCH2THR), 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("LEVEL PIDS", OSD_LEVEL_PIDS), + OSD_ELEMENT_ENTRY("POSXY PIDS", OSD_POS_XY_PIDS), + OSD_ELEMENT_ENTRY("POSZ PIDS", OSD_POS_Z_PIDS), + OSD_ELEMENT_ENTRY("VELXY PIDS", OSD_VEL_XY_PIDS), + OSD_ELEMENT_ENTRY("VELZ PIDS", OSD_VEL_Z_PIDS), + OSD_ELEMENT_ENTRY("HEAD P", OSD_HEADING_P), + OSD_ELEMENT_ENTRY("ALIGN ROLL", OSD_BOARD_ALIGN_ROLL), + OSD_ELEMENT_ENTRY("ALIGN PITCH", OSD_BOARD_ALIGN_PITCH), + OSD_ELEMENT_ENTRY("0THR PITCH", OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE), OSD_ELEMENT_ENTRY("ATTI PITCH", OSD_ATTITUDE_PITCH), OSD_ELEMENT_ENTRY("ATTI ROLL", OSD_ATTITUDE_ROLL), diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index dbb1e308829..70870cd890b 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -41,6 +41,7 @@ #include "navigation/navigation.h" +#include "flight/mixer.h" #include "flight/pid.h" #include "io/beeper.h" @@ -184,6 +185,74 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_PITCH_BOARD_ALIGNMENT, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 5 }} + }, { + .adjustmentFunction = ADJUSTMENT_LEVEL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_LEVEL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_LEVEL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_XY_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_XY_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_XY_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_Z_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_Z_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_Z_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_HEADING_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_XY_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_XY_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_XY_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_Z_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_Z_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_Z_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 5 }} #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT }, { .adjustmentFunction = ADJUSTMENT_PROFILE, @@ -414,6 +483,103 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t updateBoardAlignment(0, delta); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_BOARD_ALIGNMENT, boardAlignment()->pitchDeciDegrees); break; + case ADJUSTMENT_LEVEL_P: + newValue = constrain((int)pidBank()->pid[PID_LEVEL].P + delta, 0, 200); + pidBankMutable()->pid[PID_LEVEL].P = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_LEVEL_P, newValue); + break; + case ADJUSTMENT_LEVEL_I: + newValue = constrain((int)pidBank()->pid[PID_LEVEL].I + delta, 0, 200); + pidBankMutable()->pid[PID_LEVEL].I = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_LEVEL_I, newValue); + break; + case ADJUSTMENT_LEVEL_D: + newValue = constrain((int)pidBank()->pid[PID_LEVEL].D + delta, 0, 200); + pidBankMutable()->pid[PID_LEVEL].D = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_LEVEL_D, newValue); + break; + case ADJUSTMENT_POS_XY_P: + newValue = constrain((int)pidBank()->pid[PID_POS_XY].P + delta, 0, 200); + pidBankMutable()->pid[PID_POS_XY].P = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_POS_XY_P, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_XY_I: + newValue = constrain((int)pidBank()->pid[PID_POS_XY].I + delta, 0, 200); + pidBankMutable()->pid[PID_POS_XY].I = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_POS_XY_I, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_XY_D: + newValue = constrain((int)pidBank()->pid[PID_POS_XY].D + delta, 0, 200); + pidBankMutable()->pid[PID_POS_XY].D = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_POS_XY_D, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_Z_P: + newValue = constrain((int)pidBank()->pid[PID_POS_Z].P + delta, 0, 200); + pidBankMutable()->pid[PID_POS_Z].P = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_POS_Z_P, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_Z_I: + newValue = constrain((int)pidBank()->pid[PID_POS_Z].I + delta, 0, 200); + pidBankMutable()->pid[PID_POS_Z].I = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_POS_Z_I, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_Z_D: + newValue = constrain((int)pidBank()->pid[PID_POS_Z].D + delta, 0, 200); + pidBankMutable()->pid[PID_POS_Z].D = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_POS_Z_D, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_HEADING_P: + newValue = constrain((int)pidBank()->pid[PID_HEADING].P + delta, 0, 200); + pidBankMutable()->pid[PID_HEADING].P = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HEADING_P, newValue); + break; + case ADJUSTMENT_VEL_XY_P: + newValue = constrain((int)pidBank()->pid[PID_VEL_XY].P + delta, 0, 200); + pidBankMutable()->pid[PID_VEL_XY].P = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_VEL_XY_P, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_XY_I: + newValue = constrain((int)pidBank()->pid[PID_VEL_XY].I + delta, 0, 200); + pidBankMutable()->pid[PID_VEL_XY].I = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_VEL_XY_I, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_XY_D: + newValue = constrain((int)pidBank()->pid[PID_VEL_XY].D + delta, 0, 200); + pidBankMutable()->pid[PID_VEL_XY].D = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_VEL_XY_D, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_Z_P: + newValue = constrain((int)pidBank()->pid[PID_VEL_Z].P + delta, 0, 200); + pidBankMutable()->pid[PID_VEL_Z].P = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_VEL_Z_P, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_Z_I: + newValue = constrain((int)pidBank()->pid[PID_VEL_Z].I + delta, 0, 200); + pidBankMutable()->pid[PID_VEL_Z].I = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_VEL_Z_I, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_Z_D: + newValue = constrain((int)pidBank()->pid[PID_VEL_Z].D + delta, 0, 200); + pidBankMutable()->pid[PID_VEL_Z].D = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_VEL_Z_D, newValue); + navigationUsePIDs(); + break; + case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: + newValue = constrain((int)mixerConfig()->fwMinThrottleDownPitchAngle + delta, 0, FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX); + mixerConfigMutable()->fwMinThrottleDownPitchAngle = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, newValue); + break; default: break; }; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 1de4b46764c..4849b010016 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -57,8 +57,25 @@ typedef enum { ADJUSTMENT_NAV_FW_PITCH2THR = 29, ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 30, ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 31, + ADJUSTMENT_LEVEL_P = 32, + ADJUSTMENT_LEVEL_I = 33, + ADJUSTMENT_LEVEL_D = 34, + ADJUSTMENT_POS_XY_P = 35, + ADJUSTMENT_POS_XY_I = 36, + ADJUSTMENT_POS_XY_D = 37, + ADJUSTMENT_POS_Z_P = 38, + ADJUSTMENT_POS_Z_I = 39, + ADJUSTMENT_POS_Z_D = 40, + ADJUSTMENT_HEADING_P = 41, + ADJUSTMENT_VEL_XY_P = 42, + ADJUSTMENT_VEL_XY_I = 43, + ADJUSTMENT_VEL_XY_D = 44, + ADJUSTMENT_VEL_Z_P = 45, + ADJUSTMENT_VEL_Z_I = 46, + ADJUSTMENT_VEL_Z_D = 47, + ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48, #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - ADJUSTMENT_PROFILE = 32, + ADJUSTMENT_PROFILE = 49, #endif ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index f14943f8a84..7e2d46c8aea 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -28,6 +28,8 @@ #define YAW_JUMP_PREVENTION_LIMIT_LOW 80 #define YAW_JUMP_PREVENTION_LIMIT_HIGH 500 +#define FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX 450 + typedef enum { PLATFORM_MULTIROTOR = 0, PLATFORM_AIRPLANE = 1, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e8b5e5f20a7..9bb4047842d 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -64,6 +64,7 @@ #include "fc/fc_core.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" @@ -71,6 +72,7 @@ #include "fc/fc_tasks.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "flight/pid.h" #include "flight/rth_estimator.h" #include "flight/wind_estimator.h" @@ -80,6 +82,7 @@ #include "rx/rx.h" #include "sensors/battery.h" +#include "sensors/boardalignment.h" #include "sensors/diagnostics.h" #include "sensors/sensors.h" #include "sensors/pitotmeter.h" @@ -1035,6 +1038,43 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_ displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); } +static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, const pid8_t *pid, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD) { + textAttributes_t elemAttr; + char buff[3]; + + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->P); + if (isAdjustmentFunctionSelected(adjFuncP)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->I); + if (isAdjustmentFunctionSelected(adjFuncI)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->D); + if (isAdjustmentFunctionSelected(adjFuncD)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); +} + +static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, const char *str, const uint8_t valueOffset, const float value, const uint8_t valueLength, const uint8_t maxDecimals, adjustmentFunction_e adjFunc) { + char buff[8]; + textAttributes_t elemAttr; + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + + elemAttr = TEXT_ATTRIBUTES_NONE; + osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8)); + if (isAdjustmentFunctionSelected(adjFunc)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdConfig()->item_pos[currentLayout][item]; @@ -1596,79 +1636,168 @@ static bool osdDrawSingleElement(uint8_t item) #endif case OSD_ROLL_PIDS: - { - displayWrite(osdDisplayPort, elemPosX, elemPosY, "ROL"); + displayWrite(osdDisplayPort, elemPosX, elemPosY, "ROL"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_ROLL].P); - if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P) || isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_P)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pidBank()->pid[PID_ROLL].P); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P) || isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_P)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_ROLL].I); - if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I) || isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_I)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pidBank()->pid[PID_ROLL].I); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I) || isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_I)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_ROLL].D); - if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D) || isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_D)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pidBank()->pid[PID_ROLL].D); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D) || isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_D)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); - return true; - } + return true; case OSD_PITCH_PIDS: - { - displayWrite(osdDisplayPort, elemPosX, elemPosY, "PIT"); + displayWrite(osdDisplayPort, elemPosX, elemPosY, "PIT"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_PITCH].P); - if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_P)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pidBank()->pid[PID_PITCH].P); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_P)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_PITCH].I); - if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_I)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pidBank()->pid[PID_PITCH].I); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_I)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_PITCH].D); - if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_D)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pidBank()->pid[PID_PITCH].D); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_D)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); - return true; - } + return true; case OSD_YAW_PIDS: - { - displayWrite(osdDisplayPort, elemPosX, elemPosY, "YAW"); + osdDisplayPIDValues(elemPosX, elemPosY, "YAW", &pidBank()->pid[PID_YAW], ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D); + return true; - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_YAW].P); - if (isAdjustmentFunctionSelected(ADJUSTMENT_YAW_P)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + case OSD_LEVEL_PIDS: + osdDisplayPIDValues(elemPosX, elemPosY, "LEV", &pidBank()->pid[PID_LEVEL], ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); + return true; - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_YAW].I); - if (isAdjustmentFunctionSelected(ADJUSTMENT_YAW_I)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); + case OSD_POS_XY_PIDS: + osdDisplayPIDValues(elemPosX, elemPosY, "PXY", &pidBank()->pid[PID_POS_XY], ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); + return true; - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidBank()->pid[PID_YAW].D); - if (isAdjustmentFunctionSelected(ADJUSTMENT_YAW_D)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); + case OSD_POS_Z_PIDS: + osdDisplayPIDValues(elemPosX, elemPosY, "PZ", &pidBank()->pid[PID_POS_Z], ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); + return true; - return true; - } + case OSD_VEL_XY_PIDS: + if (STATE(FIXED_WING)) + osdDisplayPIDValues(elemPosX, elemPosY, "VXY", &pidBank()->pid[PID_VEL_XY], ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); + return true; + + case OSD_VEL_Z_PIDS: + if (STATE(FIXED_WING)) + osdDisplayPIDValues(elemPosX, elemPosY, "VZ", &pidBank()->pid[PID_VEL_Z], ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); + return true; + + case OSD_HEADING_P: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P); + return true; + + case OSD_BOARD_ALIGN_ROLL: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT); + return true; + + case OSD_BOARD_ALIGN_PITCH: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT); + return true; + + case OSD_RC_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO); + return true; + + case OSD_RC_YAW_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO); + return true; + + case OSD_THROTTLE_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO); + return true; + + case OSD_PITCH_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; + + case OSD_ROLL_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; + + case OSD_YAW_RATE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YRT", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + return true; + + case OSD_MANUAL_RC_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO); + return true; + + case OSD_MANUAL_RC_YAW_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO); + return true; + + case OSD_MANUAL_PITCH_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; + + case OSD_MANUAL_ROLL_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; + + case OSD_MANUAL_YAW_RATE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + return true; + + case OSD_NAV_FW_CRUISE_THR: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRS", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); + return true; + + case OSD_NAV_FW_PITCH2THR: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); + return true; + + case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); + return true; case OSD_POWER: { @@ -2081,6 +2210,29 @@ 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_LEVEL_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10); + osdConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11); + osdConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = 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); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 8e588461982..791922691d7 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -80,6 +80,28 @@ typedef enum { OSD_MAP_NORTH, OSD_MAP_TAKEOFF, OSD_RADAR, + OSD_LEVEL_PIDS, + OSD_POS_XY_PIDS, + OSD_POS_Z_PIDS, + OSD_VEL_XY_PIDS, + OSD_VEL_Z_PIDS, + OSD_HEADING_P, + OSD_BOARD_ALIGN_ROLL, + OSD_BOARD_ALIGN_PITCH, + OSD_RC_EXPO, + OSD_RC_YAW_EXPO, + OSD_THROTTLE_EXPO, + OSD_PITCH_RATE, + OSD_ROLL_RATE, + OSD_YAW_RATE, + OSD_MANUAL_RC_EXPO, + OSD_MANUAL_RC_YAW_EXPO, + OSD_MANUAL_PITCH_RATE, + OSD_MANUAL_ROLL_RATE, + OSD_MANUAL_YAW_RATE, + OSD_NAV_FW_CRUISE_THR, + OSD_NAV_FW_PITCH2THR, + OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, OSD_DEBUG, // Number 46. Intentionally absent from configurator and CMS. Set it from CLI. OSD_WIND_SPEED_HORIZONTAL, OSD_WIND_SPEED_VERTICAL,