dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors: minor cleanup, bugs fixed, use unsigned long for discharged integration to avoid rounding errors.

This commit is contained in:
Anton Babushkin 2013-11-10 15:51:47 +04:00
parent 20db1602d7
commit e8487b7498
2 changed files with 33 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/modules/sensors/sensors.cpp
View File

@ -126,8 +126,7 @@
#endif
#define BAT_VOL_INITIAL 0.f
#define BAT_VOL_LOWPASS_1 0.99f
#define BAT_VOL_LOWPASS_2 0.01f
#define BAT_VOL_LOWPASS 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
/**
@ -216,6 +215,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 */
unsigned long _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];
@ -462,7 +464,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 */
@ -1149,17 +1153,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])) {
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);
@ -1176,8 +1179,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
_battery_status.voltage_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));;
_battery_status.timestamp = t;
_battery_status.voltage_v += (voltage - _battery_status.voltage_v) * BAT_VOL_LOWPASS;
} else {
/* mark status as invalid */
@ -1187,9 +1190,14 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
/* handle current only if voltage is valid */
if (_battery_status.timestamp != 0) {
_battery_status.current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling);
float dt = fminf(20.0f, (hrt_absolute_time() - _last_adc) * 0.001f); // in ms, limit to 20ms
_battery_status.discharged_mah += _battery_status.current_a * dt / 3600.0f;
float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
_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) {
@ -1206,7 +1214,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;
@ -1219,11 +1227,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
_last_adc = hrt_absolute_time();
}
}
_last_adc = t;
if (_battery_status.timestamp != 0) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
@ -1235,6 +1240,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
}
void
Sensors::rc_poll()

4
src/modules/uORB/topics/battery_status.h
View File

@ -54,8 +54,8 @@
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 current_a; /**< Battery current in amperes, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */
};
/**