From 32c313578868ddb1cf203ab820c4024de9c0013f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 24 Feb 2016 18:08:56 +0000
Subject: [PATCH] commander: move battery calculations to systemlib
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.
---
msg/battery_status.msg | 11 +-
msg/vehicle_status.msg | 8 -
src/drivers/blinkm/blinkm.cpp | 53 ++++--
src/drivers/frsky_telemetry/frsky_data.c | 57 +++---
src/drivers/frsky_telemetry/frsky_data.h | 1 +
src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +
src/drivers/frsky_telemetry/sPort_data.c | 35 ++--
src/drivers/px4io/px4io.cpp | 36 ++--
src/modules/commander/commander.cpp | 152 ++++++++--------
src/modules/commander/commander_helper.cpp | 72 --------
src/modules/commander/commander_helper.h | 17 --
src/modules/commander/commander_params.c | 82 +--------
src/modules/gpio_led/gpio_led.c | 36 ++--
src/modules/mavlink/mavlink_messages.cpp | 34 ++--
src/modules/sdlog2/sdlog2.c | 14 +-
src/modules/sdlog2/sdlog2_messages.h | 8 +-
src/modules/sensors/sensors.cpp | 80 +++------
src/modules/systemlib/CMakeLists.txt | 7 +-
src/modules/systemlib/battery.cpp | 162 ++++++++++++++++++
src/modules/systemlib/battery.h | 97 +++++++++++
src/modules/systemlib/battery_params.c | 123 +++++++++++++
21 files changed, 661 insertions(+), 426 deletions(-)
create mode 100644 src/modules/systemlib/battery.cpp
create mode 100644 src/modules/systemlib/battery.h
create mode 100644 src/modules/systemlib/battery_params.c
diff --git a/msg/battery_status.msg b/msg/battery_status.msg
index 75b144c6e8..ff4485378a 100644
--- a/msg/battery_status.msg
+++ b/msg/battery_status.msg
@@ -3,4 +3,13 @@ float32 voltage_v # Battery voltage in volts, 0 if unknown
float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown
float32 current_a # Battery current in amperes, -1 if unknown
float32 discharged_mah # Discharged amount in mAh, -1 if unknown
-bool is_powering_off # Power off event imminent indication, false if unknown
+float32 remaining # From 1 to 0, -1 if unknown
+int32 cell_count # Number of cells
+#bool is_powering_off # Power off event imminent indication, false if unknown
+
+
+uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active
+uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage
+uint8 BATTERY_WARNING_CRITICAL = 2 # alerting of critical voltage
+
+uint8 warning # current battery warning
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
index b6a0486456..33503c4e68 100644
--- a/msg/vehicle_status.msg
+++ b/msg/vehicle_status.msg
@@ -77,7 +77,6 @@ bool is_vtol # True if the system is VTOL capable
bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode
bool in_transition_mode # True if VTOL is doing a transition
-bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized
bool condition_system_prearm_error_reported # true if errors have already been reported
@@ -125,11 +124,4 @@ uint32 onboard_control_sensors_enabled
uint32 onboard_control_sensors_health
float32 load # processor load from 0 to 1
-float32 battery_voltage
-float32 battery_current
-float32 battery_remaining
-float32 battery_discharged_mah
-uint32 battery_cell_count
-
-uint8 battery_warning # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 78fce9162e..b2098f06c5 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012-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
@@ -119,12 +119,12 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/safety.h>
-static const float MAX_CELL_VOLTAGE = 4.3f;
static const int LED_ONTIME = 120;
static const int LED_OFFTIME = 120;
static const int LED_BLINK = 1;
@@ -195,6 +195,7 @@ private:
bool systemstate_run;
int vehicle_status_sub_fd;
+ int battery_status_sub_fd;
int vehicle_control_mode_sub_fd;
int vehicle_gps_position_sub_fd;
int actuator_armed_sub_fd;
@@ -202,7 +203,6 @@ private:
int num_of_cells;
int detected_cells_runcount;
-
int t_led_color[8];
int t_led_blink;
int led_thread_runcount;
@@ -291,6 +291,7 @@ BlinkM::BlinkM(int bus, int blinkm) :
led_blink(LED_NOBLINK),
systemstate_run(false),
vehicle_status_sub_fd(-1),
+ battery_status_sub_fd(-1),
vehicle_control_mode_sub_fd(-1),
vehicle_gps_position_sub_fd(-1),
actuator_armed_sub_fd(-1),
@@ -434,6 +435,9 @@ BlinkM::led()
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 250);
+ battery_status_sub_fd = orb_subscribe(ORB_ID(battery_status));
+ orb_set_interval(battery_status_sub_fd, 250);
+
vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(vehicle_control_mode_sub_fd, 250);
@@ -510,29 +514,33 @@ BlinkM::led()
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
- struct vehicle_status_s vehicle_status_raw;
- struct vehicle_control_mode_s vehicle_control_mode;
- struct actuator_armed_s actuator_armed;
- struct vehicle_gps_position_s vehicle_gps_position_raw;
- struct safety_s safety;
+ struct vehicle_status_s vehicle_status_raw = {};
+ struct battery_status_s battery_status = {};
+ struct vehicle_control_mode_s vehicle_control_mode = {};
+ struct actuator_armed_s actuator_armed = {};
+ struct vehicle_gps_position_s vehicle_gps_position_raw = {};
+ struct safety_s safety = {};
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
memset(&safety, 0, sizeof(safety));
bool new_data_vehicle_status;
+ bool new_data_battery_status;
bool new_data_vehicle_control_mode;
bool new_data_actuator_armed;
bool new_data_vehicle_gps_position;
bool new_data_safety;
- orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
int no_data_vehicle_status = 0;
+ int no_data_battery_status = 0;
int no_data_vehicle_control_mode = 0;
int no_data_actuator_armed = 0;
int no_data_vehicle_gps_position = 0;
+ orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
+
if (new_data_vehicle_status) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
no_data_vehicle_status = 0;
@@ -545,6 +553,19 @@ BlinkM::led()
}
}
+ orb_check(battery_status_sub_fd, &new_data_battery_status);
+
+ if (new_data_battery_status) {
+ orb_copy(ORB_ID(battery_status), battery_status_sub_fd, &battery_status);
+
+ } else {
+ no_data_battery_status++;
+
+ if (no_data_battery_status >= 3) {
+ no_data_battery_status = 3;
+ }
+ }
+
orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
if (new_data_vehicle_control_mode) {
@@ -597,19 +618,13 @@ BlinkM::led()
/* get number of used satellites in navigation */
num_of_used_sats = vehicle_gps_position_raw.satellites_used;
- if (new_data_vehicle_status || no_data_vehicle_status < 3) {
+ if (new_data_battery_status || no_data_battery_status < 3) {
if (num_of_cells == 0) {
- /* looking for lipo cells that are connected */
- printf("<blinkm> checking cells\n");
-
- for (num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
- if (vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) { break; }
- }
- printf("<blinkm> cells found:%d\n", num_of_cells);
+ num_of_cells = battery_status.cell_count;
} else {
- if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
+ if (battery_status.warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
@@ -633,7 +648,7 @@ BlinkM::led()
led_color_8 = LED_BLUE;
led_blink = LED_BLINK;
- } else if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
+ } else if (battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 70b2c2ab43..723a51415d 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -53,7 +53,6 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>
@@ -95,12 +94,15 @@
static struct battery_status_s *battery_status;
static struct vehicle_global_position_s *global_pos;
static struct sensor_combined_s *sensor_data;
-static struct vehicle_status_s *vehicle_status;
-static int battery_sub = -1;
+static int battery_status_sub = -1;
static int global_position_sub = -1;
static int sensor_sub = -1;
-static int vehicle_status_sub = -1;
+
+static struct battery_status_s battery_status;
+static struct sensor_combined_s raw;
+static struct vehicle_global_position_s global_pos;
+
/**
* Initializes the uORB subscriptions.
@@ -110,16 +112,14 @@ bool frsky_init()
battery_status = malloc(sizeof(struct battery_status_s));
global_pos = malloc(sizeof(struct vehicle_global_position_s));
sensor_data = malloc(sizeof(struct sensor_combined_s));
- vehicle_status = malloc(sizeof(struct vehicle_status_s));
- if (battery_status == NULL || global_pos == NULL || sensor_data == NULL || vehicle_status == NULL) {
+ if (battery_status == NULL || global_pos == NULL || sensor_data == NULL) {
return false;
}
- battery_sub = orb_subscribe(ORB_ID(battery_status));
+ battery_status_sub = orb_subscribe(ORB_ID(battery_status));
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
- vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
return true;
}
@@ -128,7 +128,6 @@ void frsky_deinit()
free(battery_status);
free(global_pos);
free(sensor_data);
- free(vehicle_status);
}
/**
@@ -178,18 +177,35 @@ static void frsky_send_data(int uart, uint8_t id, int16_t data)
frsky_send_byte(uart, udata >> 8); /* MSB */
}
+void frsky_update_topics()
+{
+ bool updated;
+
+ /* get a local copy of the current sensor values */
+ orb_check(sensor_sub, &updated);
+ if (updated) {
+ orb_copy(ORB_ID(sensor_combined), sensor_sub, raw);
+ }
+
+ /* get a local copy of the battery data */
+ orb_check(battery_status_sub, &updated);
+ if (updated) {
+ orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
+ }
+
+ /* get a local copy of the global position data */
+ orb_check(global_position_sub, &updated);
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
+ }
+}
+
/**
* Sends frame 1 (every 200ms):
* acceleration values, barometer altitude, temperature, battery voltage & current
*/
void frsky_send_frame1(int uart)
{
- /* get a local copy of the current sensor values */
- orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_data);
-
- /* get a local copy of the battery data */
- orb_copy(ORB_ID(battery_status), battery_sub, battery_status);
-
/* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X,
roundf(sensor_data->accelerometer_m_s2[0] * 1000.0f));
@@ -229,12 +245,6 @@ static float frsky_format_gps(float dec)
*/
void frsky_send_frame2(int uart)
{
- /* get a local copy of the global position data */
- orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
-
- /* get a local copy of the vehicle status data */
- orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
-
/* send formatted frame */
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
@@ -273,7 +283,7 @@ void frsky_send_frame2(int uart)
frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
frsky_send_data(uart, FRSKY_ID_FUEL,
- roundf(vehicle_status->battery_remaining * 100.0f));
+ roundf(battery_status->remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
@@ -286,9 +296,6 @@ void frsky_send_frame2(int uart)
*/
void frsky_send_frame3(int uart)
{
- /* get a local copy of the battery data */
- orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
-
/* send formatted frame */
time_t time_gps = global_pos->time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h
index b3c41d0aa4..a66f939ad7 100644
--- a/src/drivers/frsky_telemetry/frsky_data.h
+++ b/src/drivers/frsky_telemetry/frsky_data.h
@@ -47,6 +47,7 @@
// Public functions
bool frsky_init(void);
void frsky_deinit(void);
+void frsky_update_topics(void);
void frsky_send_frame1(int uart);
void frsky_send_frame2(int uart);
void frsky_send_frame3(int uart);
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 35b4ed2c47..3cac256ddd 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -456,6 +456,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
host_frame.ad1, host_frame.ad2, host_frame.linkq);
}
+ frsky_update_topics();
+
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
if (iteration % 2 == 0) {
frsky_send_frame1(uart);
diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c
index 41d0661c59..c373640a94 100644
--- a/src/drivers/frsky_telemetry/sPort_data.c
+++ b/src/drivers/frsky_telemetry/sPort_data.c
@@ -51,7 +51,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/battery_status.h>
#include <drivers/drv_hrt.h>
@@ -59,35 +59,35 @@
static int sensor_sub = -1;
static int global_position_sub = -1;
-static int vehicle_status_sub = -1;
-static struct vehicle_status_s *vehicle_status;
+static int battery_status_sub = -1;
static struct sensor_combined_s *sensor_data;
static struct vehicle_global_position_s *global_pos;
+static struct battery_status_s *battery_status;
/**
* Initializes the uORB subscriptions.
*/
bool sPort_init()
{
- vehicle_status = malloc(sizeof(struct vehicle_status_s));
sensor_data = malloc(sizeof(struct sensor_combined_s));
global_pos = malloc(sizeof(struct vehicle_global_position_s));
+ battery_status = malloc(sizeof(struct battery_status_s));
- if (vehicle_status == NULL || sensor_data == NULL || global_pos == NULL) {
+ if (sensor_data == NULL || global_pos == NULL || battery_status == NULL) {
return false;
}
- global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
- vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+ global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ battery_status_sub = orb_subscribe(ORB_ID(battery_status));
return true;
}
void sPort_deinit()
{
- free(vehicle_status);
free(sensor_data);
free(global_pos);
+ free(battery_status);
}
static void update_crc(uint16_t *crc, unsigned char b)
@@ -158,19 +158,24 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
// scaling correct with OpenTX 2.1.7
void sPort_send_BATV(int uart)
{
- /* get a local copy of the vehicle status data */
- orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
+ /* get a local copy of the battery data */
+ orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
/* send battery voltage as VFAS */
- uint32_t voltage = (int)(100 * vehicle_status->battery_voltage);
+ uint32_t voltage = (int)(100 * battery_status->voltage_v);
+
sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage);
}
// verified scaling
void sPort_send_CUR(int uart)
{
+ /* get a local copy of the battery data */
+ orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
+
/* send data */
- uint32_t current = (int)(10 * vehicle_status->battery_current);
+ uint32_t current = (int)(10 * battery_status->current_a);
+
sPort_send_data(uart, SMARTPORT_ID_CURR, current);
}
@@ -207,8 +212,12 @@ void sPort_send_VSPD(int uart, float speed)
// verified scaling
void sPort_send_FUEL(int uart)
{
+ /* get a local copy of the battery data */
+ orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
+
/* send data */
- uint32_t fuel = (int)(100 * vehicle_status->battery_remaining);
+ uint32_t fuel = (int)(100 * vehicle_status->remaining_mah);
+
sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index f5421b4307..3159154158 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -74,6 +74,7 @@
#include <systemlib/param/param.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/mavlink_log.h>
+#include <systemlib/battery.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
@@ -308,10 +309,14 @@ private:
bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled
bool _in_esc_calibration_mode; ///< do not send control outputs to IO (used for esc calibration)
+ Battery _battery;
+
int32_t _rssi_pwm_chan; ///< RSSI PWM input channel
int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel
int32_t _rssi_pwm_min; ///< min RSSI input on PWM channel
+ float _last_throttle; ///< last throttle value for battery calculation
+
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power
#endif
@@ -538,9 +543,11 @@ PX4IO::PX4IO(device::Device *interface) :
_battery_last_timestamp(0),
_cb_flighttermination(true),
_in_esc_calibration_mode(false),
+ _battery{},
_rssi_pwm_chan(0),
_rssi_pwm_max(0),
- _rssi_pwm_min(0)
+ _rssi_pwm_min(0),
+ _last_throttle(0.0f)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
, _dsm_vcc_ctl(false)
#endif
@@ -1188,6 +1195,8 @@ PX4IO::task_main()
(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
}
}
+ // Also trigger param update in Battery instance.
+ _battery.updateParams();
}
}
@@ -1297,6 +1306,11 @@ PX4IO::io_set_control_state(unsigned group)
regs[i] = FLOAT_TO_REG(ctrl);
}
+ // save last throttle for battery calculation
+ if (group == 0) {
+ _last_throttle = controls.control[3];
+ }
+
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
}
@@ -1648,29 +1662,19 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
}
battery_status_s battery_status;
- battery_status.timestamp = hrt_absolute_time();
+ const hrt_abstime timestamp = hrt_absolute_time();
/* voltage is scaled to mV */
- battery_status.voltage_v = vbatt / 1000.0f;
- battery_status.voltage_filtered_v = vbatt / 1000.0f;
+ const float voltage_v = vbatt / 1000.0f;
/*
ibatt contains the raw ADC count, as 12 bit ADC
value, with full range being 3.3v
*/
- battery_status.current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
- battery_status.current_a += _battery_amp_bias;
-
- /*
- integrate battery over time to get total mAh used
- */
- if (_battery_last_timestamp != 0) {
- _battery_mamphour_total += battery_status.current_a *
- (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
- }
+ float current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
+ current_a += _battery_amp_bias;
- battery_status.discharged_mah = _battery_mamphour_total;
- _battery_last_timestamp = battery_status.timestamp;
+ _battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, &battery_status);
/* the announced battery status would conflict with the simulated battery status in HIL */
if (!(_pub_blocked)) {
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1a5476972d..4c659075af 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -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) {
+
+ /* 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");
+ }
- /* 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);
+ } 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(×tamp_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(×tamp_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) {
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 30448594be..758c231320 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -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;
-}
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index d2ab41f887..f0f5eb49f3 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -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_ */
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 6a4d409103..e559f80ac1 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -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.
*
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 43ff2e5b87..2fffdcb97e 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -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);
+ }
+
+ 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 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)
}
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 8b71547c10..5fcc24690a 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -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);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index f6d24fbd30..c89cab2fd4 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -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)) {
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 3eec110ac8..5505998400 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -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"),
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 5cb035ff9c..31c7211f7e 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -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);
diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt
index 25f7d26a50..4507974e85 100644
--- a/src/modules/systemlib/CMakeLists.txt
+++ b/src/modules/systemlib/CMakeLists.txt
@@ -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 :
diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp
new file mode 100644
index 0000000000..91b08812bb
--- /dev/null
+++ b/src/modules/systemlib/battery.cpp
@@ -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;
+ }
+}
diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h
new file mode 100644
index 0000000000..e85c19b775
--- /dev/null
+++ b/src/modules/systemlib/battery.h
@@ -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;
+};
+
diff --git a/src/modules/systemlib/battery_params.c b/src/modules/systemlib/battery_params.c
new file mode 100644
index 0000000000..bc7bb77347
--- /dev/null
+++ b/src/modules/systemlib/battery_params.c
@@ -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);
--
GitLab