diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 5766ddf096..5efe23adb7 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -336,7 +336,8 @@ public: 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); + float lat_deg, float lon_deg, + bool force_use=false); #if HAL_MSP_COMPASS_ENABLED void handle_msp(const MSP::msp_compass_data_message_t &pkt); diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index e156ec433a..af05912c3c 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -483,7 +483,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, - float lat_deg, float lon_deg) + float lat_deg, float lon_deg, bool force_use) { _reset_compass_id(); if (is_zero(lat_deg) && is_zero(lon_deg)) { @@ -526,7 +526,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, // skip this compass continue; } - if (!use_for_yaw(i)) { + if (!force_use && !use_for_yaw(i)) { continue; } if (!healthy(i)) { diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index f383db1f11..a9b3e9683c 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -1,268 +1,68 @@ -#include <AP_Math/AP_Math.h> #include <AP_AHRS/AP_AHRS.h> #include <AP_Compass/AP_Compass.h> -#include <AP_Declination/AP_Declination.h> -#include <AP_Logger/AP_Logger.h> #include "Compass_learn.h" #include <GCS_MAVLink/GCS.h> - -#include <stdio.h> #include <AP_Vehicle/AP_Vehicle.h> +#include <AP_NavEKF/EKFGSF_yaw.h> #if COMPASS_LEARN_ENABLED extern const AP_HAL::HAL &hal; -static const uint8_t COMPASS_LEARN_NUM_SAMPLES = 30; -static const uint8_t COMPASS_LEARN_BEST_ERROR_THRESHOLD = 50; -static const uint8_t COMPASS_LEARN_WORST_ERROR_THRESHOLD = 65; - // constructor CompassLearn::CompassLearn(Compass &_compass) : compass(_compass) { gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised"); - for (Compass::Priority i(0); i<compass.get_count(); i++) { - if (compass._use_for_yaw[Compass::Priority(i)]) { - // reset scale factors, we can't learn scale factors in - // flight - compass.set_and_save_scale_factor(uint8_t(i), 0.0); - } - } } +// accuracy threshold applied for GSF yaw estimate +#define YAW_ACCURACY_THRESHOLD_DEG 5.0 + /* update when new compass sample available */ void CompassLearn::update(void) { const AP_Vehicle *vehicle = AP::vehicle(); - if (converged || compass.get_learn_type() != Compass::LEARN_INFLIGHT || - !hal.util->get_soft_armed() || vehicle->get_time_flying_ms() < 3000) { + if (compass.get_learn_type() != Compass::LEARN_INFLIGHT || + !hal.util->get_soft_armed() || + vehicle->get_time_flying_ms() < 3000) { // only learn when flying and with enough time to be clear of // the ground return; } - const AP_AHRS &ahrs = AP::ahrs(); - if (!have_earth_field) { - Location loc; - if (!ahrs.get_position(loc)) { - // need to wait till we have a global position - return; - } - - // setup the expected earth field in mGauss at this location - mag_ef = AP_Declination::get_earth_field_ga(loc) * 1000; - have_earth_field = true; - - // form eliptical correction matrix and invert it. This is - // needed to remove the effects of the eliptical correction - // when calculating new offsets - const Vector3f &diagonals = compass.get_diagonals(0); - const Vector3f &offdiagonals = compass.get_offdiagonals(0); - mat = Matrix3f( - diagonals.x, offdiagonals.x, offdiagonals.y, - offdiagonals.x, diagonals.y, offdiagonals.z, - offdiagonals.y, offdiagonals.z, diagonals.z - ); - if (!mat.invert()) { - // if we can't invert, use the identity matrix - mat.identity(); - } - - // set initial error to field intensity - float intensity = mag_ef.length(); - for (uint16_t i=0; i<num_sectors; i++) { - errors[i] = intensity; - } - - gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: have earth field"); - hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void)); + const auto &ahrs = AP::ahrs(); + const auto *gsf = ahrs.get_yaw_estimator(); + if (gsf == nullptr) { + // no GSF available + return; + } + if (degrees(fabsf(ahrs.get_pitch())) > 50) { + // we don't want to be too close to nose up, or yaw gets + // problematic. Tailsitters need to wait till they are in + // forward flight + return; } AP_Notify::flags.compass_cal_running = true; - if (sample_available) { - // last sample still being processed by IO thread + ftype yaw_rad, yaw_variance; + if (!gsf->getYawData(yaw_rad, yaw_variance) || + !is_positive(yaw_variance) || + yaw_variance >= sq(radians(YAW_ACCURACY_THRESHOLD_DEG))) { + // not converged return; } - Vector3f field = compass.get_field(0); - Vector3f field_change = field - last_field; - if (field_change.length() < min_field_change) { - return; - } - - { - WITH_SEMAPHORE(sem); - // give a sample to the backend to process - new_sample.field = field; - new_sample.offsets = compass.get_offsets(0); - new_sample.attitude = Vector3f(ahrs.roll, ahrs.pitch, ahrs.yaw); - sample_available = true; - last_field = field; - num_samples++; - } - - if (sample_available) { - // @LoggerMessage: COFS - // @Description: Current compass learn offsets - // @Field: TimeUS: Time since system startup - // @Field: OfsX: best learnt offset, x-axis - // @Field: OfsY: best learnt offset, y-axis - // @Field: OfsZ: best learnt offset, z-axis - // @Field: Var: error of best offset vector - // @Field: Yaw: best learnt yaw - // @Field: WVar: error of best learn yaw - // @Field: N: number of samples used - AP::logger().WriteStreaming("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI", - AP_HAL::micros64(), - (double)best_offsets.x, - (double)best_offsets.y, - (double)best_offsets.z, - (double)best_error, - (double)best_yaw_deg, - (double)worst_error, - num_samples); - } - - if (!converged) { - WITH_SEMAPHORE(sem); - - // set offsets to current best guess - compass.set_offsets(0, best_offsets); - - // set non-primary offsets to match primary - Vector3f field_primary = compass.get_field(0); - for (uint8_t i=1; i<compass.get_count(); i++) { - if (!compass._use_for_yaw[Compass::Priority(i)]) { - continue; - } - Vector3f field2 = compass.get_field(i); - Vector3f new_offsets = compass.get_offsets(i) + (field_primary - field2); - compass.set_offsets(i, new_offsets); - } - - // stop updating the offsets once converged - if (num_samples > COMPASS_LEARN_NUM_SAMPLES && - best_error < COMPASS_LEARN_BEST_ERROR_THRESHOLD && - worst_error > COMPASS_LEARN_WORST_ERROR_THRESHOLD) { - // set the offsets and enable compass for EKF use. Let the - // EKF learn the remaining compass offset error - for (uint8_t i=0; i<compass.get_count(); i++) { - if (compass._use_for_yaw[Compass::Priority(i)]) { - compass.save_offsets(i); - compass.set_and_save_scale_factor(i, 0.0); - } - } - compass.set_learn_type(Compass::LEARN_NONE, true); - // setup so use can trigger it again - converged = false; - sample_available = false; - num_samples = 0; - have_earth_field = false; - for (auto &v : predicted_offsets) { - v.zero(); - } - worst_error = 0; - best_error = 0; - best_yaw_deg = 0; - best_offsets.zero(); - gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: finished"); - AP_Notify::flags.compass_cal_running = false; - AP_Notify::events.compass_cal_saved = true; - } - } -} - -/* - we run the math intensive calculations in the IO thread - */ -void CompassLearn::io_timer(void) -{ - if (!sample_available) { - return; - } - - struct sample s; - - { - WITH_SEMAPHORE(sem); - s = new_sample; - sample_available = false; - } - - process_sample(s); -} - -/* - process a new compass sample - */ -void CompassLearn::process_sample(const struct sample &s) -{ - uint16_t besti = 0; - float bestv = 0, worstv=0; - - /* - we run through the 72 possible yaw error values, and for each - one we calculate a value for the compass offsets if that yaw - error is correct. - */ - for (uint16_t i=0; i<num_sectors; i++) { - float yaw_err_deg = i*(360/num_sectors); - - // form rotation matrix for the euler attitude - Matrix3f dcm; - dcm.from_euler(s.attitude.x, s.attitude.y, wrap_2PI(s.attitude.z + radians(yaw_err_deg))); - - // calculate the field we would expect to get if this yaw error is correct - Vector3f expected_field = dcm.transposed() * mag_ef; - - // calculate a value for the compass offsets for this yaw error - Vector3f v1 = mat * s.field; - Vector3f v2 = mat * expected_field; - Vector3f offsets = (v2 - v1) + s.offsets; - float delta = (offsets - predicted_offsets[i]).length(); - - if (num_samples == 1) { - predicted_offsets[i] = offsets; - } else { - // lowpass the predicted offsets and the error - const float learn_rate = 0.92f; - predicted_offsets[i] = predicted_offsets[i] * learn_rate + offsets * (1-learn_rate); - errors[i] = errors[i] * learn_rate + delta * (1-learn_rate); - } - - // keep track of the current best prediction - if (i == 0 || errors[i] < bestv) { - besti = i; - bestv = errors[i]; - } - // also keep the worst error. This is used as part of the convergence test - if (i == 0 || errors[i] > worstv) { - worstv = errors[i]; - } - } - - WITH_SEMAPHORE(sem); - - // pass the current estimate to the front-end - best_offsets = predicted_offsets[besti]; - best_error = bestv; - worst_error = worstv; - best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors)); - - // send current learn state to gcs - const uint32_t now = AP_HAL::millis(); - if (!converged && now - last_learn_progress_sent_ms >= 5000) { - float percent = (MIN(num_samples / COMPASS_LEARN_NUM_SAMPLES, 1.0f) + - MIN(COMPASS_LEARN_BEST_ERROR_THRESHOLD / (best_error + 1.0f), 1.0f) + - MIN(worst_error / COMPASS_LEARN_WORST_ERROR_THRESHOLD, 1.0f)) / 3.0f * 100.f; - gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: %d%%", (int) percent); - last_learn_progress_sent_ms = now; + 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) { + AP_Notify::flags.compass_cal_running = false; + compass.set_learn_type(Compass::LEARN_NONE, true); + gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Finished"); } } diff --git a/libraries/AP_Compass/Compass_learn.h b/libraries/AP_Compass/Compass_learn.h index 42acb1bcc0..f24de57fef 100644 --- a/libraries/AP_Compass/Compass_learn.h +++ b/libraries/AP_Compass/Compass_learn.h @@ -3,7 +3,7 @@ #include <AP_AHRS/AP_AHRS.h> /* - compass learning using magnetic field tables from AP_Declination + compass learning using magnetic field tables from AP_Declination and GSF */ class CompassLearn { @@ -15,46 +15,4 @@ public: private: Compass &compass; - bool have_earth_field; - - // 5 degree resolution - static const uint16_t num_sectors = 72; - - Vector3f predicted_offsets[num_sectors]; - float errors[num_sectors]; - uint32_t num_samples; - - // earth field - Vector3f mag_ef; - - // semaphore for access to shared data with IO thread - HAL_Semaphore sem; - - struct sample { - // milliGauss body field and offsets - Vector3f field; - Vector3f offsets; - - // euler radians attitude - Vector3f attitude; - }; - - Matrix3f mat; - - struct sample new_sample; - bool sample_available; - Vector3f last_field; - static const uint32_t min_field_change = 60; - - Vector3f best_offsets; - float best_error; - float best_yaw_deg; - float worst_error; - bool converged; - - // notification - uint32_t last_learn_progress_sent_ms; - - void io_timer(void); - void process_sample(const struct sample &s); };