Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

chore: return source of expected mag strength for mag cal

Browse Source
This commit is contained in:
bresch
2026-03-30 16:45:53 +02:00
parent 710adefb4c
commit 5d766d14ef
1 changed files with 16 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+16 -9
View File
@@ -227,9 +227,16 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y,
return calibrate_return_ok; return calibrate_return_ok;
} }
static float get_sphere_radius() enum class SphereRadiusSource { WMM, DEFAULT };
struct SphereRadius {
float value;
SphereRadiusSource source;
};
static SphereRadius get_sphere_radius()
{ {
// if GPS is available use real field intensity from world magnetic model // if GNSS is available use real field intensity from the World Magnetic Model (WMM)
uORB::SubscriptionMultiArray<sensor_gps_s, 3> gps_subs{ORB_ID::vehicle_gps_position}; uORB::SubscriptionMultiArray<sensor_gps_s, 3> gps_subs{ORB_ID::vehicle_gps_position};
for (auto &gps_sub : gps_subs) { for (auto &gps_sub : gps_subs) {
@@ -237,13 +244,13 @@ static float get_sphere_radius()
if (gps_sub.copy(&gps)) { if (gps_sub.copy(&gps)) {
if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) { if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
// magnetic field data returned by the geo library using the current GPS position // magnetic field data returned by the geo library using the current GNSS position (WMM)
return get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg); return {get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg), SphereRadiusSource::WMM};
} }
} }
} }
return MAG_SPHERE_RADIUS_DEFAULT; return {MAG_SPHERE_RADIUS_DEFAULT, SphereRadiusSource::DEFAULT};
} }
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data) static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data)
@@ -253,7 +260,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
mag_worker_data_t *worker_data = (mag_worker_data_t *)(data); mag_worker_data_t *worker_data = (mag_worker_data_t *)(data);
float mag_sphere_radius = get_sphere_radius(); const SphereRadius mag_sphere_radius = get_sphere_radius();
// notify user to start rotating // notify user to start rotating
set_tune(tune_control_s::TUNE_ID_SINGLE_BEEP); set_tune(tune_control_s::TUNE_ID_SINGLE_BEEP);
@@ -386,7 +393,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], 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_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); mag_sphere_radius.value);
if (!reject) { if (!reject) {
new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z}; new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
@@ -615,10 +622,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
Vector3f offdiag[MAX_MAGS]; Vector3f offdiag[MAX_MAGS];
float sphere_radius[MAX_MAGS]; float sphere_radius[MAX_MAGS];
const float mag_sphere_radius = get_sphere_radius(); const SphereRadius mag_sphere_radius = get_sphere_radius();
for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
sphere_radius[cur_mag] = mag_sphere_radius; sphere_radius[cur_mag] = mag_sphere_radius.value;
sphere[cur_mag].zero(); sphere[cur_mag].zero();
diag[cur_mag] = Vector3f{1.f, 1.f, 1.f}; diag[cur_mag] = Vector3f{1.f, 1.f, 1.f};
offdiag[cur_mag].zero(); offdiag[cur_mag].zero();