diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 66e2bc0ff4..f08a63e81c 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 @@ public: 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 @@ private: // 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 d573888c77..b8c498483a 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 ce8989f2be..df53675ea8 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 fe57f712e4..12861e3fe2 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<