Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Add battery profiles feature

Browse source
This commit is contained in:
Michel Pastor 2018-02-12 06:06:03 +01:00
parent 99f7b1daf2
commit ce8621ee8f
26 changed files with 578 additions and 176 deletions
Download patch file Download diff file Expand all files Collapse all files

158
src/main/fc/fc_msp.c
View file

@ -419,7 +419,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif
sbufWriteU16(dst, packSensorStatus());
sbufWriteU16(dst, averageSystemLoadPercent);
sbufWriteU8(dst, getConfigProfile());
sbufWriteU8(dst, (getConfigBatteryProfile() << 4) || getConfigProfile());
sbufWriteU32(dst, armingFlags);
sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags));
}
@ -670,10 +670,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, compassConfig()->mag_declination / 10);
sbufWriteU8(dst, batteryConfig()->voltage.scale / 10);
sbufWriteU8(dst, batteryConfig()->voltage.cellMin / 10);
sbufWriteU8(dst, batteryConfig()->voltage.cellMax / 10);
sbufWriteU8(dst, batteryConfig()->voltage.cellWarning / 10);
sbufWriteU8(dst, batteryMetersConfig()->voltage_scale / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
break;
case MSP2_INAV_MISC:
@ -698,32 +698,32 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, compassConfig()->mag_declination / 10);
sbufWriteU16(dst, batteryConfig()->voltage.scale);
sbufWriteU16(dst, batteryConfig()->voltage.cellDetect);
sbufWriteU16(dst, batteryConfig()->voltage.cellMin);
sbufWriteU16(dst, batteryConfig()->voltage.cellMax);
sbufWriteU16(dst, batteryConfig()->voltage.cellWarning);
sbufWriteU16(dst, batteryMetersConfig()->voltage_scale);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning);
sbufWriteU32(dst, batteryConfig()->capacity.value);
sbufWriteU32(dst, batteryConfig()->capacity.warning);
sbufWriteU32(dst, batteryConfig()->capacity.critical);
sbufWriteU8(dst, batteryConfig()->capacity.unit);
sbufWriteU32(dst, currentBatteryProfile->capacity.value);
sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
break;
case MSP2_INAV_BATTERY_CONFIG:
sbufWriteU16(dst, batteryConfig()->voltage.scale);
sbufWriteU16(dst, batteryConfig()->voltage.cellDetect);
sbufWriteU16(dst, batteryConfig()->voltage.cellMin);
sbufWriteU16(dst, batteryConfig()->voltage.cellMax);
sbufWriteU16(dst, batteryConfig()->voltage.cellWarning);
sbufWriteU16(dst, batteryMetersConfig()->voltage_scale);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning);
sbufWriteU16(dst, batteryConfig()->current.offset);
sbufWriteU16(dst, batteryConfig()->current.scale);
sbufWriteU16(dst, batteryMetersConfig()->current.offset);
sbufWriteU16(dst, batteryMetersConfig()->current.scale);
sbufWriteU32(dst, batteryConfig()->capacity.value);
sbufWriteU32(dst, batteryConfig()->capacity.warning);
sbufWriteU32(dst, batteryConfig()->capacity.critical);
sbufWriteU8(dst, batteryConfig()->capacity.unit);
sbufWriteU32(dst, currentBatteryProfile->capacity.value);
sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
break;
case MSP_MOTOR_PINS:
@ -809,17 +809,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_VOLTAGE_METER_CONFIG:
sbufWriteU8(dst, batteryConfig()->voltage.scale / 10);
sbufWriteU8(dst, batteryConfig()->voltage.cellMin / 10);
sbufWriteU8(dst, batteryConfig()->voltage.cellMax / 10);
sbufWriteU8(dst, batteryConfig()->voltage.cellWarning / 10);
sbufWriteU8(dst, batteryMetersConfig()->voltage_scale / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
break;
case MSP_CURRENT_METER_CONFIG:
sbufWriteU16(dst, batteryConfig()->current.scale);
sbufWriteU16(dst, batteryConfig()->current.offset);
sbufWriteU8(dst, batteryConfig()->current.type);
sbufWriteU16(dst, constrain(batteryConfig()->capacity.value, 0, 0xFFFF));
sbufWriteU16(dst, batteryMetersConfig()->current.scale);
sbufWriteU16(dst, batteryMetersConfig()->current.offset);
sbufWriteU8(dst, batteryMetersConfig()->current.type);
sbufWriteU16(dst, constrain(currentBatteryProfile->capacity.value, 0, 0xFFFF));
break;
case MSP_MIXER:
@ -879,8 +879,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, boardAlignment()->pitchDeciDegrees);
sbufWriteU16(dst, boardAlignment()->yawDeciDegrees);
sbufWriteU16(dst, batteryConfig()->current.scale);
sbufWriteU16(dst, batteryConfig()->current.offset);
sbufWriteU16(dst, batteryMetersConfig()->current.scale);
sbufWriteU16(dst, batteryMetersConfig()->current.offset);
break;
case MSP_CF_SERIAL_CONFIG:
@ -964,7 +964,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif
sbufWriteU8(dst, osdConfig()->units);
sbufWriteU8(dst, osdConfig()->rssi_alarm);
sbufWriteU16(dst, batteryConfig()->capacity.warning);
sbufWriteU16(dst, currentBatteryProfile->capacity.warning);
sbufWriteU16(dst, osdConfig()->time_alarm);
sbufWriteU16(dst, osdConfig()->alt_alarm);
sbufWriteU16(dst, osdConfig()->dist_alarm);
@ -1415,6 +1415,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
{
uint8_t tmp_u8;
uint16_t tmp_u16;
batteryProfile_t *currentBatteryProfileMutable = (batteryProfile_t*)currentBatteryProfile;
const unsigned int dataSize = sbufBytesRemaining(src);
@ -1625,10 +1626,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU16(src);
#endif
batteryConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
batteryConfigMutable()->voltage.cellMin = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->voltage.cellMax = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->voltage.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert
batteryMetersConfigMutable()->voltage_scale = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI
currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert
} else
return MSP_RESULT_ERROR;
break;
@ -1663,18 +1664,18 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU16(src);
#endif
batteryConfigMutable()->voltage.scale = sbufReadU16(src);
batteryConfigMutable()->voltage.cellDetect = sbufReadU16(src);
batteryConfigMutable()->voltage.cellMin = sbufReadU16(src);
batteryConfigMutable()->voltage.cellMax = sbufReadU16(src);
batteryConfigMutable()->voltage.cellWarning = sbufReadU16(src);
batteryMetersConfigMutable()->voltage_scale = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src);
batteryConfigMutable()->capacity.value = sbufReadU32(src);
batteryConfigMutable()->capacity.warning = sbufReadU32(src);
batteryConfigMutable()->capacity.critical = sbufReadU32(src);
batteryConfigMutable()->capacity.unit = sbufReadU8(src);
if ((batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
batteryConfigMutable()->capacity.unit = BAT_CAPACITY_UNIT_MAH;
currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
return MSP_RESULT_ERROR;
}
} else
@ -1683,21 +1684,21 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP2_INAV_SET_BATTERY_CONFIG:
if (dataSize == 27) {
batteryConfigMutable()->voltage.scale = sbufReadU16(src);
batteryConfigMutable()->voltage.cellDetect = sbufReadU16(src);
batteryConfigMutable()->voltage.cellMin = sbufReadU16(src);
batteryConfigMutable()->voltage.cellMax = sbufReadU16(src);
batteryConfigMutable()->voltage.cellWarning = sbufReadU16(src);
batteryMetersConfigMutable()->voltage_scale = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src);
batteryConfigMutable()->current.offset = sbufReadU16(src);
batteryConfigMutable()->current.scale = sbufReadU16(src);
batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
batteryMetersConfigMutable()->current.scale = sbufReadU16(src);
batteryConfigMutable()->capacity.value = sbufReadU32(src);
batteryConfigMutable()->capacity.warning = sbufReadU32(src);
batteryConfigMutable()->capacity.critical = sbufReadU32(src);
batteryConfigMutable()->capacity.unit = sbufReadU8(src);
if ((batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
batteryConfigMutable()->capacity.unit = BAT_CAPACITY_UNIT_MAH;
currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
return MSP_RESULT_ERROR;
}
} else
@ -2089,7 +2090,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif
osdConfigMutable()->units = sbufReadU8(src);
osdConfigMutable()->rssi_alarm = sbufReadU8(src);
batteryConfigMutable()->capacity.warning = sbufReadU16(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU16(src);
osdConfigMutable()->time_alarm = sbufReadU16(src);
osdConfigMutable()->alt_alarm = sbufReadU16(src);
// Won't be read if they weren't provided
@ -2234,20 +2235,20 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_VOLTAGE_METER_CONFIG:
if (dataSize >= 4) {
batteryConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
batteryConfigMutable()->voltage.cellMin = sbufReadU8(src) * 10;
batteryConfigMutable()->voltage.cellMax = sbufReadU8(src) * 10;
batteryConfigMutable()->voltage.cellWarning = sbufReadU8(src) * 10;
batteryMetersConfigMutable()->voltage_scale = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10;
} else
return MSP_RESULT_ERROR;
break;
case MSP_SET_CURRENT_METER_CONFIG:
if (dataSize >= 7) {
batteryConfigMutable()->current.scale = sbufReadU16(src);
batteryConfigMutable()->current.offset = sbufReadU16(src);
batteryConfigMutable()->current.type = sbufReadU8(src);
batteryConfigMutable()->capacity.value = sbufReadU16(src);
batteryMetersConfigMutable()->current.scale = sbufReadU16(src);
batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
batteryMetersConfigMutable()->current.type = sbufReadU8(src);
currentBatteryProfileMutable->capacity.value = sbufReadU16(src);
} else
return MSP_RESULT_ERROR;
break;
@ -2331,8 +2332,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src); // board_align_pitch
boardAlignmentMutable()->yawDeciDegrees = sbufReadU16(src); // board_align_yaw
batteryConfigMutable()->current.scale = sbufReadU16(src);
batteryConfigMutable()->current.offset = sbufReadU16(src);
batteryMetersConfigMutable()->current.scale = sbufReadU16(src);
batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
} else
return MSP_RESULT_ERROR;
break;
@ -2517,6 +2518,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break;
#endif
case MSP2_INAV_SELECT_BATTERY_PROFILE:
if (!ARMING_FLAG(ARMED)) {
if (sbufReadU8Safe(&tmp_u8, src))
setConfigBatteryProfileAndWriteEEPROM(tmp_u8);
}
break;
default:
return MSP_RESULT_ERROR;
}