Don't check batteryConfig fields from the telemetry, call APIs instead

This makes the telemetry code less dependendant on the battery
implementation.

New functions introduced:
    isBatteryVoltageAvailable()
    isAmperageAvailable()
This commit is contained in:
Alberto García Hierro 2017-12-04 16:40:04 +00:00
parent 97a46aa0e9
commit d666151188
9 changed files with 27 additions and 9 deletions

View File

@ -450,6 +450,11 @@ void batteryUpdateAlarms(void)
}
}
bool isBatteryVoltageAvailable(void)
{
return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0;
}
uint16_t getBatteryVoltage(void)
{
return voltageMeter.filtered;
@ -470,6 +475,11 @@ uint16_t getBatteryAverageCellVoltage(void)
return voltageMeter.filtered / batteryCellCount;
}
bool isAmperageAvailable(void)
{
return batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
}
int32_t getAmperage(void) {
return currentMeter.amperage;
}

View File

@ -77,11 +77,13 @@ struct rxConfig_s;
float calculateVbatPidCompensation(void);
uint8_t calculateBatteryPercentageRemaining(void);
bool isBatteryVoltageAvailable(void);
uint16_t getBatteryVoltage(void);
uint16_t getBatteryVoltageLatest(void);
uint8_t getBatteryCellCount(void);
uint16_t getBatteryAverageCellVoltage(void);
bool isAmperageAvailable(void);
int32_t getAmperage(void);
int32_t getAmperageLatest(void);
int32_t getMAhDrawn(void);

View File

@ -399,7 +399,7 @@ void initCrsfTelemetry(void)
if (sensors(SENSOR_ACC)) {
crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
}
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE || batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
if (isBatteryVoltageAvailable() || isAmperageAvailable()) {
crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
}
crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX);

View File

@ -586,7 +586,7 @@ void handleFrSkyTelemetry(timeUs_t currentTimeUs)
sendVoltageCells();
sendVoltageAmp();
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
if (isAmperageAvailable()) {
sendAmperage();
sendFuelLevel();
}

View File

@ -222,11 +222,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)
(batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) ? getBatteryVoltage() * 100 : 0,
isBatteryVoltageAvailable() ? getBatteryVoltage() * 100 : 0,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
(batteryConfig()->currentMeterSource != CURRENT_METER_NONE) ? getAmperage() : -1,
isAmperageAvailable() ? getAmperage() : -1,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
(batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) ? calculateBatteryPercentageRemaining() : 100,
isBatteryVoltageAvailable() ? calculateBatteryPercentageRemaining() : 100,
// 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)

View File

@ -372,7 +372,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
switch (id) {
case FSSP_DATAID_VFAS :
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
if (isBatteryVoltageAvailable()) {
uint16_t vfasVoltage;
if (telemetryConfig()->report_cell_voltage) {
vfasVoltage = getBatteryVoltage() / getBatteryCellCount();
@ -384,7 +384,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
}
break;
case FSSP_DATAID_CURRENT :
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
if (isAmperageAvailable()) {
smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
*clearToSend = false;
}
@ -397,7 +397,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
}
break;
case FSSP_DATAID_FUEL :
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
if (isAmperageAvailable()) {
smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit
*clearToSend = false;
}
@ -553,7 +553,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
break;
#endif
case FSSP_DATAID_A4 :
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
if (isBatteryVoltageAvailable()) {
smartPortSendPackage(id, getBatteryVoltage() * 10 / getBatteryCellCount()); // given in 0.1V, convert to volts
*clearToSend = false;
}

View File

@ -288,4 +288,6 @@ serialPort_t *telemetrySharedPort = NULL;
void crsfScheduleDeviceInfoResponse(void) {};
void crsfScheduleMspResponse(void) {};
bool bufferMspFrame(uint8_t *, int) {return true;}
bool isBatteryVoltageAvailable(void) { return true; }
bool isAmperageAvailable(void) { return true; }
}

View File

@ -254,9 +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; }
uint16_t getBatteryVoltage(void) {
return testBatteryVoltage;
}
bool isAmperageAvailable(void) { return true; }
int32_t getAmperage(void) {
return testAmperage;
}

View File

@ -336,5 +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; }
}