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

AP_Compass: make compass.mag_cal_fixed_yaw return boolean 

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
#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<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
*/
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

2
libraries/AP_Compass/AP_Compass_config.h
View File

@ -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

4
libraries/AP_Compass/Compass_learn.cpp
View File

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