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

commander: move battery calculations to systemlib

Browse Source 
The commander used to consume the battery_status topic and write the
contents after some calculations into the system state. Instead, the
calculations now happen in library calls in systemlib/battery.

This moves the battery fields out of the vehicle_status message into the
battery_status topic.

This brought quite some changes in all modules that need battery
information. The current state is compiling but untested.
This commit is contained in:
Julian Oes
2016-04-11 18:01:47 +02:00
parent 699b08c9fd
commit 32c3135788
21 changed files with 661 additions and 426 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+71 -81
View File
@@ -255,7 +255,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
*/
int commander_thread_main(int argc, char *argv[]);
void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
battery_status_s *battery);
void get_circuit_breaker_params();
@@ -1224,10 +1225,6 @@ int commander_thread_main(int argc, char *argv[])
mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BUZZER INIT FAIL");
}
if (battery_init() != OK) {
mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BATTERY INIT FAIL");
}
/* vehicle status topic */
memset(&status, 0, sizeof(status));
status.condition_landed = true; // initialize to safe value
@@ -1255,10 +1252,6 @@ int commander_thread_main(int argc, char *argv[])
status.offboard_control_signal_lost = true;
status.data_link_lost = true;
/* set battery warning flag */
battery_warning = vehicle_battery_warning::NONE;
status.condition_battery_voltage_valid = false;
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
@@ -1471,7 +1464,7 @@ int commander_thread_main(int argc, char *argv[])
vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing
control_status_leds(&status, &armed, true);
control_status_leds(&status, &armed, true, &battery);
/* now initialized */
commander_initialized = true;
@@ -1967,20 +1960,37 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.battery_discharged_mah = battery.discharged_mah;
status.condition_battery_voltage_valid = true;
status.battery_cell_count = battery_get_n_cells();
if (hrt_absolute_time() > commander_boot_timestamp + 2000000
&& battery.voltage_filtered_v > FLT_EPSILON) {
/* get throttle (if armed), as we only care about energy negative throttle also counts */
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah,
throttle);
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (battery.warning == battery_status_s::BATTERY_WARNING_LOW &&
!low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
if (armed.armed) {
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED");
} else {
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED");
}
} else if (!status.usb_connected &&
battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL &&
!critical_battery_voltage_actions_done &&
low_battery_voltage_actions_done) {
critical_battery_voltage_actions_done = true;
if (armed.armed) {
mavlink_and_console_log_critical(&mavlink_log_pub, "CRITICAL BATTERY, SHUT SYSTEM DOWN");
} else {
mavlink_and_console_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LAND IMMEDIATELY");
}
status_changed = true;
}
/* End battery voltage check */
}
}
@@ -2037,34 +2047,6 @@ int commander_thread_main(int argc, char *argv[])
last_idle_time = system_load.tasks[0].total_runtime;
}
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
if (armed.armed) {
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED");
} else {
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED");
}
battery_warning = vehicle_battery_warning::LOW;
status_changed = true;
} else if (!status.usb_connected && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
&& !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
battery_warning = vehicle_battery_warning::CRITICAL;
if (armed.armed) {
mavlink_and_console_log_critical(&mavlink_log_pub, "CRITICAL BATTERY, SHUT SYSTEM DOWN");
} else {
mavlink_and_console_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LAND IMMEDIATELY");
}
status_changed = true;
}
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status,
@@ -2518,33 +2500,41 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* Check engine failure
* only for fixed wing for now
*/
if (!circuit_breaker_engaged_enginefailure_check &&
status.is_rotary_wing == false &&
armed.armed &&
((actuator_controls.control[3] > ef_throttle_thres &&
battery.current_a / actuator_controls.control[3] <
ef_current2throttle_thres) ||
(status.engine_failure))) {
/* potential failure, measure time */
if (timestamp_engine_healthy > 0 &&
hrt_elapsed_time(&timestamp_engine_healthy) >
ef_time_thres * 1e6 &&
!status.engine_failure) {
status.engine_failure = true;
status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "Engine Failure");
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(actuator_controls_sub, &updated);
} else {
/* no failure reset flag */
timestamp_engine_healthy = hrt_absolute_time();
if (updated) {
/* got command */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
if (status.engine_failure) {
status.engine_failure = false;
status_changed = true;
/* Check engine failure
* only for fixed wing for now
*/
if (!circuit_breaker_engaged_enginefailure_check &&
status.is_rotary_wing == false &&
armed.armed &&
((actuator_controls.control[3] > ef_throttle_thres &&
battery.current_a / actuator_controls.control[3] <
ef_current2throttle_thres) ||
(status.engine_failure))) {
/* potential failure, measure time */
if (timestamp_engine_healthy > 0 &&
hrt_elapsed_time(&timestamp_engine_healthy) >
ef_time_thres * 1e6 &&
!status.engine_failure) {
status.engine_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "Engine Failure");
}
} else {
/* no failure reset flag */
timestamp_engine_healthy = hrt_absolute_time();
if (status.engine_failure) {
status.engine_failure = false;
status_changed = true;
}
}
}
@@ -2711,12 +2701,12 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = true;
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
battery_warning == vehicle_battery_warning::CRITICAL) {
(battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
/* play tune on battery critical */
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(battery_warning == vehicle_battery_warning::LOW || status.failsafe)) {
(battery.warning == battery_status_s::BATTERY_WARNING_LOW)) {
/* play tune on battery warning or failsafe */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
@@ -2758,12 +2748,12 @@ int commander_thread_main(int argc, char *argv[])
/* blinking LED message, don't touch LEDs */
if (blink_state == 2) {
/* blinking LED message completed, restore normal state */
control_status_leds(&status, &armed, true);
control_status_leds(&status, &armed, true, &battery);
}
} else {
/* normal state */
control_status_leds(&status, &armed, status_changed);
control_status_leds(&status, &armed, status_changed, &battery);
}
status_changed = false;
@@ -2829,7 +2819,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
}
void
control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed)
control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery)
{
/* driving rgbled */
if (changed) {
@@ -2862,9 +2852,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
/* set color */
if (status_local->failsafe) {
rgbled_set_color(RGBLED_COLOR_PURPLE);
} else if (battery_warning == vehicle_battery_warning::LOW) {
} else if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) {
rgbled_set_color(RGBLED_COLOR_AMBER);
} else if (battery_warning == vehicle_battery_warning::CRITICAL) {
} else if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
rgbled_set_color(RGBLED_COLOR_RED);
} else {
if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {
src/modules/commander/commander_helper.cpp
-72
View File
@@ -105,30 +105,6 @@ static DevHandle h_leds;
static DevHandle h_rgbleds;
static DevHandle h_buzzer;
static param_t bat_v_empty_h;
static param_t bat_v_full_h;
static param_t bat_n_cells_h;
static param_t bat_capacity_h;
static param_t bat_v_load_drop_h;
static float bat_v_empty = 3.4f;
static float bat_v_full = 4.2f;
static float bat_v_load_drop = 0.06f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static unsigned int counter = 0;
static float throttle_lowpassed = 0.0f;
int battery_init()
{
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_CHARGED");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
return PX4_OK;
}
int buzzer_init()
{
tune_end = 0;
@@ -346,51 +322,3 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern);
}
unsigned battery_get_n_cells() {
return bat_n_cells;
}
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
{
float ret = 0;
if (counter % 100 == 0) {
param_get(bat_v_empty_h, &bat_v_empty);
param_get(bat_v_full_h, &bat_v_full);
param_get(bat_v_load_drop_h, &bat_v_load_drop);
param_get(bat_n_cells_h, &bat_n_cells);
param_get(bat_capacity_h, &bat_capacity);
}
counter++;
// XXX this time constant needs to become tunable
// but really, the right fix are smart batteries.
float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f;
if (PX4_ISFINITE(val)) {
throttle_lowpassed = val;
}
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed);
/* the range from full to empty is the same for batteries under load and without load,
* since the voltage drop applies to both the full and empty state
*/
float voltage_range = (bat_v_full - bat_v_empty);
float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range);
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;
}
/* limit to sane values */
ret = (ret < 0.0f) ? 0.0f : ret;
ret = (ret > 1.0f) ? 1.0f : ret;
return ret;
}
src/modules/commander/commander_helper.h
-17
View File
@@ -77,21 +77,4 @@ void rgbled_set_color(rgbled_color_t color);
void rgbled_set_mode(rgbled_mode_t mode);
void rgbled_set_pattern(rgbled_pattern_t *pattern);
int battery_init();
/**
* Estimate remaining battery charge.
*
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
* else use simple estimate based on voltage.
*
* @param voltage the current battery voltage
* @param discharged the discharged capacity
* @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
unsigned battery_get_n_cells();
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander_params.c
+1 -81
View File
@@ -93,87 +93,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
/**
* Empty cell voltage.
*
* Defines the voltage where a single cell of the battery is considered empty.
*
* @group Battery Calibration
* @unit V
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
/**
* Full cell voltage.
*
* Defines the voltage where a single cell of the battery is considered full.
*
* @group Battery Calibration
* @unit V
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
/**
* Voltage drop per cell on 100% load
*
* This implicitely defines the internal resistance
* to maximum current ratio and assumes linearity.
*
* @group Battery Calibration
* @unit V
* @min 0.0
* @max 1.5
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
/**
* Number of cells.
*
* Defines the number of cells the attached battery consists of.
*
* @group Battery Calibration
* @unit S
* @min 2
* @max 10
* @value 2 2S Battery
* @value 3 3S Battery
* @value 4 4S Battery
* @value 5 5S Battery
* @value 6 6S Battery
* @value 7 7S Battery
* @value 8 8S Battery
* @value 9 9S Battery
* @value 10 10S Battery
* @value 11 11S Battery
* @value 12 12S Battery
* @value 13 13S Battery
* @value 14 14S Battery
* @value 15 15S Battery
* @value 16 16S Battery
*/
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
/**
* Battery capacity.
*
* Defines the capacity of the attached battery.
*
* @group Battery Calibration
* @unit mA
* @decimal 0
* @min -1.0
* @max 100000
* @increment 50
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
/**
* Datalink loss failsafe.
* Datalink loss mode enabled.
*
* Set to 1 to enable actions triggered when the datalink is lost.
*
src/modules/gpio_led/gpio_led.c
+26 -10
View File
@@ -51,6 +51,7 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
@@ -60,8 +61,10 @@ struct gpio_led_s {
int gpio_fd;
bool use_io;
int pin;
struct vehicle_status_s status;
struct vehicle_status_s vehicle_status;
struct battery_status_s battery_status;
int vehicle_status_sub;
int battery_status_sub;
bool led_state;
int counter;
};
@@ -238,11 +241,17 @@ void gpio_led_start(FAR void *arg)
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* initialize vehicle status structure */
memset(&priv->status, 0, sizeof(priv->status));
memset(&priv->vehicle_status, 0, sizeof(priv->vehicle_status));
/* initialize battery status structure */
memset(&priv->battery_status, 0, sizeof(priv->battery_status));
/* subscribe to vehicle status topic */
priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to battery status topic */
priv->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
/* add worker to queue */
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
@@ -258,32 +267,39 @@ void gpio_led_cycle(FAR void *arg)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
/* check for status updates*/
/* check for vehicle status updates*/
bool updated;
orb_check(priv->vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->vehicle_status);
}
/* select pattern for current status */
orb_check(priv->battery_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(battery_status), priv->battery_status_sub, &priv->battery_status);
}
/* select pattern for current vehiclestatus */
int pattern = 0;
if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
if (priv->vehicle_status.arming_state == ARMING_STATE_ARMED_ERROR) {
pattern = 0x2A; // *_*_*_ fast blink (armed, error)
} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {
} else if (priv->vehicle_status.arming_state == ARMING_STATE_ARMED) {
if (priv->battery_status.warning == BATTERY_WARNING_NONE
&& !priv->vehicle_status.failsafe) {
pattern = 0x3f; // ****** solid (armed)
} else {
pattern = 0x3e; // *****_ slow blink (armed, battery low or failsafe)
}
} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
} else if (priv->vehicle_status.arming_state == ARMING_STATE_STANDBY) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
} else if (priv->vehicle_status.arming_state == ARMING_STATE_STANDBY_ERROR) {
pattern = 0x28; // *_*___ slow double blink (disarmed, error)
}
src/modules/mavlink/mavlink_messages.cpp
+15 -19
View File
@@ -557,6 +557,7 @@ public:
private:
MavlinkOrbSubscription *_status_sub;
MavlinkOrbSubscription *_battery_status_sub;
/* do not allow top copying this class */
MavlinkStreamSysStatus(MavlinkStreamSysStatus &);
@@ -564,24 +565,28 @@ private:
protected:
explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
_battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status)))
{}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct battery_status_s battery_status;
if (_status_sub->update(&status)) {
bool updated = _status_sub->update(&status);
updated = (updated || _battery_status_sub->update(&battery_status));
if (updated) {
mavlink_sys_status_t msg;
msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
msg.load = status.load * 1000.0f;
msg.voltage_battery = status.battery_voltage * 1000.0f;
msg.current_battery = status.battery_current * 100.0f;
msg.battery_remaining = (status.condition_battery_voltage_valid) ?
status.battery_remaining * 100.0f : -1;
msg.voltage_battery = battery_status.voltage_v * 1000.0f;
msg.current_battery = battery_status.current_a * 100.0f;
msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;
// TODO: fill in something useful in the fields below
msg.drop_rate_comm = 0;
msg.errors_comm = 0;
@@ -595,28 +600,19 @@ protected:
/* battery status message with higher resolution */
mavlink_battery_status_t bat_msg;
bat_msg.id = 0;
bat_msg.id = 0;
bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL;
bat_msg.type = MAV_BATTERY_TYPE_LIPO;
bat_msg.temperature = INT16_MAX;
for (unsigned i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) {
if (i < status.battery_cell_count) {
bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f;
if (i < battery_status.cell_count) {
bat_msg.voltages[i] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f;
} else {
bat_msg.voltages[i] = UINT16_MAX;
}
}
if (status.condition_battery_voltage_valid) {
bat_msg.current_battery = (bat_msg.current_battery >= 0.0f) ?
status.battery_current * 100.0f : -1;
bat_msg.current_consumed = (bat_msg.current_consumed >= 0.0f) ?
status.battery_discharged_mah : -1;
bat_msg.battery_remaining = status.battery_remaining * 100.0f;
} else {
bat_msg.current_battery = -1.0f;
bat_msg.current_consumed = -1.0f;
bat_msg.battery_remaining = -1.0f;
}
// TODO: calculate this
bat_msg.energy_consumed = -1.0f;
_mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg);
src/modules/sdlog2/sdlog2.c
+12 -2
View File
@@ -1447,8 +1447,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.main_state = buf_status.main_state;
log_msg.body.log_STAT.arming_state = buf_status.arming_state;
log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
log_msg.body.log_STAT.load = buf_status.load;
LOGBUFFER_WRITE_AND_COUNT(STAT);
@@ -1794,6 +1792,18 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
/* --- BATTERY --- */
if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) {
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
log_msg.body.log_BATT.current = buf.battery.current_a;
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
log_msg.body.log_BATT.remaining = buf.battery.remaining;
log_msg.body.log_BATT.warning = buf.battery.warning;
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), &subs.triplet_sub, &buf.triplet)) {
src/modules/sdlog2/sdlog2_messages.h
+4 -4
View File
@@ -180,8 +180,6 @@ struct log_STAT_s {
uint8_t main_state;
uint8_t arming_state;
uint8_t failsafe;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
float load;
};
@@ -295,6 +293,8 @@ struct log_BATT_s {
float voltage_filtered;
float current;
float discharged;
float remaining;
uint8_t warning;
};
/* --- DIST - RANGE SENSOR DISTANCE --- */
@@ -623,7 +623,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
LOG_FORMAT(STAT, "BBBBf", "MainState,ArmS,Failsafe,Landed,Load"),
LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
@@ -636,7 +636,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(BATT, "fffffB", "V,VFilt,C,Discharged,Remaining,Warning"),
LOG_FORMAT(DIST, "BBBff", "Id,Type,Orientation,Distance,Covariance"),
LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
src/modules/sensors/sensors.cpp
+25 -55
View File
@@ -83,6 +83,7 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/battery.h>
#include <conversion/rotation.h>
#include <systemlib/airspeed.h>
@@ -128,9 +129,6 @@ using namespace DriverFramework;
*/
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 2.1f
/**
* HACK - true temperature is much less than indicated temperature in baro,
* subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
@@ -257,6 +255,8 @@ private:
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
Battery _battery; /**< Helper lib to publish battery_status topic. */
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
@@ -1518,6 +1518,7 @@ Sensors::parameter_update_poll(bool forced)
/* do not output this for now, as its covered in preflight checks */
// warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
_battery.updateParams();
}
}
@@ -1644,6 +1645,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* read all channels available */
int ret = _h_adc.read(&buf_adc, sizeof(buf_adc));
float bat_voltage_v = 0.0f;
float bat_current_a = 0.0f;
bool updated_battery = false;
if (ret >= (int)sizeof(buf_adc[0])) {
/* Read add channels we got */
@@ -1657,48 +1662,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* look for specific channels and process the raw voltage to measurement data */
if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
if (voltage > BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_v = voltage;
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_filtered_v = voltage;
}
_battery_status.timestamp = t;
_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
} else {
/* mark status as invalid */
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
}
bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
updated_battery = true;
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
/* handle current only if voltage is valid */
if (_battery_status.voltage_v > 0.0f) {
float current = ((buf_adc[i].am_data - _parameters.battery_current_offset) * _parameters.battery_current_scaling);
/* check measured current value */
if (current >= 0.0f) {
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
/* initialize discharged value */
if (_battery_status.discharged_mah < 0.0f) {
_battery_status.discharged_mah = 0.0f;
}
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
}
}
}
_battery_current_timestamp = t;
bat_current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling);
updated_battery = true;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
@@ -1735,16 +1704,21 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
if (updated_battery) {
// XXX TODO: throttle is hardcoded here. The dependency to throttle would need to be
// removed, or it needs to be subscribed to actuator controls.
const float throttle = 0.0f;
_battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, throttle, &_battery_status);
}
_last_adc = t;
if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
/* announce the battery status if needed, just publish else */
if (_battery_pub != nullptr) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
/* announce the battery status if needed, just publish else */
if (_battery_pub != nullptr) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
}
@@ -2158,11 +2132,7 @@ Sensors::task_main()
raw.adc_voltage_v[2] = 0.0f;
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
_battery.reset(&_battery_status);
/* get a set of initial values */
accel_poll(raw);
src/modules/systemlib/CMakeLists.txt
+4 -3
View File
@@ -48,6 +48,7 @@ set(SRCS
mcu_version.c
bson/tinybson.c
circuit_breaker.cpp
battery.cpp
)
if(${OS} STREQUAL "nuttx")
@@ -55,7 +56,7 @@ if(${OS} STREQUAL "nuttx")
err.c
printload.c
param/param.c
up_cxxinitialize.c
up_cxxinitialize.c
)
elseif ("${CONFIG_SHMEM}" STREQUAL "1")
list(APPEND SRCS
@@ -66,7 +67,7 @@ else()
list(APPEND SRCS
param/param.c
print_load_posix.c
)
)
endif()
if(NOT ${OS} STREQUAL "qurt")
@@ -85,4 +86,4 @@ px4_add_module(
platforms__common
modules__param
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/systemlib/battery.cpp
+162
View File
@@ -0,0 +1,162 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file battery.cpp
*
* Library calls for battery functionality.
*
* @author Julian Oes <julian@oes.ch>
*/
#include "battery.h"
Battery::Battery() :
SuperBlock(NULL, NULL),
_param_v_empty(this, "BAT_V_EMPTY", false),
_param_v_full(this, "BAT_V_CHARGED", false),
_param_n_cells(this, "BAT_N_CELLS", false),
_param_capacity(this, "BAT_CAPACITY", false),
_param_v_load_drop(this, "BAT_V_LOAD_DROP", false),
_voltage_filtered_v(0.0f),
_throttle_filtered(0.0f),
_discharged_mah(0.0f),
_remaining(1.0f),
_warning(battery_status_s::BATTERY_WARNING_NONE),
_last_timestamp(0)
{
/* load initial params */
updateParams();
}
Battery::~Battery()
{
}
void
Battery::reset(battery_status_s *battery_status)
{
battery_status->voltage_v = 0.0f;
battery_status->voltage_filtered_v = 0.0f;
battery_status->current_a = -1.0f;
battery_status->discharged_mah = -1.0f;
battery_status->cell_count = _param_n_cells.get();
// TODO: check if it is sane to reset warning to NONE
battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
}
void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
battery_status_s *battery_status)
{
filterVoltage(voltage_v);
sumDischarged(timestamp, current_a);
estimateRemaining(voltage_v, throttle_normalized);
determineWarning();
if (_voltage_filtered_v > 2.1f) {
battery_status->voltage_v = voltage_v;
battery_status->voltage_filtered_v = _voltage_filtered_v;
battery_status->current_a = current_a;
battery_status->discharged_mah = _discharged_mah;
battery_status->cell_count = _param_n_cells.get();
battery_status->warning = _warning;
} else {
reset(battery_status);
}
}
void
Battery::filterVoltage(float voltage_v)
{
// TODO: inspect that filter performance
const float filtered_next = _voltage_filtered_v * 0.999f + voltage_v * 0.001f;
if (PX4_ISFINITE(filtered_next)) {
_voltage_filtered_v = filtered_next;
}
}
void
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
{
// Ignore first update because we don't know dT.
if (_last_timestamp != 0) {
_discharged_mah = current_a * (timestamp - _last_timestamp) * 1.0e-3f / 3600.0f;
}
_last_timestamp = timestamp;
}
void
Battery::estimateRemaining(float voltage_v, float throttle_normalized)
{
// XXX this time constant needs to become tunable but really, the right fix are smart batteries.
const float filtered_next = _throttle_filtered * 0.97f + throttle_normalized * 0.03f;
if (PX4_ISFINITE(filtered_next)) {
_throttle_filtered = filtered_next;
}
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
const float bat_v_empty_dynamic = _param_v_empty.get() - (_param_v_load_drop.get() * _throttle_filtered);
/* the range from full to empty is the same for batteries under load and without load,
* since the voltage drop applies to both the full and empty state
*/
const float voltage_range = (_param_v_full.get() - _param_v_empty.get());
const float remaining_voltage = (voltage_v - (_param_n_cells.get() * bat_v_empty_dynamic))
/ (_param_n_cells.get() * voltage_range);
if (_param_capacity.get() > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
_remaining = fminf(remaining_voltage, 1.0f - _discharged_mah / _param_capacity.get());
} else {
/* else use voltage */
_remaining = remaining_voltage;
}
/* limit to sane values */
_remaining = (_remaining < 0.0f) ? 0.0f : _remaining;
_remaining = (_remaining > 1.0f) ? 1.0f : _remaining;
}
void
Battery::determineWarning()
{
// TODO: Determine threshold or make params.
if (_remaining < 0.18f) {
_warning = battery_status_s::BATTERY_WARNING_LOW;
} else if (_remaining < 0.09f) {
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
}
}
src/modules/systemlib/battery.h
+97
View File
@@ -0,0 +1,97 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file battery.h
*
* Library calls for battery functionality.
*
* @author Julian Oes <julian@oes.ch>
*/
#pragma once
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/topics/battery_status.h>
#include <drivers/drv_hrt.h>
class Battery : public control::SuperBlock
{
public:
/**
* Constructor
*/
Battery();
/**
* Destructor
*/
~Battery();
/**
* Reset all battery stats and report invalid/nothing.
*/
void reset(battery_status_s *battery_status);
/**
* Update current battery status message.
*
* @param voltage_v: current voltage in V
* @param current_a: current current in A
* @param throttle_normalized: throttle from 0 to 1
*/
void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
battery_status_s *status);
private:
void filterVoltage(float voltage_v);
void sumDischarged(hrt_abstime timestamp, float current_a);
void estimateRemaining(float voltage_v, float throttle_normalized);
void determineWarning();
control::BlockParamFloat _param_v_empty;
control::BlockParamFloat _param_v_full;
control::BlockParamFloat _param_n_cells;
control::BlockParamFloat _param_capacity;
control::BlockParamFloat _param_v_load_drop;
float _voltage_filtered_v;
float _throttle_filtered;
float _discharged_mah;
float _remaining;
uint8_t _warning;
hrt_abstime _last_timestamp;
};
src/modules/systemlib/battery_params.c
+123
View File
@@ -0,0 +1,123 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file battery_params.c
*
* Parameters defined by the battery lib.
*
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/**
* Empty cell voltage.
*
* Defines the voltage where a single cell of the battery is considered empty.
*
* @group Battery Calibration
* @unit V
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
/**
* Full cell voltage.
*
* Defines the voltage where a single cell of the battery is considered full.
*
* @group Battery Calibration
* @unit V
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
/**
* Voltage drop per cell on 100% load
*
* This implicitely defines the internal resistance
* to maximum current ratio and assumes linearity.
*
* @group Battery Calibration
* @unit V
* @min 0.0
* @max 1.5
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
/**
* Number of cells.
*
* Defines the number of cells the attached battery consists of.
*
* @group Battery Calibration
* @unit S
* @min 2
* @max 10
* @value 2 2S Battery
* @value 3 3S Battery
* @value 4 4S Battery
* @value 5 5S Battery
* @value 6 6S Battery
* @value 7 7S Battery
* @value 8 8S Battery
* @value 9 9S Battery
* @value 10 10S Battery
* @value 11 11S Battery
* @value 12 12S Battery
* @value 13 13S Battery
* @value 14 14S Battery
* @value 15 15S Battery
* @value 16 16S Battery
*/
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
/**
* Battery capacity.
*
* Defines the capacity of the attached battery.
*
* @group Battery Calibration
* @unit mA
* @decimal 0
* @min -1.0
* @max 100000
* @increment 50
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);