fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 handle accelerometer clipping

Browse Source 
- track clipping per IMU axis and pass through to ecl/EKF
 - update ecl/EKF to include delta velocity clipping changes (PX4/ecl#663)
This commit is contained in:
Daniel Agar
2020-04-07 20:11:08 -04:00
committed by GitHub
parent 74aa3201ce
commit cf37be8c44
12 changed files with 101 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+12
View File
@@ -777,6 +777,12 @@ void Ekf2::Run()
imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
if (imu.delta_velocity_clipping > 0) {
imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X;
imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y;
imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
}
imu_dt = imu.dt;
bias.accel_device_id = imu.accel_device_id;
@@ -792,6 +798,12 @@ void Ekf2::Run()
imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
if (sensor_combined.accelerometer_clipping > 0) {
imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X;
imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y;
imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z;
}
imu_dt = sensor_combined.gyro_integral_dt;
}
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+1 -2
View File
@@ -121,8 +121,7 @@ void VehicleIMU::Run()
delta_velocity.copyTo(imu.delta_velocity);
imu.dt = accel.dt;
imu.integrated_samples = accel.samples;
imu.clip_count = accel.clip_count;
//imu.clip_count = accel.clip_count;
imu.timestamp = hrt_absolute_time();
_vehicle_imu_pub.publish(imu);
src/modules/sensors/voted_sensors_update.cpp
+24
View File
@@ -505,6 +505,28 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1);
_last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2);
// record if there's any clipping per axis
_last_sensor_data[uorb_index].accelerometer_clipping = 0;
if (accel_report.clip_counter[0] > 0 || accel_report.clip_counter[1] > 0 || accel_report.clip_counter[2] > 0) {
const Vector3f sensor_clip_count{(float)accel_report.clip_counter[0], (float)accel_report.clip_counter[1], (float)accel_report.clip_counter[2]};
const Vector3f clipping{_board_rotation * sensor_clip_count};
static constexpr float CLIP_COUNT_THRESHOLD = 1.f;
if (fabsf(clipping(0)) >= CLIP_COUNT_THRESHOLD) {
_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_X;
}
if (fabsf(clipping(1)) >= CLIP_COUNT_THRESHOLD) {
_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_Y;
}
if (fabsf(clipping(2)) >= CLIP_COUNT_THRESHOLD) {
_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_Z;
}
}
_last_accel_timestamp[uorb_index] = accel_report.timestamp;
_accel.voter.put(uorb_index, accel_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2,
accel_report.error_count, _accel.priority[uorb_index]);
@@ -520,6 +542,8 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[best_index].accelerometer_m_s2, sizeof(raw.accelerometer_m_s2));
raw.accelerometer_clipping = _last_sensor_data[best_index].accelerometer_clipping;
if (best_index != _accel.last_best_vote) {
_accel.last_best_vote = (uint8_t)best_index;
}