forked from Archive/PX4-Autopilot
mag_cal: fix mag bias estimate to mag cal
- since last_us is set to 0 every time the bias is not observable, the total time was also reset -> needed 30 consecutive seconds in mag 3D to be declared "stable" - after landing, the mag_aligned_in_flight flag is reset. Using this for bias validity makes it invalid before we have a chance to save it to the calibration.
This commit is contained in:
parent
eb23779c57
commit
61036599db
|
@ -70,7 +70,7 @@ void Magnetometer::set_device_id(uint32_t device_id)
|
||||||
|
|
||||||
bool Magnetometer::set_offset(const Vector3f &offset)
|
bool Magnetometer::set_offset(const Vector3f &offset)
|
||||||
{
|
{
|
||||||
if (Vector3f(_offset - offset).longerThan(0.01f)) {
|
if (Vector3f(_offset - offset).longerThan(0.005f)) {
|
||||||
if (offset.isAllFinite()) {
|
if (offset.isAllFinite()) {
|
||||||
_offset = offset;
|
_offset = offset;
|
||||||
_calibration_count++;
|
_calibration_count++;
|
||||||
|
|
|
@ -2456,8 +2456,10 @@ void EKF2::UpdateCalibration(const hrt_abstime ×tamp, InFlightCalibration &
|
||||||
// consider bias estimates stable when all checks pass consistently and bias hasn't changed more than 10% of the limit
|
// consider bias estimates stable when all checks pass consistently and bias hasn't changed more than 10% of the limit
|
||||||
const float bias_change_limit = 0.1f * bias_limit;
|
const float bias_change_limit = 0.1f * bias_limit;
|
||||||
|
|
||||||
if ((cal.last_us != 0) && !(cal.bias - bias).longerThan(bias_change_limit)) {
|
if (!(cal.bias - bias).longerThan(bias_change_limit)) {
|
||||||
|
if (cal.last_us != 0) {
|
||||||
cal.total_time_us += timestamp - cal.last_us;
|
cal.total_time_us += timestamp - cal.last_us;
|
||||||
|
}
|
||||||
|
|
||||||
if (cal.total_time_us > 30_s) {
|
if (cal.total_time_us > 30_s) {
|
||||||
cal.cal_available = true;
|
cal.cal_available = true;
|
||||||
|
@ -2517,7 +2519,6 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp)
|
||||||
void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp)
|
void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp)
|
||||||
{
|
{
|
||||||
const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D)
|
const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D)
|
||||||
&& _ekf.control_status_flags().mag_aligned_in_flight
|
|
||||||
&& !_ekf.control_status_flags().mag_fault
|
&& !_ekf.control_status_flags().mag_fault
|
||||||
&& !_ekf.control_status_flags().mag_field_disturbed;
|
&& !_ekf.control_status_flags().mag_field_disturbed;
|
||||||
|
|
||||||
|
|
|
@ -329,7 +329,7 @@ void VehicleMagnetometer::UpdateMagCalibration()
|
||||||
|
|
||||||
if (_calibration[mag_index].set_offset(mag_cal_offset)) {
|
if (_calibration[mag_index].set_offset(mag_cal_offset)) {
|
||||||
|
|
||||||
PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])",
|
PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] (full [%.3f, %.3f, %.3f])",
|
||||||
mag_index, _calibration[mag_index].device_id(), i,
|
mag_index, _calibration[mag_index].device_id(), i,
|
||||||
(double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
|
(double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
|
||||||
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
|
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
|
||||||
|
|
Loading…
Reference in New Issue