diff --git a/msg/sensor_accel_integrated.msg b/msg/sensor_accel_integrated.msg index bacea620a3..cbc7d35234 100644 --- a/msg/sensor_accel_integrated.msg +++ b/msg/sensor_accel_integrated.msg @@ -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 diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index eaa4414165..92a0b4729b 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -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 diff --git a/msg/sensor_gyro_integrated.msg b/msg/sensor_gyro_integrated.msg index 18fe115ebb..35b3944097 100644 --- a/msg/sensor_gyro_integrated.msg +++ b/msg/sensor_gyro_integrated.msg @@ -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 diff --git a/msg/vehicle_imu.msg b/msg/vehicle_imu.msg index 0f036b4936..c5ebfdf2ab 100644 --- a/msg/vehicle_imu.msg +++ b/msg/vehicle_imu.msg @@ -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 diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index bf6345c7bb..63a4a09d7b 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -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; } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index a846d0c4bf..6819ea9433 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -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}; - }; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 3bcd4fbeb4..b342252962 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -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; } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 12198f49c3..e2785e3d6c 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -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) _param_imu_gyro_rate_max diff --git a/src/lib/ecl b/src/lib/ecl index 2fa43419d2..47624a0f02 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 2fa43419d2bd7319024682b311f52c78f9a86f52 +Subproject commit 47624a0f021e2c187cc4763ad829afe2aa4fb11a diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index c0a123b42a..9b5a358eb8 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -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; } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 983896d1fe..174e71b186 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -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); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 62be97492e..d920e18820 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -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; }