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

sensors/vehicle_acceleration: apply calibration before filtering

This commit is contained in:
Daniel Agar 2020-07-02 11:46:35 -04:00
parent 692cb717b7
commit d3157f4266
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@ -253,22 +253,23 @@ void VehicleAcceleration::Run()
CheckFilters(); CheckFilters();
// Filter: apply low-pass // Apply calibration and filter
const Vector3f accel_filtered = _lp_filter.apply(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}); // - calibration offsets, scale factors, and thermal scale (if available)
// - estimated in run bias (if available)
// - biquad low-pass filter
const Vector3f accel_corrected = _calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias;
const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
_acceleration_prev = accel_filtered; _acceleration_prev = accel_corrected;
// publish once all new samples are processed // publish once all new samples are processed
sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated(); sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
if (!sensor_updated) { if (!sensor_updated) {
// correct for in-run bias errors
const Vector3f accel = _calibration.Correct(accel_filtered) - _bias;
// Publish vehicle_acceleration // Publish vehicle_acceleration
vehicle_acceleration_s v_acceleration; vehicle_acceleration_s v_acceleration;
v_acceleration.timestamp_sample = sensor_data.timestamp_sample; v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(v_acceleration.xyz); accel_filtered.copyTo(v_acceleration.xyz);
v_acceleration.timestamp = hrt_absolute_time(); v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(v_acceleration); _vehicle_acceleration_pub.publish(v_acceleration);