fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 16:03: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/lib/drivers/accelerometer/PX4Accelerometer.cpp
+23 -12
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;
}
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+2 -3
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};
};
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+23 -12
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;
}
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+2 -2
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
src/lib/ecl
+1 -1
Submodule src/lib/ecl updated: 2fa43419d2...47624a0f02