fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

mag calibration add notifications to rotate

Browse Source
This commit is contained in:
Daniel Agar
2017-07-08 21:54:04 +02:00
committed by Lorenz Meier
parent 1a6a871689
commit f396224d41
2 changed files with 22 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/calibration_routines.cpp
+3
View File
@@ -759,6 +759,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient));
usleep(20000);
continue;
@@ -784,8 +785,10 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
// Note that this side is complete
side_data_collected[orient] = true;
// output neutral tune
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
// temporary priority boost for the white blinking led to come trough
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1);
usleep(200000);
src/modules/commander/mag_calibration.cpp
+19 -16
View File
@@ -55,11 +55,12 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_tone_alarm.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <uORB/topics/sensor_combined.h>
static const char *sensor_name = "mag";
static constexpr unsigned max_mags = 4;
@@ -116,17 +117,17 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
// reset the learned EKF mag in-flight bias offsets which have been learned for the previous
// sensor calibration and will be invalidated by a new sensor calibration
(void)sprintf(str, "EKF2_MAGBIAS_X");
result = param_set(param_find(str), &mscale_null.x_offset);
result = param_set_no_notification(param_find(str), &mscale_null.x_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "EKF2_MAGBIAS_Y");
result = param_set(param_find(str), &mscale_null.y_offset);
result = param_set_no_notification(param_find(str), &mscale_null.y_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "EKF2_MAGBIAS_Z");
result = param_set(param_find(str), &mscale_null.z_offset);
result = param_set_no_notification(param_find(str), &mscale_null.z_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
@@ -150,42 +151,42 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
#else
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.x_offset);
result = param_set_no_notification(param_find(str), &mscale_null.x_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.y_offset);
result = param_set_no_notification(param_find(str), &mscale_null.y_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.z_offset);
result = param_set_no_notification(param_find(str), &mscale_null.z_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.x_scale);
result = param_set_no_notification(param_find(str), &mscale_null.x_scale);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.y_scale);
result = param_set_no_notification(param_find(str), &mscale_null.y_scale);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.z_scale);
result = param_set_no_notification(param_find(str), &mscale_null.z_scale);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
@@ -193,6 +194,8 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
#endif
param_notify_changes();
#ifdef __PX4_NUTTX
// Attempt to open mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
@@ -340,6 +343,9 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
mag_worker_data_t *worker_data = (mag_worker_data_t *)(data);
// notify user to start rotating
set_tune(TONE_SINGLE_BEEP_TUNE);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle around the detected orientation");
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %s %u s",
detect_orientation_str(orientation), worker_data->calibration_interval_perside_seconds);
@@ -530,8 +536,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
calibration_sides = 0;
for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) /
sizeof(worker_data.side_data_collected[0])); i++) {
for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) / sizeof(worker_data.side_data_collected[0])); i++) {
if ((cal_mask & (1 << i)) > 0) {
// mark as missing
@@ -869,12 +874,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
} else {
calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
usleep(200000);
}