Commit c1fcde91 authored by Damien SIX's avatar Damien SIX
Browse files

Send vehicle unfilitered acceleration data

parent 79954fff
......@@ -23,5 +23,3 @@ param set COM_RC_IN_MODE 1
param set NAV_RCL_ACT 4
param set COM_RC_OVERRIDE 0
param set IMU_ACCEL_CUTOFF 0
......@@ -433,4 +433,8 @@ rtps:
- msg: estimator_wind
id: 203
alias: wind
- msg: vehicle_acceleration_raw
id: 204
alias: vehicle_acceleration
send: true
########## multi topics: end ##########
......@@ -4,3 +4,5 @@ uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
float32[3] xyz # Bias corrected acceleration (including gravity) in the FRD body frame XYZ-axis in m/s^2
# TOPICS vehicle_acceleration vehicle_acceleration_raw
......@@ -224,6 +224,15 @@ void VehicleAcceleration::Run()
// - 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;
if (!_sensor_sub.updated()) {
// Publish corrected but unfiltered acceleration data
vehicle_acceleration_s v_acceleration;
v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
accel_corrected.copyTo(v_acceleration.xyz);
v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_raw_pub.publish(v_acceleration);
}
const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
_acceleration_prev = accel_corrected;
......
......@@ -78,6 +78,7 @@ private:
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_raw_pub{ORB_ID(vehicle_acceleration_raw)};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment