Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mavlink split BATTERY_STATUS from SYS_STATUS update and handle all bricks #12034

Merged
merged 8 commits into from
Jul 18, 2019
Merged

Mavlink split BATTERY_STATUS from SYS_STATUS update and handle all bricks #12034

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
aa26727
Mavlink split BATTERY_STATUS from SYS_STATUS update and handle all br…
dagar Aug 3, 2018
71616e1
Added battery status stream
Jul 2, 2019
786a008
Fixed BATTERY_STATUS stream rate
Jul 2, 2019
bb97247
Publish lowest battery data to SYS_STATUS
Jul 3, 2019
2b229d9
Choose lowest battery for HIGH_LATENCY2
Jul 3, 2019
c8ef3cd
Code cleanup
Jul 9, 2019
95153d3
Fixed array bounds
Jul 10, 2019
452deb3
Code cleanup
Jul 17, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 47 additions & 13 deletions src/modules/mavlink/mavlink_high_latency2.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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