Skip to content

Commit

Permalink
mavlink: split BATTERY_STATUS from SYS_STATUS update and handle all b…
Browse files Browse the repository at this point in the history 
…ricks (#12034)
  • Loading branch information
@dagar @bkueng
dagar authored and bkueng committed Jul 18, 2019
1 parent 51d4239 commit 9986a88
Show file tree
Hide file tree
Showing 4 changed files with 198 additions and 57 deletions.
60 changes: 47 additions & 13 deletions src/modules/mavlink/mavlink_high_latency2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))),
Expand All @@ -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),
Expand All @@ -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();
}

Expand All @@ -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();
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;
}
}
}

Expand Down Expand Up @@ -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);
}
}
}

Expand Down
15 changes: 11 additions & 4 deletions src/modules/mavlink/mavlink_high_latency2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,15 @@ class MavlinkStreamHighLatency2 : public MavlinkStream
}

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;

Expand All @@ -95,9 +104,6 @@ class MavlinkStreamHighLatency2 : public MavlinkStream
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;

Expand Down Expand Up @@ -130,7 +136,6 @@ class MavlinkStreamHighLatency2 : public MavlinkStream

SimpleAnalyzer _airspeed;
SimpleAnalyzer _airspeed_sp;
SimpleAnalyzer _battery;
SimpleAnalyzer _climb_rate;
SimpleAnalyzer _eph;
SimpleAnalyzer _epv;
Expand All @@ -143,6 +148,8 @@ class MavlinkStreamHighLatency2 : public MavlinkStream
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 &);
Expand Down
5 changes: 5 additions & 0 deletions src/modules/mavlink/mavlink_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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;


Expand All @@ -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:
Expand Down Expand Up @@ -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:
Expand Down
Loading

0 comments on commit 9986a88

Please sign in to comment.