fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 15:33:33 +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/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;
}