diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 757a620ceb..22cae20ecd 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -450,9 +450,9 @@ void batteryUpdateAlarms(void) } } -bool isBatteryVoltageAvailable(void) +bool isBatteryVoltageConfigured(void) { - return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0; + return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE; } uint16_t getBatteryVoltage(void) @@ -475,7 +475,7 @@ uint16_t getBatteryAverageCellVoltage(void) return voltageMeter.filtered / batteryCellCount; } -bool isAmperageAvailable(void) +bool isAmperageConfigured(void) { return batteryConfig()->currentMeterSource != CURRENT_METER_NONE; } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 55238d84b2..955f7ad665 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -77,13 +77,13 @@ struct rxConfig_s; float calculateVbatPidCompensation(void); uint8_t calculateBatteryPercentageRemaining(void); -bool isBatteryVoltageAvailable(void); +bool isBatteryVoltageConfigured(void); uint16_t getBatteryVoltage(void); uint16_t getBatteryVoltageLatest(void); uint8_t getBatteryCellCount(void); uint16_t getBatteryAverageCellVoltage(void); -bool isAmperageAvailable(void); +bool isAmperageConfigured(void); int32_t getAmperage(void); int32_t getAmperageLatest(void); int32_t getMAhDrawn(void); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index c3917d368d..22bb295de0 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -399,7 +399,7 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_ACC)) { crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); } - if (isBatteryVoltageAvailable() || isAmperageAvailable()) { + if (isBatteryVoltageConfigured() || isAmperageConfigured()) { crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); } crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 0eaada8ec9..c0fd7ae9d2 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -352,22 +352,26 @@ static void sendVario(void) static void sendVoltageCells(void) { static uint16_t currentCell; + uint32_t cellVoltage = 0; + const uint8_t cellCount = getBatteryCellCount(); - uint8_t cellCount = getBatteryCellCount(); - currentCell %= cellCount; - - /* - * Format for Voltage Data for single cells is like this: - * - * llll llll cccc hhhh - * l: Low voltage bits - * h: High voltage bits - * c: Cell number (starting at 0) - * - * The actual value sent for cell voltage has resolution of 0.002 volts - * Since vbat has resolution of 0.1 volts it has to be multiplied by 50 - */ - uint32_t cellVoltage = ((uint32_t)getBatteryVoltage() * 100 + cellCount) / (cellCount * 2); + if (cellCount) { + currentCell %= cellCount; + /* + * Format for Voltage Data for single cells is like this: + * + * llll llll cccc hhhh + * l: Low voltage bits + * h: High voltage bits + * c: Cell number (starting at 0) + * + * The actual value sent for cell voltage has resolution of 0.002 volts + * Since vbat has resolution of 0.1 volts it has to be multiplied by 50 + */ + cellVoltage = ((uint32_t)getBatteryVoltage() * 100 + cellCount) / (cellCount * 2); + } else { + currentCell = 0; + } // Cell number is at bit 9-12 uint16_t data = (currentCell << 4); @@ -389,6 +393,7 @@ static void sendVoltageCells(void) static void sendVoltageAmp(void) { uint16_t voltage = getBatteryVoltage(); + const uint8_t cellCount = getBatteryCellCount(); if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { // Use new ID 0x39 to send voltage directly in 0.1 volts resolution @@ -396,8 +401,8 @@ static void sendVoltageAmp(void) } else { // send in 0.2 volts resolution voltage *= 110 / 21; - if (telemetryConfig()->report_cell_voltage) { - voltage /= getBatteryCellCount(); + if (telemetryConfig()->report_cell_voltage && cellCount) { + voltage /= cellCount; } frSkyHubWriteFrame(ID_VOLTAGE_AMP_BP, voltage / 100); @@ -554,11 +559,11 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs) sendTemperature1(); sendThrottleOrBatterySizeAsRpm(); - if (isBatteryVoltageAvailable()) { + if (isBatteryVoltageConfigured()) { sendVoltageCells(); sendVoltageAmp(); - if (isAmperageAvailable()) { + if (isAmperageConfigured()) { sendAmperage(); sendFuelLevel(); } diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index e5aabdb577..392282fe56 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -208,6 +208,16 @@ void mavlinkSendSystemStatus(void) if (sensors(SENSOR_BARO)) onboardControlAndSensors |= 8200; if (sensors(SENSOR_GPS)) onboardControlAndSensors |= 16416; + uint16_t batteryVoltage = 0; + int16_t batteryAmperage = -1; + int8_t batteryRemaining = 100; + + if (getBatteryState() < BATTERY_NOT_PRESENT) { + batteryVoltage = isBatteryVoltageConfigured() ? getBatteryVoltage() * 100 : batteryVoltage; + batteryAmperage = isAmperageConfigured() ? getAmperage() : batteryAmperage; + batteryRemaining = isBatteryVoltageConfigured() ? calculateBatteryPercentageRemaining() : batteryRemaining; + } + mavlink_msg_sys_status_pack(0, 200, &mavMsg, // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. //Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, @@ -222,11 +232,11 @@ void mavlinkSendSystemStatus(void) // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 0, // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - isBatteryVoltageAvailable() ? getBatteryVoltage() * 100 : 0, + batteryVoltage, // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - isAmperageAvailable() ? getAmperage() : -1, + batteryAmperage, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - isBatteryVoltageAvailable() ? calculateBatteryPercentageRemaining() : 100, + batteryRemaining, // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index bdc61e11a0..81ec89b2f1 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -376,10 +376,11 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear switch (id) { case FSSP_DATAID_VFAS : - if (isBatteryVoltageAvailable()) { + if (isBatteryVoltageConfigured()) { uint16_t vfasVoltage; if (telemetryConfig()->report_cell_voltage) { - vfasVoltage = getBatteryVoltage() / getBatteryCellCount(); + const uint8_t cellCount = getBatteryCellCount(); + vfasVoltage = cellCount ? getBatteryVoltage() / cellCount : 0; } else { vfasVoltage = getBatteryVoltage(); } @@ -388,7 +389,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } break; case FSSP_DATAID_CURRENT : - if (isAmperageAvailable()) { + if (isAmperageConfigured()) { smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit *clearToSend = false; } @@ -401,7 +402,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } break; case FSSP_DATAID_FUEL : - if (isAmperageAvailable()) { + if (isAmperageConfigured()) { smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit *clearToSend = false; } @@ -563,8 +564,10 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; #endif case FSSP_DATAID_A4 : - if (isBatteryVoltageAvailable()) { - smartPortSendPackage(id, getBatteryVoltage() * 10 / getBatteryCellCount()); // given in 0.1V, convert to volts + if (isBatteryVoltageConfigured()) { + const uint8_t cellCount = getBatteryCellCount(); + const uint32_t vfasVoltage = cellCount ? (getBatteryVoltage() * 10 / cellCount) : 0; // given in 0.1V, convert to volts + smartPortSendPackage(id, vfasVoltage); *clearToSend = false; } break; diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 2dc71a6756..a7dd43646a 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -254,11 +254,11 @@ extern "C" { uint32_t micros(void) {return dummyTimeUs;} serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) {return NULL;} serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} - bool isBatteryVoltageAvailable(void) { return true; } + bool isBatteryVoltageConfigured(void) { return true; } uint16_t getBatteryVoltage(void) { return testBatteryVoltage; } - bool isAmperageAvailable(void) { return true; } + bool isAmperageConfigured(void) { return true; } int32_t getAmperage(void) { return testAmperage; } diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 1a128ca483..ed6ae76921 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -336,7 +336,7 @@ int32_t getMAhDrawn(void){ bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; } bool handleMspFrame(uint8_t *, int) { return false; } void crsfScheduleMspResponse(void) {}; -bool isBatteryVoltageAvailable(void) { return true; } -bool isAmperageAvailable(void) { return true; } +bool isBatteryVoltageConfigured(void) { return true; } +bool isAmperageConfigured(void) { return true; } }