diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index 1506cf9aec..ac2859f1d2 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -70,8 +70,6 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink _airspeed_time(0), _attitude_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), _attitude_sp_time(0), - _battery_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))), - _battery_time(0), _estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), _estimator_status_time(0), _pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))), @@ -94,7 +92,6 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink _wind_time(0), _airspeed(SimpleAnalyzer::AVERAGE), _airspeed_sp(SimpleAnalyzer::AVERAGE), - _battery(SimpleAnalyzer::AVERAGE), _climb_rate(SimpleAnalyzer::MAX), _eph(SimpleAnalyzer::MAX), _epv(SimpleAnalyzer::MAX), @@ -103,6 +100,11 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink _throttle(SimpleAnalyzer::AVERAGE), _windspeed(SimpleAnalyzer::AVERAGE) { + + for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + _batteries[i].subscription = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); + } + reset_last_sent(); } @@ -116,7 +118,11 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) bool updated = _airspeed.valid(); updated |= _airspeed_sp.valid(); - updated |= _battery.valid(); + + for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + updated |= _batteries[i].analyzer.valid(); + } + updated |= _climb_rate.valid(); updated |= _eph.valid(); updated |= _epv.valid(); @@ -151,10 +157,27 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) _airspeed_sp.get_scaled(msg.airspeed_sp, 5.0f); } - if (_battery.valid()) { - _battery.get_scaled(msg.battery, 100.0f); + int lowest = 0; + + for (int i = 1; i < BOARD_NUMBER_BRICKS; 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); + + if (battery_connected && battery_is_lowest) { + lowest = i; + } + } + if (_batteries[lowest].connected) { + _batteries[lowest].analyzer.get_scaled(msg.battery, 100.0f); + + } else { + msg.battery = -1; + } + + if (_climb_rate.valid()) { _climb_rate.get_scaled(msg.climb_rate, 10.0f); } @@ -206,7 +229,11 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t) // reset the analyzers _airspeed.reset(); _airspeed_sp.reset(); - _battery.reset(); + + for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + _batteries[i].analyzer.reset(); + } + _climb_rate.reset(); _eph.reset(); _epv.reset(); @@ -249,12 +276,16 @@ bool MavlinkStreamHighLatency2::write_attitude_sp(mavlink_high_latency2_t *msg) bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *msg) { struct battery_status_s battery; + bool updated = false; - const bool updated = _battery_sub->update(&_battery_time, &battery); + for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + if (_batteries[i].subscription->update(&_batteries[i].timestamp, &battery)) { + updated = true; + _batteries[i].connected = battery.connected; - if (_battery_time > 0) { - if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) { - msg->failure_flags |= HL_FAILURE_FLAG_BATTERY; + if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) { + msg->failure_flags |= HL_FAILURE_FLAG_BATTERY; + } } } @@ -504,8 +535,11 @@ void MavlinkStreamHighLatency2::update_battery_status() { battery_status_s battery; - if (_battery_sub->update(&battery)) { - _battery.add_value(battery.remaining, _update_rate_filtered); + for (int i = 0; i < BOARD_NUMBER_BRICKS; 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 052cb63c93..9de3be2f9b 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -83,6 +83,15 @@ public: } private: + + struct PerBatteryData { + PerBatteryData() {} + MavlinkOrbSubscription *subscription; + SimpleAnalyzer analyzer{SimpleAnalyzer::AVERAGE}; + uint64_t timestamp{0}; + bool connected{false}; + }; + MavlinkOrbSubscription *_actuator_sub_0; uint64_t _actuator_time_0; @@ -95,9 +104,6 @@ private: MavlinkOrbSubscription *_attitude_sp_sub; uint64_t _attitude_sp_time; - MavlinkOrbSubscription *_battery_sub; - uint64_t _battery_time; - MavlinkOrbSubscription *_estimator_status_sub; uint64_t _estimator_status_time; @@ -130,7 +136,6 @@ private: SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; - SimpleAnalyzer _battery; SimpleAnalyzer _climb_rate; SimpleAnalyzer _eph; SimpleAnalyzer _epv; @@ -143,6 +148,8 @@ private: hrt_abstime _last_update_time = 0; float _update_rate_filtered = 0.0f; + PerBatteryData _batteries[BOARD_NUMBER_BRICKS]; + /* do not allow top copying this class */ MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &); MavlinkStreamHighLatency2 &operator = (const MavlinkStreamHighLatency2 &); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 27c676c683..946aa8cce4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1633,6 +1633,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("WIND_COV", 1.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); break; case MAVLINK_MODE_ONBOARD: @@ -1675,6 +1676,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("WIND_COV", 10.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); break; case MAVLINK_MODE_EXTVISION: @@ -1716,6 +1718,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("WIND_COV", 1.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); break; @@ -1734,6 +1737,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("VFR_HUD", 25.0f); configure_stream_local("WIND_COV", 2.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); break; case MAVLINK_MODE_MAGIC: @@ -1786,6 +1790,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 20.0f); configure_stream_local("WIND_COV", 10.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); break; case MAVLINK_MODE_IRIDIUM: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 45e2e3dc64..64555ec531 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -542,11 +542,11 @@ public: private: MavlinkOrbSubscription *_status_sub; MavlinkOrbSubscription *_cpuload_sub; - MavlinkOrbSubscription *_battery_status_sub; + MavlinkOrbSubscription *_battery_status_sub[BOARD_NUMBER_BRICKS]; uint64_t _status_timestamp{0}; uint64_t _cpuload_timestamp{0}; - uint64_t _battery_status_timestamp{0}; + uint64_t _battery_status_timestamp[BOARD_NUMBER_BRICKS]; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete; @@ -555,28 +555,46 @@ private: protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), - _cpuload_sub(_mavlink->add_orb_subscription(ORB_ID(cpuload))), - _battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))) - {} + _cpuload_sub(_mavlink->add_orb_subscription(ORB_ID(cpuload))) + { + for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + _battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); + _battery_status_timestamp[i] = 0; + } + } bool send(const hrt_abstime t) { vehicle_status_s status = {}; cpuload_s cpuload = {}; - battery_status_s battery_status = {}; + battery_status_s battery_status[BOARD_NUMBER_BRICKS] = {}; const bool updated_status = _status_sub->update(&_status_timestamp, &status); const bool updated_cpuload = _cpuload_sub->update(&_cpuload_timestamp, &cpuload); - const bool updated_battery = _battery_status_sub->update(&_battery_status_timestamp, &battery_status); + bool updated_any = updated_status || updated_cpuload; + bool updated_battery[BOARD_NUMBER_BRICKS]; - if (updated_status || updated_battery || updated_cpuload) { + 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]; + } + + if (updated_any) { if (!updated_status) { _status_sub->update(&status); } - if (!updated_battery) { - _battery_status_sub->update(&battery_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]; + } } if (!updated_cpuload) { @@ -589,9 +607,14 @@ protected: 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; - msg.voltage_battery = (battery_status.connected) ? battery_status.voltage_filtered_v * 1000.0f : UINT16_MAX; - msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100.0f : -1; - msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.0f) : -1; + // 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; @@ -602,34 +625,6 @@ protected: mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg); - /* battery status message with higher resolution */ - mavlink_battery_status_t bat_msg = {}; - bat_msg.id = 0; - bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL; - bat_msg.type = MAV_BATTERY_TYPE_LIPO; - bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1; - bat_msg.energy_consumed = -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.0f) : -1; - bat_msg.temperature = (battery_status.connected) ? (int16_t)battery_status.temperature : 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; - - for (unsigned int i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) { - if ((int)i < battery_status.cell_count && battery_status.connected) { - bat_msg.voltages[i] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f; - - } else { - bat_msg.voltages[i] = UINT16_MAX; - } - } - - mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg); - return true; } @@ -637,6 +632,105 @@ protected: } }; +class MavlinkStreamBatteryStatus : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamBatteryStatus::get_name_static(); + } + + static const char *get_name_static() + { + return "BATTERY_STATUS"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_BATTERY_STATUS; + } + + uint16_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamBatteryStatus(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_battery_status_sub[BOARD_NUMBER_BRICKS] {}; + + uint64_t _battery_status_timestamp[BOARD_NUMBER_BRICKS] {}; + + /* do not allow top copying this class */ + MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete; + MavlinkStreamBatteryStatus &operator = (const MavlinkStreamSysStatus &) = delete; + +protected: + explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + _battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); + } + } + + bool send(const hrt_abstime t) + { + bool updated = false; + + for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + + if (!_battery_status_sub[i]) { + continue; + } + + battery_status_s battery_status; + + if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status)) { + /* battery status message with higher resolution */ + mavlink_battery_status_t bat_msg{}; + bat_msg.id = i; + bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL; + bat_msg.type = MAV_BATTERY_TYPE_LIPO; + bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1; + bat_msg.energy_consumed = -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.0f) : -1; + bat_msg.temperature = (battery_status.connected) ? (int16_t)battery_status.temperature : 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; + + 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) { + bat_msg.voltages[cell] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f; + + } else { + bat_msg.voltages[cell] = UINT16_MAX; + } + } + + mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg); + + updated = true; + } + + } + + return updated; + } +}; class MavlinkStreamHighresIMU : public MavlinkStream { @@ -4914,6 +5008,7 @@ static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static), StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static, &MavlinkStreamCommandLong::get_id_static), StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static, &MavlinkStreamSysStatus::get_id_static), + StreamListItem(&MavlinkStreamBatteryStatus::new_instance, &MavlinkStreamBatteryStatus::get_name_static, &MavlinkStreamBatteryStatus::get_id_static), StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static, &MavlinkStreamHighresIMU::get_id_static), StreamListItem(&MavlinkStreamScaledIMU::new_instance, &MavlinkStreamScaledIMU::get_name_static, &MavlinkStreamScaledIMU::get_id_static), StreamListItem(&MavlinkStreamScaledIMU2::new_instance, &MavlinkStreamScaledIMU2::get_name_static, &MavlinkStreamScaledIMU2::get_id_static),