diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bef7c54de40..d2cf7ad36dd 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -383,6 +383,68 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, uint16_t } #endif +// Calibration data is stored as int16_t but transmitted as raw bytes via sbufWriteU16. +static void sbufWriteAxisU16(sbuf_t *dst, const int16_t *arr) +{ + sbufWriteU16(dst, arr[X]); + sbufWriteU16(dst, arr[Y]); + sbufWriteU16(dst, arr[Z]); +} + +static void sbufReadAxisU16(sbuf_t *src, int16_t *arr) +{ + arr[X] = sbufReadU16(src); + arr[Y] = sbufReadU16(src); + arr[Z] = sbufReadU16(src); +} + +static void mspReadRates(sbuf_t *src, uint8_t *rates) +{ + for (int i = 0; i < 3; ++i) { + uint8_t v = sbufReadU8(src); + rates[i] = (i == FD_YAW) ? constrain(v, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX) + : constrain(v, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); + } +} + +static void mspDeserializeServoParams(sbuf_t *src, uint8_t servoIndex) +{ + servoParamsMutable(servoIndex)->min = sbufReadU16(src); + servoParamsMutable(servoIndex)->max = sbufReadU16(src); + servoParamsMutable(servoIndex)->middle = sbufReadU16(src); + servoParamsMutable(servoIndex)->rate = sbufReadU8(src); + servoComputeScalingFactors(servoIndex); +} + +static void mspSerializeServoParams(sbuf_t *dst, const servoParam_t *sp) +{ + sbufWriteU16(dst, sp->min); + sbufWriteU16(dst, sp->max); + sbufWriteU16(dst, sp->middle); + sbufWriteU8(dst, sp->rate); +} + +static void mspSerializeMotorMixer(sbuf_t *dst, const motorMixer_t *m) +{ + sbufWriteU16(dst, constrainf(m->throttle + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(m->roll + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(m->pitch + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(m->yaw + 2.0f, 0.0f, 4.0f) * 1000); +} + +static void mspSerializeServoMixer(sbuf_t *dst, const servoMixer_t *m) +{ + sbufWriteU8(dst, m->targetChannel); + sbufWriteU8(dst, m->inputSource); + sbufWriteU16(dst, m->rate); + sbufWriteU8(dst, m->speed); +#ifdef USE_PROGRAMMING_FRAMEWORK + sbufWriteU8(dst, m->conditionId); +#else + sbufWriteU8(dst, -1); +#endif +} + /* * Returns true if the command was processd, false otherwise. * May set mspPostProcessFunc to a function to be called once the command has been processed @@ -538,10 +600,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_SERVO_CONFIGURATIONS: for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - sbufWriteU16(dst, servoParams(i)->min); - sbufWriteU16(dst, servoParams(i)->max); - sbufWriteU16(dst, servoParams(i)->middle); - sbufWriteU8(dst, servoParams(i)->rate); + mspSerializeServoParams(dst, servoParams(i)); sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); sbufWriteU8(dst, 255); // used to be forwardFromChannel, not used anymore, send 0xff for compatibility reasons @@ -550,10 +609,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_SERVO_CONFIG: for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - sbufWriteU16(dst, servoParams(i)->min); - sbufWriteU16(dst, servoParams(i)->max); - sbufWriteU16(dst, servoParams(i)->middle); - sbufWriteU8(dst, servoParams(i)->rate); + mspSerializeServoParams(dst, servoParams(i)); } break; case MSP_SERVO_MIX_RULES: @@ -569,27 +625,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_SERVO_MIXER: for (int i = 0; i < MAX_SERVO_RULES; i++) { - sbufWriteU8(dst, customServoMixers(i)->targetChannel); - sbufWriteU8(dst, customServoMixers(i)->inputSource); - sbufWriteU16(dst, customServoMixers(i)->rate); - sbufWriteU8(dst, customServoMixers(i)->speed); - #ifdef USE_PROGRAMMING_FRAMEWORK - sbufWriteU8(dst, customServoMixers(i)->conditionId); - #else - sbufWriteU8(dst, -1); - #endif + mspSerializeServoMixer(dst, customServoMixers(i)); } if(MAX_MIXER_PROFILE_COUNT==1) break; for (int i = 0; i < MAX_SERVO_RULES; i++) { - sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel); - sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].inputSource); - sbufWriteU16(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate); - sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed); - #ifdef USE_PROGRAMMING_FRAMEWORK - sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].conditionId); - #else - sbufWriteU8(dst, -1); - #endif + mspSerializeServoMixer(dst, &mixerServoMixersByIndex(nextMixerProfileIndex)[i]); } break; #ifdef USE_PROGRAMMING_FRAMEWORK @@ -649,17 +689,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif case MSP2_COMMON_MOTOR_MIXER: for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->throttle + 2.0f, 0.0f, 4.0f) * 1000); - sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000); - sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000); - sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000); + mspSerializeMotorMixer(dst, primaryMotorMixer(i)); } if (MAX_MIXER_PROFILE_COUNT==1) break; for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].throttle + 2.0f, 0.0f, 4.0f) * 1000); - sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].roll + 2.0f, 0.0f, 4.0f) * 1000); - sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].pitch + 2.0f, 0.0f, 4.0f) * 1000); - sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].yaw + 2.0f, 0.0f, 4.0f) * 1000); + mspSerializeMotorMixer(dst, &mixerMotorMixersByIndex(nextMixerProfileIndex)[i]); } break; @@ -1453,17 +1487,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_CALIBRATION_DATA: sbufWriteU8(dst, accGetCalibrationAxisFlags()); - sbufWriteU16(dst, accelerometerConfig()->accZero.raw[X]); - sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Y]); - sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Z]); - sbufWriteU16(dst, accelerometerConfig()->accGain.raw[X]); - sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Y]); - sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Z]); + sbufWriteAxisU16(dst, accelerometerConfig()->accZero.raw); + sbufWriteAxisU16(dst, accelerometerConfig()->accGain.raw); #ifdef USE_MAG - sbufWriteU16(dst, compassConfig()->magZero.raw[X]); - sbufWriteU16(dst, compassConfig()->magZero.raw[Y]); - sbufWriteU16(dst, compassConfig()->magZero.raw[Z]); + sbufWriteAxisU16(dst, compassConfig()->magZero.raw); #else sbufWriteU16(dst, 0); sbufWriteU16(dst, 0); @@ -2098,15 +2126,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons // need to cast away const to set controlProfile ((controlConfig_t*)currentControlProfile)->stabilized.rcExpo8 = sbufReadU8(src); - for (int i = 0; i < 3; i++) { - tmp_u8 = sbufReadU8(src); - if (i == FD_YAW) { - ((controlConfig_t*)currentControlProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); - } - else { - ((controlConfig_t*)currentControlProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); - } - } + mspReadRates(src, ((controlConfig_t*)currentControlProfile)->stabilized.rates); tmp_u8 = sbufReadU8(src); ((controlConfig_t*)currentControlProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX); ((controlConfig_t*)currentControlProfile)->throttle.rcMid8 = sbufReadU8(src); @@ -2135,26 +2155,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) // stabilized currentControlProfile_p->stabilized.rcExpo8 = sbufReadU8(src); currentControlProfile_p->stabilized.rcYawExpo8 = sbufReadU8(src); - for (uint8_t i = 0; i < 3; ++i) { - tmp_u8 = sbufReadU8(src); - if (i == FD_YAW) { - currentControlProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); - } else { - currentControlProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); - } - } + mspReadRates(src, currentControlProfile_p->stabilized.rates); // manual currentControlProfile_p->manual.rcExpo8 = sbufReadU8(src); currentControlProfile_p->manual.rcYawExpo8 = sbufReadU8(src); - for (uint8_t i = 0; i < 3; ++i) { - tmp_u8 = sbufReadU8(src); - if (i == FD_YAW) { - currentControlProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); - } else { - currentControlProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); - } - } + mspReadRates(src, currentControlProfile_p->manual.rates); } else { return MSP_RESULT_ERROR; @@ -2346,15 +2352,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (tmp_u8 >= MAX_SUPPORTED_SERVOS) { return MSP_RESULT_ERROR; } else { - servoParamsMutable(tmp_u8)->min = sbufReadU16(src); - servoParamsMutable(tmp_u8)->max = sbufReadU16(src); - servoParamsMutable(tmp_u8)->middle = sbufReadU16(src); - servoParamsMutable(tmp_u8)->rate = sbufReadU8(src); + mspDeserializeServoParams(src, tmp_u8); sbufReadU8(src); sbufReadU8(src); sbufReadU8(src); // used to be forwardFromChannel, ignored sbufReadU32(src); // used to be reversedSources - servoComputeScalingFactors(tmp_u8); } break; @@ -2366,11 +2368,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (tmp_u8 >= MAX_SUPPORTED_SERVOS) { return MSP_RESULT_ERROR; } else { - servoParamsMutable(tmp_u8)->min = sbufReadU16(src); - servoParamsMutable(tmp_u8)->max = sbufReadU16(src); - servoParamsMutable(tmp_u8)->middle = sbufReadU16(src); - servoParamsMutable(tmp_u8)->rate = sbufReadU8(src); - servoComputeScalingFactors(tmp_u8); + mspDeserializeServoParams(src, tmp_u8); } break; @@ -2821,17 +2819,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_CALIBRATION_DATA: if (dataSize >= 18) { - accelerometerConfigMutable()->accZero.raw[X] = sbufReadU16(src); - accelerometerConfigMutable()->accZero.raw[Y] = sbufReadU16(src); - accelerometerConfigMutable()->accZero.raw[Z] = sbufReadU16(src); - accelerometerConfigMutable()->accGain.raw[X] = sbufReadU16(src); - accelerometerConfigMutable()->accGain.raw[Y] = sbufReadU16(src); - accelerometerConfigMutable()->accGain.raw[Z] = sbufReadU16(src); + sbufReadAxisU16(src, accelerometerConfigMutable()->accZero.raw); + sbufReadAxisU16(src, accelerometerConfigMutable()->accGain.raw); #ifdef USE_MAG - compassConfigMutable()->magZero.raw[X] = sbufReadU16(src); - compassConfigMutable()->magZero.raw[Y] = sbufReadU16(src); - compassConfigMutable()->magZero.raw[Z] = sbufReadU16(src); + sbufReadAxisU16(src, compassConfigMutable()->magZero.raw); #else sbufReadU16(src); sbufReadU16(src); @@ -2844,9 +2836,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif #ifdef USE_MAG if (dataSize >= 22) { - compassConfigMutable()->magGain[X] = sbufReadU16(src); - compassConfigMutable()->magGain[Y] = sbufReadU16(src); - compassConfigMutable()->magGain[Z] = sbufReadU16(src); + sbufReadAxisU16(src, compassConfigMutable()->magGain); } #else if (dataSize >= 22) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 43f6c8f55ad..92be7493a94 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -441,112 +441,35 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) */ int osdFormatVelocityStr(char* buff, int32_t vel, osd_SpeedTypes_e speedType, bool _max) { - int strLen = 0; - + char unitSym, unit3dSym; switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: - FALLTHROUGH; case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (_max) { - switch (speedType) { - case OSD_SPEED_TYPE_GROUND: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_MPH); - break; - case OSD_SPEED_TYPE_AIR: - strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_MPH); - break; - case OSD_SPEED_TYPE_3D: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_MPH); - break; - case OSD_SPEED_TYPE_MIN_GROUND: - break; - } - } else { - switch (speedType){ - case OSD_SPEED_TYPE_GROUND: - strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_MPH); - break; - case OSD_SPEED_TYPE_AIR: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_MPH); - break; - case OSD_SPEED_TYPE_3D: - strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_MPH); - break; - case OSD_SPEED_TYPE_MIN_GROUND: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_MPH); - break; - } - } - break; - case OSD_UNIT_METRIC: - if (_max) { - switch (speedType) { - case OSD_SPEED_TYPE_GROUND: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_KMH); - break; - case OSD_SPEED_TYPE_AIR: - strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KMH); - break; - case OSD_SPEED_TYPE_3D: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_KMH); - break; - case OSD_SPEED_TYPE_MIN_GROUND: - break; - } - } else { - switch (speedType) { - case OSD_SPEED_TYPE_GROUND: - strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_KMH); - break; - case OSD_SPEED_TYPE_AIR: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KMH); - break; - case OSD_SPEED_TYPE_3D: - strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_KMH); - break; - case OSD_SPEED_TYPE_MIN_GROUND: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_KMH); - break; - } - } - break; + unitSym = SYM_MPH; unit3dSym = SYM_3D_MPH; break; case OSD_UNIT_GA: - if (_max) { - switch (speedType) { - case OSD_SPEED_TYPE_GROUND: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_KT); - break; - case OSD_SPEED_TYPE_AIR: - strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KT); - break; - case OSD_SPEED_TYPE_3D: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_KT); - break; - case OSD_SPEED_TYPE_MIN_GROUND: - break; - } - } else { - switch (speedType) { - case OSD_SPEED_TYPE_GROUND: - strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_KT); - break; - case OSD_SPEED_TYPE_AIR: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KT); - break; - case OSD_SPEED_TYPE_3D: - strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_KT); - break; - case OSD_SPEED_TYPE_MIN_GROUND: - strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_KT); - break; - } - } - break; - } - - return strLen; + unitSym = SYM_KT; unit3dSym = SYM_3D_KT; break; + case OSD_UNIT_METRIC: + default: + unitSym = SYM_KMH; unit3dSym = SYM_3D_KMH; break; + } + + int val = (int)osdConvertVelocityToUnit(vel); + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + return _max ? tfp_sprintf(buff, "%c%3d%c", SYM_MAX, val, unitSym) + : tfp_sprintf(buff, "%3d%c", val, unitSym); + case OSD_SPEED_TYPE_AIR: + return _max ? tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, val, unitSym) + : tfp_sprintf(buff, "%c%3d%c", SYM_AIR, val, unitSym); + case OSD_SPEED_TYPE_3D: + return _max ? tfp_sprintf(buff, "%c%3d%c", SYM_MAX, val, unit3dSym) + : tfp_sprintf(buff, "%3d%c", val, unit3dSym); + case OSD_SPEED_TYPE_MIN_GROUND: + return _max ? 0 + : tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, val, unitSym); + } + return 0; } /** @@ -1830,6 +1753,45 @@ static bool osdElementEnabled(uint8_t elementID, bool onlyCurrentLayout) { return elementEnabled; } +static void osdDisplayBattVoltDJI(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t total_digits, uint8_t decimals) +{ +#ifndef DISABLE_MSP_DJI_COMPAT + if (isDJICompatibleVideoSystem(osdConfig())) total_digits++; +#endif + osdDisplayBatteryVoltage(elemPosX, elemPosY, voltage, total_digits, decimals); +} + +static void osdFormatAngleDeg(char *buff, char sym, bool valid, int16_t degrees) +{ + buff[0] = sym; + if (valid) + osdFormatIntUnit(&buff[1], 3, degrees, 0); + else + buff[1] = buff[2] = buff[3] = '-'; + buff[4] = SYM_DEGREES; + buff[5] = '\0'; +} + +// rawValue must be pre-computed by the caller; this function owns the GPS/speed +// guard and returns 0 (show dashes) when GPS is absent or speed is zero. +static int32_t osdUpdateEfficiencyFilter(pt1Filter_t *state, timeUs_t *lastUpdated, float rawValue) +{ + if (!(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) || gpsSol.groundSpeed == 0) { + return 0; + } + timeUs_t currentTimeUs = micros(); + timeDelta_t delta = cmpTimeUs(currentTimeUs, *lastUpdated); + if (delta >= EFFICIENCY_UPDATE_INTERVAL) { + pt1FilterApply4(state, rawValue, 1, US2S(delta)); + *lastUpdated = currentTimeUs; + } + return (int32_t)state->state; +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; @@ -1843,45 +1805,18 @@ static bool osdDrawSingleElement(uint8_t item) switch (item) { case OSD_CUSTOM_ELEMENT_1: - { - customElementDrawElement(buff, 0); - break; - } case OSD_CUSTOM_ELEMENT_2: - { - customElementDrawElement(buff, 1); - break; - } case OSD_CUSTOM_ELEMENT_3: - { - customElementDrawElement(buff, 2); + customElementDrawElement(buff, item - OSD_CUSTOM_ELEMENT_1); break; - } + case OSD_CUSTOM_ELEMENT_4: - { - customElementDrawElement(buff, 3); - break; - } case OSD_CUSTOM_ELEMENT_5: - { - customElementDrawElement(buff, 4); - break; - } case OSD_CUSTOM_ELEMENT_6: - { - customElementDrawElement(buff, 5); - break; - } case OSD_CUSTOM_ELEMENT_7: - { - customElementDrawElement(buff, 6); - break; - } case OSD_CUSTOM_ELEMENT_8: - { - customElementDrawElement(buff, 7); + customElementDrawElement(buff, item - OSD_CUSTOM_ELEMENT_4 + 3); break; - } case OSD_RSSI_VALUE: { uint8_t osdRssi = osdConvertRSSI(); @@ -1897,27 +1832,15 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_MAIN_BATT_VOLTAGE: { - uint8_t base_digits = 2U; -#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it - if(isDJICompatibleVideoSystem(osdConfig())) { - base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space - } -#endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); + case OSD_MAIN_BATT_VOLTAGE: + osdDisplayBattVoltDJI(elemPosX, elemPosY, getBatteryRawVoltage(), + 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); return true; - } - case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: { - uint8_t base_digits = 2U; -#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it - if(isDJICompatibleVideoSystem(osdConfig())) { - base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space - } -#endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); + case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: + osdDisplayBattVoltDJI(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), + 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); return true; - } case OSD_CURRENT_DRAW: { osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3, false); @@ -2213,17 +2136,9 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_GROUND_COURSE: - { - buff[0] = SYM_GROUND_COURSE; - if (osdIsHeadingValid()) { - osdFormatIntUnit(&buff[1], 3, (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog), 0); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + osdFormatAngleDeg(buff, SYM_GROUND_COURSE, osdIsHeadingValid(), + (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog)); + break; case OSD_COURSE_HOLD_ERROR: { @@ -3573,28 +3488,12 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_MAIN_BATT_CELL_VOLTAGE: - { - uint8_t base_digits = 3U; -#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it - if(isDJICompatibleVideoSystem(osdConfig())) { - base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space - } -#endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), base_digits, 2); - return true; - } + osdDisplayBattVoltDJI(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2); + return true; case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE: - { - uint8_t base_digits = 3U; -#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it - if(isDJICompatibleVideoSystem(osdConfig())) { - base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space - } -#endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), base_digits, 2); - return true; - } + osdDisplayBattVoltDJI(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), 3, 2); + return true; case OSD_SCALED_THROTTLE_POS: { @@ -3604,18 +3503,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HEADING: { - buff[0] = SYM_HEADING; - if (osdIsHeadingValid()) { - int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading()); - if (h < 0) { - h += 360; - } - osdFormatIntUnit(&buff[1], 3, h, 0); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; + int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading()); + if (h < 0) h += 360; + osdFormatAngleDeg(buff, SYM_HEADING, osdIsHeadingValid(), h); break; } @@ -3634,35 +3524,16 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_EFFICIENCY_MAH_PER_KM: { - // amperage is in centi amps, speed is in cms/s. We want - // mah/km. Only show when ground speed > 1m/s. + // amperage is in centi amps, speed is in cms/s. We want mah/km. static pt1Filter_t eFilterState; static timeUs_t efficiencyUpdated = 0; - int32_t value = 0; bool moreThanAh = false; - timeUs_t currentTimeUs = micros(); - timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); uint8_t digits = 3U; -#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values - if (isDJICompatibleVideoSystem(osdConfig())) { - // Increase number of digits so values above 99 don't get scaled by osdFormatCentiNumber - digits = 4U; - } -#endif - if ((STATE(GPS_FIX) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) +#ifndef DISABLE_MSP_DJI_COMPAT + if (isDJICompatibleVideoSystem(osdConfig())) digits = 4U; #endif - ) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); - - efficiencyUpdated = currentTimeUs; - } else { - value = eFilterState.state; - } - } + float rawEff = gpsSol.groundSpeed > 0 ? ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f : 0.0f; // div/0 guard only; helper owns the GPS guard + int32_t value = osdUpdateEfficiencyFilter(&eFilterState, &efficiencyUpdated, rawEff); bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); switch (osdConfig()->units) { case OSD_UNIT_UK: @@ -3717,27 +3588,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_EFFICIENCY_WH_PER_KM: { - // amperage is in centi amps, speed is in cms/s. We want - // mWh/km. Only show when ground speed > 1m/s. + // power is in mW, speed is in cms/s. We want mWh/km. static pt1Filter_t eFilterState; static timeUs_t efficiencyUpdated = 0; - int32_t value = 0; - timeUs_t currentTimeUs = micros(); - timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if ((STATE(GPS_FIX) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); - - efficiencyUpdated = currentTimeUs; - } else { - value = eFilterState.state; - } - } + float rawEff = gpsSol.groundSpeed > 0 ? ((float)getPower() / gpsSol.groundSpeed) / 0.0036f : 0.0f; + int32_t value = osdUpdateEfficiencyFilter(&eFilterState, &efficiencyUpdated, rawEff); bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); switch (osdConfig()->units) { case OSD_UNIT_UK: @@ -3901,24 +3756,10 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_AZIMUTH: { - - buff[0] = SYM_AZIMUTH; - if (osdIsHeadingValid()) { - int16_t h = GPS_directionToHome; - if (h < 0) { - h += 360; - } - if (h >= 180) - h = h - 180; - else - h = h + 180; - - osdFormatIntUnit(&buff[1], 3, h, 0); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; + int16_t h = GPS_directionToHome; + if (h < 0) h += 360; + h = (h >= 180) ? h - 180 : h + 180; + osdFormatAngleDeg(buff, SYM_AZIMUTH, osdIsHeadingValid(), h); break; } @@ -3986,25 +3827,11 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_GVAR_0: - { - osdFormatGVar(buff, 0); - break; - } case OSD_GVAR_1: - { - osdFormatGVar(buff, 1); - break; - } case OSD_GVAR_2: - { - osdFormatGVar(buff, 2); - break; - } case OSD_GVAR_3: - { - osdFormatGVar(buff, 3); + osdFormatGVar(buff, item - OSD_GVAR_0); break; - } #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) case OSD_RC_SOURCE: @@ -5408,7 +5235,6 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX) strcat(osdFormatTrimWhiteSpace(buff), "/"); itoa(stats.min_rssi_dbm, buff + strlen(buff), 10); osdWriteChar(buff + strlen(buff), SYM_DBM); - displayWrite(osdDisplayPort, statValX, row++, buff); } }