fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-15 12:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink use all ORB battery_status instances instead of BOARD_NUMBER_BRICKS

Browse Source
This commit is contained in:
Daniel Agar
2019-08-03 13:36:42 -04:00
parent 991669073e
commit 483280236b
3 changed files with 54 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+47 -55
View File
@@ -543,11 +543,11 @@ public:
private:
MavlinkOrbSubscription *_status_sub;
MavlinkOrbSubscription *_cpuload_sub;
MavlinkOrbSubscription *_battery_status_sub[BOARD_NUMBER_BRICKS];
MavlinkOrbSubscription *_battery_status_sub[ORB_MULTI_MAX_INSTANCES];
uint64_t _status_timestamp{0};
uint64_t _cpuload_timestamp{0};
uint64_t _battery_status_timestamp[BOARD_NUMBER_BRICKS];
uint64_t _battery_status_timestamp[ORB_MULTI_MAX_INSTANCES] {};
/* do not allow top copying this class */
MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete;
@@ -558,7 +558,7 @@ protected:
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
_cpuload_sub(_mavlink->add_orb_subscription(ORB_ID(cpuload)))
{
for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
_battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i);
_battery_status_timestamp[i] = 0;
}
@@ -566,63 +566,55 @@ protected:
bool send(const hrt_abstime t)
{
vehicle_status_s status = {};
cpuload_s cpuload = {};
battery_status_s battery_status[BOARD_NUMBER_BRICKS] = {};
vehicle_status_s status{};
cpuload_s cpuload{};
battery_status_s battery_status[ORB_MULTI_MAX_INSTANCES] {};
const bool updated_status = _status_sub->update(&_status_timestamp, &status);
const bool updated_cpuload = _cpuload_sub->update(&_cpuload_timestamp, &cpuload);
bool updated_any = updated_status || updated_cpuload;
bool updated_battery[BOARD_NUMBER_BRICKS];
for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
updated_battery[i] = _battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status[i]);
updated_any = updated_any || updated_battery[i];
bool updated_battery = false;
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status[i])) {
updated_battery = true;
}
}
if (updated_any) {
if (updated_status || updated_cpuload || updated_battery) {
int lowest_battery_index = 0;
if (!updated_status) {
_status_sub->update(&status);
}
battery_status_s *lowest_battery = &battery_status[0];
for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
if (!updated_battery[i]) {
_battery_status_sub[i]->update(&battery_status[i]);
}
if (battery_status[i].connected && battery_status[i].remaining < lowest_battery->remaining) {
lowest_battery = &battery_status[i];
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (battery_status[i].connected && (battery_status[i].remaining < battery_status[lowest_battery_index].remaining)) {
lowest_battery_index = i;
}
}
if (!updated_cpuload) {
_cpuload_sub->update(&cpuload);
}
mavlink_sys_status_t msg = {};
mavlink_sys_status_t msg{};
msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
msg.load = cpuload.load * 1000.0f;
// TODO: Determine what data should be put here when there are multiple batteries.
// Right now, it uses the lowest battery. This is a safety decision, because if a client is only checking
// one battery using this message, it should be the lowest.
// In the future, this should somehow determine the "main" battery, or use the "type" field of BATTERY_STATUS
// to determine which battery is more important at a given time.
msg.voltage_battery = (lowest_battery->connected) ? lowest_battery->voltage_filtered_v * 1000.0f : UINT16_MAX;
msg.current_battery = (lowest_battery->connected) ? lowest_battery->current_filtered_a * 100.0f : -1;
msg.battery_remaining = (lowest_battery->connected) ? ceilf(lowest_battery->remaining * 100.0f) : -1;
// TODO: fill in something useful in the fields below
msg.drop_rate_comm = 0;
msg.errors_comm = 0;
msg.errors_count1 = 0;
msg.errors_count2 = 0;
msg.errors_count3 = 0;
msg.errors_count4 = 0;
const battery_status_s &lowest_battery = battery_status[lowest_battery_index];
if (lowest_battery.connected) {
msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f;
msg.current_battery = lowest_battery.current_filtered_a * 100.0f;
msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f);
} else {
msg.voltage_battery = UINT16_MAX;
msg.current_battery = -1;
msg.battery_remaining = -1;
}
mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg);
@@ -667,9 +659,9 @@ public:
}
private:
MavlinkOrbSubscription *_battery_status_sub[BOARD_NUMBER_BRICKS] {};
MavlinkOrbSubscription *_battery_status_sub[ORB_MULTI_MAX_INSTANCES] {};
uint64_t _battery_status_timestamp[BOARD_NUMBER_BRICKS] {};
uint64_t _battery_status_timestamp[ORB_MULTI_MAX_INSTANCES] {};
/* do not allow top copying this class */
MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete;
@@ -678,7 +670,7 @@ private:
protected:
explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink)
{
for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
_battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i);
}
}
@@ -687,13 +679,13 @@ protected:
{
bool updated = false;
for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) {
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (!_battery_status_sub[i]) {
continue;
}
battery_status_s battery_status;
battery_status_s battery_status{};
if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status)) {
/* battery status message with higher resolution */
@@ -706,18 +698,18 @@ protected:
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.0f) : -1;
// check if temperature valid
if (battery_status.connected && PX4_ISFINITE(battery_status.temperature)) {
bat_msg.temperature = battery_status.temperature * 100.0f;
bool temperature_valid = battery_status.connected && PX4_ISFINITE(battery_status.temperature);
bat_msg.temperature = temperature_valid ? (int16_t)(battery_status.temperature * 100.0f) : INT16_MAX;
//bat_msg.average_current_battery = (battery_status.connected) ? battery_status.average_current_a * 100.0f : -1;
//bat_msg.serial_number = (battery_status.connected) ? battery_status.serial_number : 0;
//bat_msg.capacity = (battery_status.connected) ? battery_status.capacity : 0;
//bat_msg.cycle_count = (battery_status.connected) ? battery_status.cycle_count : UINT16_MAX;
//bat_msg.run_time_to_empty = (battery_status.connected) ? battery_status.run_time_to_empty * 60 : 0;
//bat_msg.average_time_to_empty = (battery_status.connected) ? battery_status.average_time_to_empty * 60 : 0;
} else {
bat_msg.temperature = INT16_MAX;
}
for (unsigned cell = 0; cell < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); cell++) {
if ((int32_t)cell < battery_status.cell_count && battery_status.connected) {
static constexpr int mavlink_cells_max = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
for (int cell = 0; cell < mavlink_cells_max; cell++) {
if ((battery_status.cell_count > 0) && (cell < battery_status.cell_count) && battery_status.connected) {
bat_msg.voltages[cell] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f;
} else {