aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/commander/commander.cpp65
-rw-r--r--src/modules/commander/commander_helper.cpp44
-rw-r--r--src/modules/commander/commander_helper.h7
-rw-r--r--src/modules/commander/commander_params.c7
-rw-r--r--src/modules/mavlink/mavlink.c4
-rw-r--r--src/modules/sdlog2/sdlog2.c30
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h14
-rw-r--r--src/modules/sensors/sensor_params.c1
-rw-r--r--src/modules/sensors/sensors.cpp91
-rw-r--r--src/modules/uORB/topics/battery_status.h9
10 files changed, 161 insertions, 111 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0357542f0..add7312de 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -107,14 +107,9 @@ static const int ERROR = -1;
extern struct system_load_s system_load;
-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
/* Decouple update interval and hysteris counters, all depends on intervals */
#define COMMANDER_MONITORING_INTERVAL 50000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define MAVLINK_OPEN_INTERVAL 50000
@@ -666,8 +661,6 @@ int commander_thread_main(int argc, char *argv[])
/* Start monitoring loop */
unsigned counter = 0;
- unsigned low_voltage_counter = 0;
- unsigned critical_voltage_counter = 0;
unsigned stick_off_counter = 0;
unsigned stick_on_counter = 0;
@@ -745,7 +738,6 @@ int commander_thread_main(int argc, char *argv[])
int battery_sub = orb_subscribe(ORB_ID(battery_status));
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
- battery.voltage_v = 0.0f;
/* Subscribe to subsystem info topic */
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
@@ -890,14 +882,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-
- // warnx("bat v: %2.2f", battery.voltage_v);
-
- /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
- if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
- status.battery_voltage = battery.voltage_v;
+ /* only consider battery voltage if system has been running 2s and battery voltage is valid */
+ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+ status.battery_voltage = battery.voltage_filtered_v;
+ status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
- status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+ status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
}
}
@@ -950,46 +940,29 @@ int commander_thread_main(int argc, char *argv[])
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
- // XXX remove later
- //warnx("bat remaining: %2.2f", status.battery_remaining);
-
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
- //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
- if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
- low_battery_voltage_actions_done = true;
- mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
- status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
- status_changed = true;
- battery_tune_played = false;
- }
-
- low_voltage_counter++;
+ low_battery_voltage_actions_done = true;
+ mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
+ status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
+ status_changed = true;
+ battery_tune_played = false;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
- if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
- critical_battery_voltage_actions_done = true;
- mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
- status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
- battery_tune_played = false;
-
- if (armed.armed) {
- arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+ critical_battery_voltage_actions_done = true;
+ mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
+ status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
+ battery_tune_played = false;
- } else {
- arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
- }
+ if (armed.armed) {
+ arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
- status_changed = true;
+ } else {
+ arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
- critical_voltage_counter++;
-
- } else {
-
- low_voltage_counter = 0;
- critical_voltage_counter = 0;
+ status_changed = true;
}
/* End battery voltage check */
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 565b4b66a..21a1c4c2c 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -44,6 +44,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
+#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
-float battery_remaining_estimate_voltage(float voltage)
+float battery_remaining_estimate_voltage(float voltage, float discharged)
{
float ret = 0;
- static param_t bat_volt_empty;
- static param_t bat_volt_full;
- static param_t bat_n_cells;
+ 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 float bat_v_empty = 3.2f;
+ static float bat_v_full = 4.0f;
+ static int bat_n_cells = 3;
+ static float bat_capacity = -1.0f;
static bool initialized = false;
static unsigned int counter = 0;
- static float ncells = 3;
- // XXX change cells to int (and param to INT32)
if (!initialized) {
- bat_volt_empty = param_find("BAT_V_EMPTY");
- bat_volt_full = param_find("BAT_V_FULL");
- bat_n_cells = param_find("BAT_N_CELLS");
+ bat_v_empty_h = param_find("BAT_V_EMPTY");
+ bat_v_full_h = param_find("BAT_V_FULL");
+ bat_n_cells_h = param_find("BAT_N_CELLS");
+ bat_capacity_h = param_find("BAT_CAPACITY");
initialized = true;
}
- static float chemistry_voltage_empty = 3.2f;
- static float chemistry_voltage_full = 4.05f;
-
if (counter % 100 == 0) {
- param_get(bat_volt_empty, &chemistry_voltage_empty);
- param_get(bat_volt_full, &chemistry_voltage_full);
- param_get(bat_n_cells, &ncells);
+ param_get(bat_v_empty_h, &bat_v_empty);
+ param_get(bat_v_full_h, &bat_v_full);
+ param_get(bat_n_cells_h, &bat_n_cells);
+ param_get(bat_capacity_h, &bat_capacity);
}
counter++;
- ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
+ /* remaining charge estimate based on voltage */
+ float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
+
+ 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;
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index e9514446c..d0393f45a 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
void rgbled_set_pattern(rgbled_pattern_t *pattern);
/**
- * Provides a coarse estimate of remaining battery power.
+ * Estimate remaining battery charge.
*
- * The estimate is very basic and based on decharging voltage curves.
+ * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
+ * else use simple estimate based on voltage.
*
* @return the estimated remaining capacity in 0..1
*/
-float battery_remaining_estimate_voltage(float voltage);
+float battery_remaining_estimate_voltage(float voltage, float discharged);
#endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 40d0386d5..e10b7f18d 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -50,6 +50,7 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 7c10e297b..20853379d 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -677,8 +677,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_health,
v_status.load * 1000.0f,
v_status.battery_voltage * 1000.0f,
- v_status.battery_current * 1000.0f,
- v_status.battery_remaining,
+ v_status.battery_current * 100.0f,
+ v_status.battery_remaining * 100.0f,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index e94b1e13c..06b1eddaa 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
@@ -702,6 +702,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
+ struct battery_status_s battery;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -726,6 +727,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
+ int battery_sub;
} subs;
/* log message buffer: header + body */
@@ -752,6 +754,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
+ struct log_BATT_s log_BATT;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -760,9 +763,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
- /* number of subscriptions */
- const ssize_t fdsc = 19;
- /* sanity check variable and index */
+ /* number of messages */
+ const ssize_t fdsc = 25;
+ /* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -881,6 +884,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- BATTERY --- */
+ subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
+ fds[fdsc_count].fd = subs.battery_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -971,8 +980,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
- log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
- log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1238,6 +1245,17 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
+ /* --- BATTERY --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(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;
+ LOGBUFFER_WRITE_AND_COUNT(BATT);
+ }
+
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index ab4dc9b00..a784a1f30 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -148,8 +148,6 @@ struct log_STAT_s {
uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state;
- float battery_voltage;
- float battery_current;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
@@ -248,6 +246,15 @@ struct log_GVSP_s {
float vz;
};
+/* --- BATT - BATTERY --- */
+#define LOG_BATT_MSG 20
+struct log_BATT_s {
+ float voltage;
+ float voltage_filtered;
+ float current;
+ float discharged;
+};
+
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
@@ -281,7 +288,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
- LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
+ LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -291,6 +298,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,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"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index caec03e04..bbc84ef93 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -379,6 +379,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
#endif
+PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index d9f037c27..ff6c5882e 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -114,6 +114,7 @@
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
+#define ADC_BATTERY_CURRENT_CHANNEL -1
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
@@ -124,10 +125,8 @@
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
-#define BAT_VOL_INITIAL 0.f
-#define BAT_VOL_LOWPASS_1 0.99f
-#define BAT_VOL_LOWPASS_2 0.01f
-#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
+#define BATT_V_LOWPASS 0.001f
+#define BATT_V_IGNORE_THRESHOLD 3.5f
/**
* HACK - true temperature is much less than indicated temperature in baro,
@@ -215,6 +214,9 @@ private:
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
+ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
+ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
+
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
@@ -265,6 +267,7 @@ private:
float rc_fs_thr;
float battery_voltage_scaling;
+ float battery_current_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -314,6 +317,7 @@ private:
param_t rc_fs_thr;
param_t battery_voltage_scaling;
+ param_t battery_current_scaling;
param_t board_rotation;
param_t external_mag_rotation;
@@ -467,7 +471,9 @@ Sensors::Sensors() :
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
- _mag_is_external(false)
+ _mag_is_external(false),
+ _battery_discharged(0),
+ _battery_current_timestamp(0)
{
/* basic r/c parameters */
@@ -560,6 +566,7 @@ Sensors::Sensors() :
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
+ _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -740,6 +747,11 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
+ /* scaling of ADC ticks to battery current */
+ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
+ warnx("Failed updating current scaling param");
+ }
+
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
@@ -1157,17 +1169,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (!_publishing)
return;
+ hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
- if (hrt_absolute_time() - _last_adc >= 10000) {
+ if (t - _last_adc >= 10000) {
/* make space for a maximum of eight channels */
struct adc_msg_s buf_adc[8];
/* read all channels available */
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
- for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-
- if (ret >= (int)sizeof(buf_adc[0])) {
-
+ if (ret >= (int)sizeof(buf_adc[0])) {
+ for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
/* Save raw voltage values */
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
@@ -1178,27 +1189,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
- if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
-
+ 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_v < 3.0f) {
- _battery_status.voltage_v = voltage;
+ if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
+ _battery_status.voltage_filtered_v = voltage;
}
- _battery_status.timestamp = hrt_absolute_time();
- _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
- /* current and discharge are unknown */
- _battery_status.current_a = -1.0f;
- _battery_status.discharged_mah = -1.0f;
+ _battery_status.timestamp = t;
+ _battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
- /* announce the battery voltage if needed, just publish else */
- if (_battery_pub > 0) {
- orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+ } else {
+ /* mark status as invalid */
+ _battery_status.voltage_v = -1.0f;
+ _battery_status.voltage_filtered_v = -1.0f;
+ }
- } else {
- _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+ } 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_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;
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1214,7 +1238,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
- _diff_pres.timestamp = hrt_absolute_time();
+ _diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.voltage = voltage;
@@ -1227,8 +1251,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
-
- _last_adc = hrt_absolute_time();
+ }
+ _last_adc = t;
+ if (_battery_status.voltage_v > 0.0f) {
+ /* announce the battery status if needed, just publish else */
+ if (_battery_pub > 0) {
+ orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+
+ } else {
+ _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+ }
}
}
}
@@ -1516,7 +1548,10 @@ Sensors::task_main()
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
- _battery_status.voltage_v = BAT_VOL_INITIAL;
+ _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;
/* get a set of initial values */
accel_poll(raw);
diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h
index c40d0d4e5..d473dff3f 100644
--- a/src/modules/uORB/topics/battery_status.h
+++ b/src/modules/uORB/topics/battery_status.h
@@ -53,9 +53,10 @@
*/
struct battery_status_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
- float voltage_v; /**< Battery voltage in volts, filtered */
- float current_a; /**< Battery current in amperes, filtered, -1 if unknown */
- float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */
+ float voltage_v; /**< Battery voltage in volts, 0 if unknown */
+ float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */
+ float current_a; /**< Battery current in amperes, -1 if unknown */
+ float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */
};
/**
@@ -65,4 +66,4 @@ struct battery_status_s {
/* register this as object request broker structure */
ORB_DECLARE(battery_status);
-#endif \ No newline at end of file
+#endif
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-06 01:04:38 +0000