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

Battery current reading implemented.

This commit is contained in:
Anton Babushkin 2013-11-07 22:23:57 +04:00
parent ea708915b9
commit d9767eb100
2 changed files with 31 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/sensors/sensor_params.c
View File

@ -197,12 +197,14 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Star
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.001f); // TODO set correct default value
#else
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
/* PX4IOAR: 0.00838095238 */
/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.001f); // TODO set correct default value
#endif
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);

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

@ -261,6 +261,7 @@ private:
float rc_scale_flaps;
float battery_voltage_scaling;
float battery_current_scaling;
} _parameters; /**< local copies of interesting parameters */
@ -306,6 +307,7 @@ private:
param_t rc_scale_flaps;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t board_rotation;
param_t external_mag_rotation;
@ -547,6 +549,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");
@ -724,6 +727,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));
@ -1162,25 +1170,25 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_v < 3.0f) {
if (_battery_status.voltage_v < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
_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));;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* 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.timestamp = 0;
}
} 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.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;
}
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@ -1214,6 +1222,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
_last_adc = hrt_absolute_time();
}
}
if (_battery_status.timestamp != 0) {
/* 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);
}
}
}
}