Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: Factor accel cal work out to INS library

Browse Source
This commit is contained in:
Michael du Breuil 2023-03-25 11:23:31 +11:00 committed by Peter Barker
parent b76d6d827f
commit a9d696752d
2 changed files with 7 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -741,8 +741,6 @@ private:
};
void log_mavlink_stats();
uint32_t last_accel_cal_ms; // used to rate limit accel cals for bad links
MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode);
// send a (textual) message to the GCS that a received message has

47
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -4247,20 +4247,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_set_sensor_offsets(const mavlin
return MAV_RESULT_ACCEPTED;
}
bool GCS_MAVLINK::calibrate_gyros()
{
#if AP_INERTIALSENSOR_ENABLED
AP::ins().init_gyro();
if (!AP::ins().gyro_calibrated_ok_all()) {
return false;
}
AP::ahrs().reset_gyro_drift();
return true;
#else
return false;
#endif
}
MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
{
// fast barometer calibration
@ -4289,9 +4275,13 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm
EXPECT_DELAY_MS(30000);
if (is_equal(packet.param1,1.0f)) {
if (!calibrate_gyros()) {
#if AP_INERTIALSENSOR_ENABLED
if (!AP::ins().calibrate_gyros()) {
return MAV_RESULT_FAILED;
}
#else
return MAV_RESULT_UNSUPPORTED;
#endif
return MAV_RESULT_ACCEPTED;
}
@ -4304,7 +4294,7 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm
#if HAL_INS_ACCELCAL_ENABLED
if (is_equal(packet.param5,1.0f)) {
// start with gyro calibration
if (!calibrate_gyros()) {
if (!AP::ins().calibrate_gyros()) {
return MAV_RESULT_FAILED;
}
// start accel cal
@ -4315,35 +4305,12 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm
#endif
#if AP_INERTIALSENSOR_ENABLED
const uint32_t now = AP_HAL::millis();
if (is_equal(packet.param5,2.0f)) {
// reject any time we've done a calibration recently
if ((now - last_accel_cal_ms) < 5000) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}
if (!calibrate_gyros()) {
last_accel_cal_ms = AP_HAL::millis();
return MAV_RESULT_FAILED;
}
Vector3f trim_rad = AP::ahrs().get_trim();
if (!AP::ins().calibrate_trim(trim_rad)) {
last_accel_cal_ms = AP_HAL::millis();
return MAV_RESULT_FAILED;
}
// reset ahrs's trim to suggested values from calibration routine
AP::ahrs().set_trim(trim_rad);
last_accel_cal_ms = AP_HAL::millis();
return MAV_RESULT_ACCEPTED;
return AP::ins().calibrate_trim();
}
if (is_equal(packet.param5,4.0f)) {
if ((now - last_accel_cal_ms) < 5000) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}
// simple accel calibration
last_accel_cal_ms = AP_HAL::millis();
return AP::ins().simple_accel_cal();
}