fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mag cal: allow 6, 3 and 2 side calibrations (and anything in-between with bitfields)

Browse Source
This commit is contained in:
Lorenz Meier 2016-05-09 00:32:54 +02:00
parent 4aec95b239
commit 0917a346e4
3 changed files with 48 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/modules/commander/mag_calibration.cpp
View File

@ -69,11 +69,11 @@ static const int ERROR = -1;
static const char *sensor_name = "mag";
static constexpr unsigned max_mags = 3;
static constexpr float mag_sphere_radius = 0.2f;
static constexpr unsigned int calibration_sides = 3; ///< The total number of sides
static unsigned int calibration_sides = 6; ///< The total number of sides
static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer
static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take
static constexpr float MAG_MAX_OFFSET_LEN = 0.75f; ///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions.
static constexpr float MAG_MAX_OFFSET_LEN = 1.3f; ///< The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions.
int32_t device_ids[max_mags];
bool internal[max_mags];
@ -447,40 +447,32 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Collect: Right-side up, Left Side, Nose down
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
// Collect: As defined by configuration
// start with a full mask, all six bits set
uint32_t cal_mask = (1 << 6) - 1;
param_get(param_find("CAL_MAG_SIDES"), &cal_mask);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_RIGHT));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_RIGHT));
usleep(100000);
calibration_sides = 0;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
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
worker_data.side_data_collected[i] = false;
calibration_sides++;
} else {
// mark as completed from the beginning
worker_data.side_data_collected[i] = true;
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(static_cast<enum detect_orientation_return>(i)));
usleep(100000);
}
}
for (size_t cur_mag = 0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
worker_data.sub_mag[cur_mag] = -1;
@ -606,8 +598,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
calibration_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag],
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
result = calibrate_return_ok;
}

19
src/modules/sensors/sensor_params.c
View File

@ -687,6 +687,25 @@ PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);
*/
PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
/**
* Bitfield selecting mag sides for calibration
*
* DETECT_ORIENTATION_TAIL_DOWN = 1
* DETECT_ORIENTATION_NOSE_DOWN = 2
* DETECT_ORIENTATION_LEFT = 4
* DETECT_ORIENTATION_RIGHT = 8
* DETECT_ORIENTATION_UPSIDE_DOWN = 16
* DETECT_ORIENTATION_RIGHTSIDE_UP = 32
*
* @min 34
* @max 63
* @value 34 Two side calibration
* @value 38 Three side calibration
* @value 63 Six side calibration
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63);
/**
* Primary baro ID
*

1
src/modules/sensors/sensors.cpp
View File

@ -672,6 +672,7 @@ Sensors::Sensors() :
(void)param_find("CAL_MAG0_ROT");
(void)param_find("CAL_MAG1_ROT");
(void)param_find("CAL_MAG2_ROT");
(void)param_find("CAL_MAG_SIDES");
(void)param_find("SYS_PARAM_VER");
(void)param_find("SYS_AUTOSTART");
(void)param_find("SYS_AUTOCONFIG");