fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 02:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: mag calibration use expected WMM sphere radius to reject samples if GPS is available

Browse Source
This commit is contained in:
Daniel Agar
2020-09-20 12:22:56 -04:00
committed by GitHub
parent 727b0cf530
commit 09af5e0637
1 changed files with 32 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+32 -23
View File
@@ -57,6 +57,7 @@
#include <lib/systemlib/err.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -135,9 +136,9 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
}
static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count,
unsigned max_count)
unsigned max_count, float mag_sphere_radius)
{
float min_sample_dist = fabsf(5.4f * MAG_SPHERE_RADIUS_DEFAULT / sqrtf(max_count)) / 3.0f;
float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
for (size_t i = 0; i < count; i++) {
float dx = sx - x[i];
@@ -222,6 +223,28 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y,
return calibrate_return_ok;
}
static float get_sphere_radius()
{
// if GPS is available use real field intensity from world magnetic model
uORB::SubscriptionMultiArray<vehicle_gps_position_s> gps_subs{ORB_ID::vehicle_gps_position};
for (auto &gps_sub : gps_subs) {
vehicle_gps_position_s gps;
if (gps_sub.copy(&gps)) {
if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
const double lat = gps.lat / 1.e7;
const double lon = gps.lon / 1.e7;
// magnetic field data returned by the geo library using the current GPS position
return get_mag_strength_gauss(lat, lon);
}
}
}
return MAG_SPHERE_RADIUS_DEFAULT;
}
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data)
{
const hrt_abstime calibration_started = hrt_absolute_time();
@@ -229,6 +252,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
mag_worker_data_t *worker_data = (mag_worker_data_t *)(data);
float mag_sphere_radius = get_sphere_radius();
// notify user to start rotating
set_tune(TONE_SINGLE_BEEP_TUNE);
@@ -334,7 +359,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
bool reject = reject_sample(mag.x, mag.y, mag.z,
worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
worker_data->calibration_counter_total[cur_mag],
worker_data->calibration_sides * worker_data->calibration_points_perside);
worker_data->calibration_sides * worker_data->calibration_points_perside,
mag_sphere_radius);
if (!reject) {
new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
@@ -487,33 +513,16 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
Vector3f offdiag[MAX_MAGS];
float sphere_radius[MAX_MAGS];
const float mag_sphere_radius = get_sphere_radius();
for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
sphere_radius[cur_mag] = MAG_SPHERE_RADIUS_DEFAULT;
sphere_radius[cur_mag] = mag_sphere_radius;
sphere[cur_mag].zero();
diag[cur_mag] = Vector3f{1.f, 1.f, 1.f};
offdiag[cur_mag].zero();
}
if (result == calibrate_return_ok) {
// if GPS is available use real field intensity from world magnetic model
uORB::SubscriptionData<vehicle_gps_position_s> gps_sub{ORB_ID(vehicle_gps_position)};
const vehicle_gps_position_s &gps = gps_sub.get();
if (hrt_elapsed_time(&gps.timestamp) < 10_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
const double lat = gps.lat / 1.e7;
const double lon = gps.lon / 1.e7;
// magnetic field data returned by the geo library using the current GPS position
const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
PX4_INFO("[cal] using current GPS for field strength: %.4f Gauss", (double)mag_strength_gps);
for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
sphere_radius[cur_mag] = mag_strength_gps;
}
}
// Sphere fit the data to get calibration values
for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
if (worker_data.calibration[cur_mag].device_id() != 0) {