From 3fda79ef8b05493ded0037c6bc5305f2b2563f72 Mon Sep 17 00:00:00 2001 From: Jon Watson Date: Thu, 13 Apr 2017 11:37:19 -0500 Subject: [PATCH] set battery warning for MAVLink battery status (#6890) --- src/modules/mavlink/mavlink_receiver.cpp | 25 +++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 4 ++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 14a906bf88..cf1406ddb5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -147,7 +147,10 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _time_offset(0), _orb_class_instance(-1), _mom_switch_pos{}, - _mom_switch_state(0) + _mom_switch_state(0), + _p_bat_emergen_thr(param_find("BAT_EMERGEN_THR")), + _p_bat_crit_thr(param_find("BAT_CRIT_THR")), + _p_bat_low_thr(param_find("BAT_LOW_THR")) { } @@ -1360,6 +1363,26 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) battery_status.cell_count = cell_count; battery_status.connected = true; + // Get the battery level thresholds. + float bat_emergen_thr; + float bat_crit_thr; + float bat_low_thr; + param_get(_p_bat_emergen_thr, &bat_emergen_thr); + param_get(_p_bat_crit_thr, &bat_crit_thr); + param_get(_p_bat_low_thr, &bat_low_thr); + + // Set the battery warning based on remaining charge. + // Note: Smallest values must come first in evaluation. + if (battery_status.remaining < bat_emergen_thr) { + battery_status.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + + } else if (battery_status.remaining < bat_crit_thr) { + battery_status.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else if (battery_status.remaining < bat_low_thr) { + battery_status.warning = battery_status_s::BATTERY_WARNING_LOW; + } + if (_battery_pub == nullptr) { _battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 5486929795..c22c34ee78 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -252,6 +252,10 @@ private: uint8_t _mom_switch_pos[MOM_SWITCH_COUNT]; uint16_t _mom_switch_state; + param_t _p_bat_emergen_thr; + param_t _p_bat_crit_thr; + param_t _p_bat_low_thr; + /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &); MavlinkReceiver operator=(const MavlinkReceiver &);