diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 5f6f1e184dfdf..3cf264ce3ec99 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -160,6 +160,8 @@ def __init__(self, Feature('Compass', 'QMC5883L', 'AP_COMPASS_QMC5883L_ENABLED', 'Enable QMC5883L compasses', 1, None), Feature('Compass', 'RM3100', 'AP_COMPASS_RM3100_ENABLED', 'Enable RM3100 compasses', 1, None), Feature('Compass', 'DRONECAN_COMPASS', 'AP_COMPASS_DRONECAN_ENABLED', 'Enable DroneCAN compasses', 0, None), + Feature('Compass', 'FixedYawCal', 'AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'Enable Fixed-Yaw Compass Calibration', 1, None), # noqa + Feature('Compass', 'CompassLearn', 'COMPASS_LEARN_ENABLED', 'Enable In-Flight Compass Learning', 1, "FixedYawCal"), Feature('Gimbal', 'MOUNT', 'HAL_MOUNT_ENABLED', 'Enable Mount', 0, None), Feature('Gimbal', 'ALEXMOS', 'HAL_MOUNT_ALEXMOS_ENABLED', 'Enable Alexmos Gimbal', 0, "MOUNT"), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index c50d87db273e0..5a8665bf7faf7 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -242,6 +242,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('FORCE_APJ_DEFAULT_PARAMETERS', 'AP_Param::param_defaults_data'), ('HAL_BUTTON_ENABLED', 'AP_Button::update'), ('HAL_LOGGING_ENABLED', 'AP_Logger::Init'), + ('AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'AP_Compass::mag_cal_fixed_yaw'), + ('COMPASS_LEARN_ENABLED', 'CompassLearn::update'), ] def progress(self, msg): diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 66e2bc0ff409f..f08a63e81c96b 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -39,7 +39,7 @@ #define COMPASS_MOT_ENABLED 1 #endif #ifndef COMPASS_LEARN_ENABLED -#define COMPASS_LEARN_ENABLED 1 +#define COMPASS_LEARN_ENABLED AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED #endif // define default compass calibration fitness and consistency checks @@ -342,12 +342,14 @@ friend class AP_Compass_Backend; uint8_t get_filter_range() const { return uint8_t(_filter_range.get()); } +#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* fast compass calibration given vehicle position and yaw */ - MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, - float lat_deg, float lon_deg, - bool force_use=false); + bool mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, + float lat_deg, float lon_deg, + bool force_use=false); +#endif #if AP_COMPASS_MSP_ENABLED void handle_msp(const MSP::msp_compass_data_message_t &pkt); @@ -405,12 +407,14 @@ friend class AP_Compass_Backend; // see if we already have probed a i2c driver by bus number and address bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const; +#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* get mag field with the effects of offsets, diagonals and off-diagonals removed */ bool get_uncorrected_field(uint8_t instance, Vector3f &field) const; - +#endif + #if COMPASS_CAL_ENABLED //keep track of which calibrators have been saved RestrictIDTypeArray _cal_saved; diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index d573888c77395..b8c498483ac05 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -504,7 +504,7 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const This assumes that the compass is correctly scaled in milliGauss */ -MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, +bool Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, float lat_deg, float lon_deg, bool force_use) { _reset_compass_id(); @@ -514,7 +514,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, if (!AP::ahrs().get_location(loc)) { if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: no position available"); - return MAV_RESULT_FAILED; + return false; } loc = AP::gps().location(); } @@ -528,7 +528,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, float inclination; if (!AP_Declination::get_mag_field_ef(lat_deg, lon_deg, intensity, declination, inclination)) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: WMM table error"); - return MAV_RESULT_FAILED; + return false; } // create a field vector and rotate to the required orientation @@ -553,13 +553,13 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, } if (!healthy(i)) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: unhealthy\n", i); - return MAV_RESULT_FAILED; + return false; } Vector3f measurement; if (!get_uncorrected_field(i, measurement)) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: bad uncorrected field", i); - return MAV_RESULT_FAILED; + return false; } Vector3f offsets = field - measurement; @@ -572,7 +572,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, #endif } - return MAV_RESULT_ACCEPTED; + return true; } #endif // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h index ce8989f2be591..df53675ea8f1e 100644 --- a/libraries/AP_Compass/AP_Compass_config.h +++ b/libraries/AP_Compass/AP_Compass_config.h @@ -19,7 +19,7 @@ #endif #ifndef AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED -#define AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED AP_GPS_ENABLED +#define AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED AP_COMPASS_ENABLED && AP_GPS_ENABLED #endif #define COMPASS_MAX_SCALE_FACTOR 1.5 diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index fe57f712e4193..12861e3fe29ca 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -60,8 +60,8 @@ void CompassLearn::update(void) return; } - const auto result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U<