forked from Archive/PX4-Autopilot
battery_status: report remaining flight time in seconds
This allows more accurate reporting and is compliant with the MAVLink interface.
This commit is contained in:
parent
4ba84d56c9
commit
54f2e91775
|
@ -8,6 +8,7 @@ float32 current_average_a # Battery current average in amperes, -1 if unknown
|
||||||
float32 discharged_mah # Discharged amount in mAh, -1 if unknown
|
float32 discharged_mah # Discharged amount in mAh, -1 if unknown
|
||||||
float32 remaining # From 1 to 0, -1 if unknown
|
float32 remaining # From 1 to 0, -1 if unknown
|
||||||
float32 scale # Power scaling factor, >= 1, or -1 if unknown
|
float32 scale # Power scaling factor, >= 1, or -1 if unknown
|
||||||
|
float32 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, -1 if unknown
|
||||||
float32 temperature # temperature of the battery. NaN if unknown
|
float32 temperature # temperature of the battery. NaN if unknown
|
||||||
int32 cell_count # Number of cells
|
int32 cell_count # Number of cells
|
||||||
|
|
||||||
|
@ -18,7 +19,6 @@ uint8 source # Battery source
|
||||||
uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
|
uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
|
||||||
uint16 capacity # actual capacity of the battery
|
uint16 capacity # actual capacity of the battery
|
||||||
uint16 cycle_count # number of discharge cycles the battery has experienced
|
uint16 cycle_count # number of discharge cycles the battery has experienced
|
||||||
uint16 run_time_to_empty # predicted remaining battery capacity based on the present rate of discharge in min
|
|
||||||
uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min
|
uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min
|
||||||
uint16 serial_number # serial number of the battery pack
|
uint16 serial_number # serial number of the battery pack
|
||||||
uint16 manufacture_date # manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512
|
uint16 manufacture_date # manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512
|
||||||
|
|
|
@ -136,7 +136,7 @@ void BATT_SMBUS::RunImpl()
|
||||||
|
|
||||||
// Read run time to empty (minutes).
|
// Read run time to empty (minutes).
|
||||||
ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
|
ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
|
||||||
new_report.run_time_to_empty = result;
|
new_report.time_remaining_s = result * 60;
|
||||||
|
|
||||||
// Read average time to empty (minutes).
|
// Read average time to empty (minutes).
|
||||||
ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
|
ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
|
||||||
|
|
|
@ -162,7 +162,7 @@ void Batmon::RunImpl()
|
||||||
|
|
||||||
// Read run time to empty (minutes).
|
// Read run time to empty (minutes).
|
||||||
ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
|
ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
|
||||||
new_report.run_time_to_empty = result;
|
new_report.time_remaining_s = result * 60;
|
||||||
|
|
||||||
// Read average time to empty (minutes).
|
// Read average time to empty (minutes).
|
||||||
ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
|
ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
|
||||||
|
|
|
@ -83,7 +83,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
|
||||||
// battery.priority = msg.;
|
// battery.priority = msg.;
|
||||||
battery.capacity = msg.full_charge_capacity_wh;
|
battery.capacity = msg.full_charge_capacity_wh;
|
||||||
// battery.cycle_count = msg.;
|
// battery.cycle_count = msg.;
|
||||||
// battery.run_time_to_empty = msg.;
|
// battery.time_remaining_s = msg.;
|
||||||
// battery.average_time_to_empty = msg.;
|
// battery.average_time_to_empty = msg.;
|
||||||
battery.serial_number = msg.model_instance_id;
|
battery.serial_number = msg.model_instance_id;
|
||||||
battery.id = msg.getSrcNodeID().get();
|
battery.id = msg.getSrcNodeID().get();
|
||||||
|
|
|
@ -79,7 +79,7 @@ UavcanCBATBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<cuav::equip
|
||||||
// battery.priority = msg.;
|
// battery.priority = msg.;
|
||||||
battery.capacity = msg.full_charge_capacity * 1000;
|
battery.capacity = msg.full_charge_capacity * 1000;
|
||||||
battery.cycle_count = msg.cycle_count;
|
battery.cycle_count = msg.cycle_count;
|
||||||
battery.run_time_to_empty = msg.average_time_to_empty;
|
battery.time_remaining_s = msg.average_time_to_empty * 60;
|
||||||
battery.average_time_to_empty = msg.average_time_to_empty;
|
battery.average_time_to_empty = msg.average_time_to_empty;
|
||||||
battery.serial_number = msg.serial_number;
|
battery.serial_number = msg.serial_number;
|
||||||
battery.manufacture_date = msg.manufacture_date;
|
battery.manufacture_date = msg.manufacture_date;
|
||||||
|
|
|
@ -107,7 +107,7 @@ public:
|
||||||
* connected (partly)
|
* connected (partly)
|
||||||
* priority
|
* priority
|
||||||
* cycle_count
|
* cycle_count
|
||||||
* run_time_to_empty
|
* time_remaining_s
|
||||||
* average_time_to_empty
|
* average_time_to_empty
|
||||||
* manufacture_date
|
* manufacture_date
|
||||||
* max_error
|
* max_error
|
||||||
|
|
|
@ -76,7 +76,9 @@ private:
|
||||||
bat_msg.energy_consumed = -1;
|
bat_msg.energy_consumed = -1;
|
||||||
bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
|
bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
|
||||||
bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.f) : -1;
|
bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.f) : -1;
|
||||||
bat_msg.time_remaining = (battery_status.connected) ? battery_status.run_time_to_empty * 60 : 0;
|
// MAVLink extension: 0 is unsupported, in uORB it's -1
|
||||||
|
bat_msg.time_remaining = (battery_status.connected && (battery_status.time_remaining_s >= 0.f)) ?
|
||||||
|
math::max((int)battery_status.time_remaining_s, 1) : 0;
|
||||||
|
|
||||||
switch (battery_status.warning) {
|
switch (battery_status.warning) {
|
||||||
case (battery_status_s::BATTERY_WARNING_NONE):
|
case (battery_status_s::BATTERY_WARNING_NONE):
|
||||||
|
|
Loading…
Reference in New Issue