Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use pitch to guess which axis the user is trying to calibrate, warn about Q_TRIM_PITCH on plane

This commit is contained in:
Iampete1 2022-04-23 12:16:08 +01:00 committed by Andrew Tridgell
parent 6c24a5ff37
commit 2d2165936a
1 changed files with 42 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -11,6 +11,7 @@
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_AHRS/AP_AHRS_View.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h> #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#if !APM_BUILD_TYPE(APM_BUILD_Rover) #if !APM_BUILD_TYPE(APM_BUILD_Rover)
#include <AP_Motors/AP_Motors_Class.h> #include <AP_Motors/AP_Motors_Class.h>
@ -1189,48 +1190,51 @@ void AP_InertialSensor::periodic()
*/ */
bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, Vector3f &trim) bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, Vector3f &trim)
{ {
// allow multiple rotations, this allows us to cope with tailsitters Rotation rotation = ROTATION_NONE;
const enum Rotation rotations[] = {ROTATION_NONE, #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#ifndef HAL_BUILD_AP_PERIPH AP_AHRS_View *view = AP::ahrs().get_view();
AP::ahrs().get_view_rotation() if (view != nullptr) {
#endif // Use pitch to guess which axis the user is trying to trim
}; // 5 deg buffer to favor normal AHRS and avoid floating point funny business
bool good_trim = false; if (fabsf(view->pitch) < (fabsf(AP::ahrs().pitch)+radians(5)) ) {
Vector3f newtrim; // user is trying to calibrate view
for (const auto r : rotations) { rotation = view->get_rotation();
newtrim = trim; if (!is_zero(view->get_pitch_trim())) {
switch (r) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Cannot calibrate with Q_TRIM_PITCH set");
case ROTATION_NONE: return false;
newtrim.y = atan2f(accel_sample.x, norm(accel_sample.y, accel_sample.z)); }
newtrim.x = atan2f(-accel_sample.y, -accel_sample.z);
break;
case ROTATION_PITCH_90: {
newtrim.y = atan2f(accel_sample.z, norm(accel_sample.y, -accel_sample.x));
newtrim.z = atan2f(-accel_sample.y, accel_sample.x);
break;
}
default:
// unsupported
continue;
}
if (fabsf(newtrim.x) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
fabsf(newtrim.y) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
fabsf(newtrim.z) <= radians(HAL_INS_TRIM_LIMIT_DEG)) {
good_trim = true;
break;
} }
} }
if (!good_trim) { #endif
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "trim over maximum of 10 degrees");
Vector3f newtrim = trim;
switch (rotation) {
case ROTATION_NONE:
newtrim.y = atan2f(accel_sample.x, norm(accel_sample.y, accel_sample.z));
newtrim.x = atan2f(-accel_sample.y, -accel_sample.z);
break;
case ROTATION_PITCH_90: {
newtrim.y = atan2f(accel_sample.z, norm(accel_sample.y, -accel_sample.x));
newtrim.z = atan2f(-accel_sample.y, accel_sample.x);
break;
}
default:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "unsupported trim rotation");
return false; return false;
} }
trim = newtrim; if (fabsf(newtrim.x) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Trim OK: roll=%.2f pitch=%.2f yaw=%.2f", fabsf(newtrim.y) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
(double)degrees(trim.x), fabsf(newtrim.z) <= radians(HAL_INS_TRIM_LIMIT_DEG)) {
(double)degrees(trim.y), trim = newtrim;
(double)degrees(trim.z)); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Trim OK: roll=%.2f pitch=%.2f yaw=%.2f",
return true; (double)degrees(trim.x),
(double)degrees(trim.y),
(double)degrees(trim.z));
return true;
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "trim over maximum of 10 degrees");
return false;
} }
void void