From 483280236ba80ecc265145f42127651d53d8193c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 3 Aug 2019 12:31:19 -0400 Subject: [PATCH] mavlink use all ORB battery_status instances instead of BOARD_NUMBER_BRICKS --- src/modules/mavlink/mavlink_high_latency2.cpp | 12 +-- src/modules/mavlink/mavlink_high_latency2.h | 2 +- src/modules/mavlink/mavlink_messages.cpp | 102 ++++++++---------- 3 files changed, 54 insertions(+), 62 deletions(-) diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index ac2859f1d2..7723f653b9 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -101,7 +101,7 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink _windspeed(SimpleAnalyzer::AVERAGE) { - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { _batteries[i].subscription = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); } @@ -119,7 +119,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) bool updated = _airspeed.valid(); updated |= _airspeed_sp.valid(); - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { updated |= _batteries[i].analyzer.valid(); } @@ -159,7 +159,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) int lowest = 0; - for (int i = 1; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid(); const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled( 100.0f); @@ -230,7 +230,7 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t) _airspeed.reset(); _airspeed_sp.reset(); - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { _batteries[i].analyzer.reset(); } @@ -278,7 +278,7 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms struct battery_status_s battery; bool updated = false; - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (_batteries[i].subscription->update(&_batteries[i].timestamp, &battery)) { updated = true; _batteries[i].connected = battery.connected; @@ -535,7 +535,7 @@ void MavlinkStreamHighLatency2::update_battery_status() { battery_status_s battery; - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (_batteries[i].subscription->update(&battery)) { _batteries[i].connected = battery.connected; _batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered); diff --git a/src/modules/mavlink/mavlink_high_latency2.h b/src/modules/mavlink/mavlink_high_latency2.h index 9de3be2f9b..bf9d63c44b 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -148,7 +148,7 @@ private: hrt_abstime _last_update_time = 0; float _update_rate_filtered = 0.0f; - PerBatteryData _batteries[BOARD_NUMBER_BRICKS]; + PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES]; /* do not allow top copying this class */ MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index de24336515..56345cb8e0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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 {