diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 26ea318a83..837c7a1fac 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -2,22 +2,24 @@ # These fields are scaled and offset-compensated where possible and do not # change with board revisions and sensor updates. -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid +int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid # gyro timstamp is equal to the timestamp of the message -float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period -uint32 gyro_integral_dt # gyro measurement sampling period in microseconds +float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period +uint32 gyro_integral_dt # gyro measurement sampling period in microseconds -int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp -float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period -uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds +int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp +float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period +uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds 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 -uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. -uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. +uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame +uint8 gyro_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame + +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index b1025e1d12..b906127b58 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -11,6 +11,8 @@ float32 temperature # temperature in degrees Celsius uint32 error_count +uint8[3] clip_counter # clip count per axis in the sample period + uint8 samples # number of raw samples that went into this message uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index a57b7c5058..f704d71933 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -1,12 +1,12 @@ # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) -float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame -float32[4] delta_q_reset # Amount by which quaternion has changed during last reset -uint8 quat_reset_counter # Quaternion reset counter +float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame +float32[4] delta_q_reset # Amount by which quaternion has changed during last reset +uint8 quat_reset_counter # Quaternion reset counter # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude # TOPICS estimator_attitude diff --git a/msg/vehicle_imu.msg b/msg/vehicle_imu.msg index 0184039157..a71bb7a01f 100644 --- a/msg/vehicle_imu.msg +++ b/msg/vehicle_imu.msg @@ -8,12 +8,15 @@ uint32 gyro_device_id # Gyroscope unique device ID for the sensor that float32[3] delta_angle # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt) float32[3] delta_velocity # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt) + uint16 delta_angle_dt # integration period in microseconds uint16 delta_velocity_dt # integration period in microseconds uint8 CLIPPING_X = 1 uint8 CLIPPING_Y = 2 uint8 CLIPPING_Z = 4 + +uint8 delta_angle_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. diff --git a/msg/vehicle_imu_status.msg b/msg/vehicle_imu_status.msg index 26db7487a7..78fb44703e 100644 --- a/msg/vehicle_imu_status.msg +++ b/msg/vehicle_imu_status.msg @@ -4,6 +4,7 @@ uint32 accel_device_id # unique device ID for the sensor that does not uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles uint32[3] accel_clipping # total clipping per axis +uint32[3] gyro_clipping # total clipping per axis uint32 accel_error_count uint32 gyro_error_count diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 432a8cb98d..fdce2004a1 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -50,6 +50,22 @@ static constexpr int32_t sum(const int16_t samples[], uint8_t len) return sum; } +static constexpr uint8_t clipping(const int16_t samples[], uint8_t len) +{ + unsigned clip_count = 0; + + for (int n = 0; n < len; n++) { + // - consider data clipped/saturated if it's INT16_MIN/INT16_MAX or within 1 + // - this accommodates rotated data (|INT16_MIN| = INT16_MAX + 1) + // and sensors that may re-use the lowest bit for other purposes (sync indicator, etc) + if ((samples[n] <= INT16_MIN + 1) || (samples[n] >= INT16_MAX - 1)) { + clip_count++; + } + } + + return clip_count; +} + PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) : _device_id{device_id}, _rotation{rotation} @@ -90,6 +106,8 @@ void PX4Gyroscope::set_scale(float scale) } _scale = scale; + + UpdateClipLimit(); } } @@ -107,6 +125,9 @@ void PX4Gyroscope::update(const hrt_abstime ×tamp_sample, float x, float y, report.x = x * _scale; report.y = y * _scale; report.z = z * _scale; + report.clip_counter[0] = (fabsf(x) >= _clip_limit); + report.clip_counter[1] = (fabsf(y) >= _clip_limit); + report.clip_counter[2] = (fabsf(z) >= _clip_limit); report.samples = 1; report.timestamp = hrt_absolute_time(); @@ -145,8 +166,17 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample) _last_sample[1] = sample.y[N - 1]; _last_sample[2] = sample.z[N - 1]; + report.clip_counter[0] = clipping(sample.x, N); + report.clip_counter[1] = clipping(sample.y, N); + report.clip_counter[2] = clipping(sample.z, N); report.samples = N; report.timestamp = hrt_absolute_time(); _sensor_pub.publish(report); } + +void PX4Gyroscope::UpdateClipLimit() +{ + // 99.9% of potential max + _clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX); +} diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 893020a490..f89514f7a1 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -1,7 +1,6 @@ - /**************************************************************************** * - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -53,7 +52,7 @@ public: void set_device_id(uint32_t device_id) { _device_id = device_id; } void set_device_type(uint8_t devtype); void set_error_count(uint32_t error_count) { _error_count = error_count; } - void set_range(float range) { _range = range; } + void set_range(float range) { _range = range; UpdateClipLimit(); } void set_scale(float scale); void set_temperature(float temperature) { _temperature = temperature; } @@ -64,6 +63,8 @@ public: int get_instance() { return _sensor_pub.get_instance(); }; private: + void UpdateClipLimit(); + uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_gyro)}; uORB::PublicationMulti _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)}; @@ -76,6 +77,8 @@ private: float _scale{1.f}; float _temperature{NAN}; + float _clip_limit{_range / _scale}; + uint32_t _error_count{0}; int16_t _last_sample[3] {}; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 12d1f1a637..e5bdddb83f 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -407,11 +407,11 @@ bool VehicleIMU::UpdateAccel() _publish_status = true; - if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) { + if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_accel_clipping_notify_time) > 3_s)) { // start notifying the user periodically if there's significant continuous clipping const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2]; - if (clipping_total > _last_clipping_notify_total_count + 1000) { + if (clipping_total > _last_accel_clipping_notify_total_count + 1000) { mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance); /* EVENT * @description Land now, and check the vehicle setup. @@ -419,8 +419,8 @@ bool VehicleIMU::UpdateAccel() */ events::send(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical, "Accel {1} clipping, not safe to fly!", _instance); - _last_clipping_notify_time = accel.timestamp_sample; - _last_clipping_notify_total_count = clipping_total; + _last_accel_clipping_notify_time = accel.timestamp_sample; + _last_accel_clipping_notify_total_count = clipping_total; } } } @@ -534,6 +534,52 @@ bool VehicleIMU::UpdateGyro() _gyro_integrator.put(gyro_raw, dt); updated = true; + + if (gyro.clip_counter[0] > 0 || gyro.clip_counter[1] > 0 || gyro.clip_counter[2] > 0) { + // rotate sensor clip counts into vehicle body frame + const Vector3f clipping{_gyro_calibration.rotation() * + Vector3f{(float)gyro.clip_counter[0], (float)gyro.clip_counter[1], (float)gyro.clip_counter[2]}}; + + // round to get reasonble clip counts per axis (after board rotation) + const uint8_t clip_x = roundf(fabsf(clipping(0))); + const uint8_t clip_y = roundf(fabsf(clipping(1))); + const uint8_t clip_z = roundf(fabsf(clipping(2))); + + _status.gyro_clipping[0] += clip_x; + _status.gyro_clipping[1] += clip_y; + _status.gyro_clipping[2] += clip_z; + + if (clip_x > 0) { + _delta_angle_clipping |= vehicle_imu_s::CLIPPING_X; + } + + if (clip_y > 0) { + _delta_angle_clipping |= vehicle_imu_s::CLIPPING_Y; + } + + if (clip_z > 0) { + _delta_angle_clipping |= vehicle_imu_s::CLIPPING_Z; + } + + _publish_status = true; + + if (_gyro_calibration.enabled() && (hrt_elapsed_time(&_last_gyro_clipping_notify_time) > 3_s)) { + // start notifying the user periodically if there's significant continuous clipping + const uint64_t clipping_total = _status.gyro_clipping[0] + _status.gyro_clipping[1] + _status.gyro_clipping[2]; + + if (clipping_total > _last_gyro_clipping_notify_total_count + 1000) { + mavlink_log_critical(&_mavlink_log_pub, "Gyro %" PRIu8 " clipping, not safe to fly!\t", _instance); + /* EVENT + * @description Land now, and check the vehicle setup. + * Clipping can lead to fly-aways. + */ + events::send(events::ID("vehicle_imu_gyro_clipping"), events::Log::Critical, + "Gyro {1} clipping, not safe to fly!", _instance); + _last_gyro_clipping_notify_time = gyro.timestamp_sample; + _last_gyro_clipping_notify_total_count = clipping_total; + } + } + } } return updated; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 2caf491b21..f6bb04c784 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -148,10 +148,15 @@ private: float _coning_norm_accum{0}; float _coning_norm_accum_total_time_s{0}; - uint8_t _delta_velocity_clipping{0}; + uint8_t _delta_angle_clipping{0}; + uint8_t _delta_velocity_clipping{0}; + + hrt_abstime _last_accel_clipping_notify_time{0}; + hrt_abstime _last_gyro_clipping_notify_time{0}; + + uint64_t _last_accel_clipping_notify_total_count{0}; + uint64_t _last_gyro_clipping_notify_total_count{0}; - hrt_abstime _last_clipping_notify_time{0}; - uint64_t _last_clipping_notify_total_count{0}; orb_advert_t _mavlink_log_pub{nullptr}; uint32_t _backup_schedule_timeout_us{20000}; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index d363ba11de..3e89e18644 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -172,6 +172,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); _last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt; + _last_sensor_data[uorb_index].gyro_clipping = imu_report.delta_angle_clipping; _last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count; _last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count; @@ -229,11 +230,14 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2, sizeof(raw.accelerometer_m_s2)); memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad)); + raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt; - raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; - raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; - raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count; - raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count; + raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; + raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count; + + raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; + raw.gyro_clipping = _last_sensor_data[gyro_best_index].gyro_clipping; + raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count; if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) { _accel.last_best_vote = (uint8_t)accel_best_index;