Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: make compass.mag_cal_fixed_yaw return boolean

Browse Source 
this method could be used from a transmitter without GCS enabled, for example
This commit is contained in:
Peter Barker 2024-02-02 11:12:03 +11:00 committed by Andrew Tridgell
parent 8cc662163b
commit 084b0aea24
4 changed files with 18 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Compass/AP_Compass.h
View File

@ -39,7 +39,7 @@
#define COMPASS_MOT_ENABLED 1 #define COMPASS_MOT_ENABLED 1
#endif #endif
#ifndef COMPASS_LEARN_ENABLED #ifndef COMPASS_LEARN_ENABLED
#define COMPASS_LEARN_ENABLED 1 #define COMPASS_LEARN_ENABLED AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
#endif #endif
// define default compass calibration fitness and consistency checks // 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()); } 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 fast compass calibration given vehicle position and yaw
*/ */
MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, bool mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
float lat_deg, float lon_deg, float lat_deg, float lon_deg,
bool force_use=false); bool force_use=false);
#endif
#if AP_COMPASS_MSP_ENABLED #if AP_COMPASS_MSP_ENABLED
void handle_msp(const MSP::msp_compass_data_message_t &pkt); 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 // 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; 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 get mag field with the effects of offsets, diagonals and
off-diagonals removed off-diagonals removed
*/ */
bool get_uncorrected_field(uint8_t instance, Vector3f &field) const; bool get_uncorrected_field(uint8_t instance, Vector3f &field) const;
#endif
#if COMPASS_CAL_ENABLED #if COMPASS_CAL_ENABLED
//keep track of which calibrators have been saved //keep track of which calibrators have been saved
RestrictIDTypeArray<bool, COMPASS_MAX_INSTANCES, Priority> _cal_saved; RestrictIDTypeArray<bool, COMPASS_MAX_INSTANCES, Priority> _cal_saved;

12
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -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 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) float lat_deg, float lon_deg, bool force_use)
{ {
_reset_compass_id(); _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::ahrs().get_location(loc)) {
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: no position available"); GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: no position available");
return MAV_RESULT_FAILED; return false;
} }
loc = AP::gps().location(); loc = AP::gps().location();
} }
@ -528,7 +528,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
float inclination; float inclination;
if (!AP_Declination::get_mag_field_ef(lat_deg, lon_deg, intensity, declination, 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"); 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 // 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)) { if (!healthy(i)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: unhealthy\n", i); GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: unhealthy\n", i);
return MAV_RESULT_FAILED; return false;
} }
Vector3f measurement; Vector3f measurement;
if (!get_uncorrected_field(i, measurement)) { if (!get_uncorrected_field(i, measurement)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: bad uncorrected field", i); GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: bad uncorrected field", i);
return MAV_RESULT_FAILED; return false;
} }
Vector3f offsets = field - measurement; Vector3f offsets = field - measurement;
@ -572,7 +572,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
#endif #endif
} }
return MAV_RESULT_ACCEPTED; return true;
} }
#endif // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED #endif // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED

2
libraries/AP_Compass/AP_Compass_config.h
View File

@ -19,7 +19,7 @@
#endif #endif
#ifndef AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED #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 #endif
#define COMPASS_MAX_SCALE_FACTOR 1.5 #define COMPASS_MAX_SCALE_FACTOR 1.5

4
libraries/AP_Compass/Compass_learn.cpp
View File

@ -60,8 +60,8 @@ void CompassLearn::update(void)
return; return;
} }
const auto result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U<<HAL_COMPASS_MAX_SENSORS)-1, 0, 0, true); const bool result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U<<HAL_COMPASS_MAX_SENSORS)-1, 0, 0, true);
if (result == MAV_RESULT_ACCEPTED) { if (result) {
AP_Notify::flags.compass_cal_running = false; AP_Notify::flags.compass_cal_running = false;
compass.set_learn_type(Compass::LEARN_NONE, true); compass.set_learn_type(Compass::LEARN_NONE, true);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CompassLearn: Finished"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CompassLearn: Finished");