fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 08:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add gyro clipping to mirror accel clipping monitoring instances.

Browse Source
This commit is contained in:
mcsauder
2022-04-22 10:43:25 -06:00
committed by Daniel Agar
parent ea10eacb99
commit e8da98fd14
10 changed files with 126 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+50 -4
View File
@@ -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<uint8_t>(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<uint8_t>(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;
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+8 -3
View File
@@ -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};
src/modules/sensors/voted_sensors_update.cpp
+8 -4
View File
@@ -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;