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

ekf2 handle accelerometer clipping 

- 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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 101 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/sensor_accel_integrated.msg
View File

@ -8,4 +8,5 @@ uint64 error_count
float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt)
uint16 dt # integration time (microseconds)
uint8 samples # number of samples integrated
uint8 clip_count # total clip count per integration period on any axis
uint8[3] clip_counter # clip count per axis over the integration period

5
msg/sensor_combined.msg
View File

@ -13,3 +13,8 @@ uint32 gyro_integral_dt # gyro measurement sampling period in us
int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
float32[3] accelerometer_m_s2 # average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period
uint32 accelerometer_integral_dt # accelerometer measurement sampling period in us
uint8 CLIPPING_X = 1
uint8 CLIPPING_Y = 2
uint8 CLIPPING_Z = 4
uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period

3
msg/sensor_gyro_integrated.msg
View File

@ -8,4 +8,5 @@ uint64 error_count
float32[3] delta_angle # delta angle in the NED board axis in rad/s over the integration time frame (dt)
uint16 dt # integration time (microseconds)
uint8 samples # number of samples integrated
uint8 clip_count # total clip count per integration period on any axis
uint8[3] clip_counter # clip count per axis over the integration period

6
msg/vehicle_imu.msg
View File

@ -11,5 +11,7 @@ float32[3] delta_velocity # delta velocity in the NED board axis in m/s ov
uint16 dt # integration period in us
uint8 integrated_samples # number of samples integrated
uint8 clip_count # total clip count per integration period on any axis
uint8 CLIPPING_X = 1
uint8 CLIPPING_Y = 2
uint8 CLIPPING_Z = 4
uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame

35
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@ -137,8 +137,8 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
// Clipping (check unscaled raw values)
for (int i = 0; i < 3; i++) {
if (fabsf(raw(i)) > _clip_limit) {
_clipping[i]++;
_integrator_clipping++;
_clipping_total[i]++;
_integrator_clipping(i)++;
}
}
@ -177,7 +177,11 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
delta_velocity.copyTo(report.delta_velocity);
report.dt = integral_dt;
report.samples = _integrator_samples;
report.clip_count = _integrator_clipping;
for (int i = 0; i < 3; i++) {
report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i)));
}
report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);
@ -230,11 +234,13 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
_clipping[0] += clip_count_x;
_clipping[1] += clip_count_y;
_clipping[2] += clip_count_z;
_clipping_total[0] += clip_count_x;
_clipping_total[1] += clip_count_y;
_clipping_total[2] += clip_count_z;
_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
_integrator_clipping(0) += clip_count_x;
_integrator_clipping(1) += clip_count_y;
_integrator_clipping(2) += clip_count_z;
// integrated data (INS)
{
@ -280,7 +286,12 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
delta_velocity.copyTo(report.delta_velocity);
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
report.samples = _integrator_fifo_samples;
report.clip_count = _integrator_clipping;
const Vector3f clipping{_rotation_dcm * _integrator_clipping};
for (int i = 0; i < 3; i++) {
report.clip_counter[i] = fabsf(roundf(clipping(i)));
}
report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);
@ -328,9 +339,9 @@ void PX4Accelerometer::PublishStatus()
status.measure_rate_hz = _update_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.clipping[0] = _clipping[0];
status.clipping[1] = _clipping[1];
status.clipping[2] = _clipping[2];
status.clipping[0] = _clipping_total[0];
status.clipping[1] = _clipping_total[1];
status.clipping[2] = _clipping_total[2];
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
@ -343,7 +354,7 @@ void PX4Accelerometer::ResetIntegrator()
_integrator_samples = 0;
_integrator_fifo_samples = 0;
_integration_raw.zero();
_integrator_clipping = 0;
_integrator_clipping.zero();
_timestamp_sample_prev = 0;
}

5
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@ -120,17 +120,16 @@ private:
uint64_t _error_count{0};
uint32_t _clipping[3] {};
uint32_t _clipping_total[3] {};
uint16_t _update_rate{1000};
// integrator
hrt_abstime _timestamp_sample_prev{0};
matrix::Vector3f _integration_raw{};
matrix::Vector3f _integrator_clipping{};
int16_t _last_sample[3] {};
uint8_t _integrator_reset_samples{4};
uint8_t _integrator_samples{0};
uint8_t _integrator_fifo_samples{0};
uint8_t _integrator_clipping{0};
};

35
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -139,8 +139,8 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
// Clipping (check unscaled raw values)
for (int i = 0; i < 3; i++) {
if (fabsf(raw(i)) > _clip_limit) {
_clipping[i]++;
_integrator_clipping++;
_clipping_total[i]++;
_integrator_clipping(i)++;
}
}
@ -179,7 +179,11 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
delta_angle.copyTo(report.delta_angle);
report.dt = integral_dt;
report.samples = _integrator_samples;
report.clip_count = _integrator_clipping;
for (int i = 0; i < 3; i++) {
report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i)));
}
report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);
@ -232,11 +236,13 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
_clipping[0] += clip_count_x;
_clipping[1] += clip_count_y;
_clipping[2] += clip_count_z;
_clipping_total[0] += clip_count_x;
_clipping_total[1] += clip_count_y;
_clipping_total[2] += clip_count_z;
_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
_integrator_clipping(0) += clip_count_x;
_integrator_clipping(1) += clip_count_y;
_integrator_clipping(2) += clip_count_z;
// integrated data (INS)
{
@ -282,7 +288,12 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
delta_angle.copyTo(report.delta_angle);
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
report.samples = _integrator_fifo_samples;
report.clip_count = _integrator_clipping;
const Vector3f clipping{_rotation_dcm * _integrator_clipping};
for (int i = 0; i < 3; i++) {
report.clip_counter[i] = fabsf(roundf(clipping(i)));
}
report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);
@ -331,9 +342,9 @@ void PX4Gyroscope::PublishStatus()
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.coning_vibration = _coning_vibration;
status.clipping[0] = _clipping[0];
status.clipping[1] = _clipping[1];
status.clipping[2] = _clipping[2];
status.clipping[0] = _clipping_total[0];
status.clipping[1] = _clipping_total[1];
status.clipping[2] = _clipping_total[2];
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
@ -346,7 +357,7 @@ void PX4Gyroscope::ResetIntegrator()
_integrator_samples = 0;
_integrator_fifo_samples = 0;
_integration_raw.zero();
_integrator_clipping = 0;
_integrator_clipping.zero();
_timestamp_sample_prev = 0;
}

4
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -122,18 +122,18 @@ private:
uint64_t _error_count{0};
uint32_t _clipping[3] {};
uint32_t _clipping_total[3] {};
uint16_t _update_rate{1000};
// integrator
hrt_abstime _timestamp_sample_prev{0};
matrix::Vector3f _integration_raw{};
matrix::Vector3f _integrator_clipping{};
int16_t _last_sample[3] {};
uint8_t _integrator_reset_samples{4};
uint8_t _integrator_samples{0};
uint8_t _integrator_fifo_samples{0};
uint8_t _integrator_clipping{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max

2
src/lib/ecl

@ -1 +1 @@
Subproject commit 2fa43419d2bd7319024682b311f52c78f9a86f52
Subproject commit 47624a0f021e2c187cc4763ad829afe2aa4fb11a

12
src/modules/ekf2/ekf2_main.cpp
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;
}

3
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
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);

24
src/modules/sensors/voted_sensors_update.cpp
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;
}