fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: split BATTERY_STATUS from SYS_STATUS update and handle all bricks (#12034)

Browse Source
This commit is contained in:
Daniel Agar
2019-07-18 13:32:41 +02:00
committed by Beat Küng
parent 51d4239eee
commit 9986a88697
4 changed files with 199 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_high_latency2.cpp
+47 -13
View File
@@ -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);
}
}
}
src/modules/mavlink/mavlink_high_latency2.h
+11 -4
View File
@@ -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 &);
src/modules/mavlink/mavlink_main.cpp
+5
View File
@@ -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:
src/modules/mavlink/mavlink_messages.cpp
+136 -41
View File
@@ -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),