forked from Archive/PX4-Autopilot
Commander: Implement calibration routines for multi-sensor setups
This commit is contained in:
Lorenz Meier
parent
ac155b0fac
commit
807cf7bd16
|
|
@ -134,7 +134,6 @@
|
|||
#include <mathlib/mathlib.h>
|
||||
#include <string.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <geo/geo.h>
|
||||
#include <conversion/rotation.h>
|
||||
|
|
@ -150,16 +149,18 @@ static const int ERROR = -1;
|
|||
|
||||
static const char *sensor_name = "accel";
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]);
|
||||
int detect_orientation(int mavlink_fd, int sub_sensor_combined);
|
||||
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
|
||||
static const unsigned max_sens = 3;
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens][3], float accel_T[max_sens][3][3]);
|
||||
int detect_orientation(int mavlink_fd, int subs[max_sens]);
|
||||
int read_accelerometer_avg(int subs[max_sens], float accel_avg[max_sens][6][3], unsigned orient, unsigned samples_num);
|
||||
int mat_invert3(float src[3][3], float dst[3][3]);
|
||||
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
|
||||
|
||||
int do_accel_calibration(int mavlink_fd)
|
||||
{
|
||||
int fd;
|
||||
int32_t device_id;
|
||||
int32_t device_id[max_sens];
|
||||
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
|
||||
|
|
@ -179,20 +180,30 @@ int do_accel_calibration(int mavlink_fd)
|
|||
|
||||
int res = OK;
|
||||
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
char str[30];
|
||||
|
||||
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
|
||||
/* reset all sensors */
|
||||
for (unsigned s = 0; s < max_sens; s++) {
|
||||
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
fd = open(str, 0);
|
||||
|
||||
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
close(fd);
|
||||
if (fd < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
|
||||
|
||||
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
float accel_offs[3];
|
||||
float accel_T[3][3];
|
||||
float accel_offs[max_sens][3];
|
||||
float accel_T[max_sens][3][3];
|
||||
|
||||
if (res == OK) {
|
||||
/* measure and calculate offsets & scales */
|
||||
|
|
@ -200,6 +211,7 @@ int do_accel_calibration(int mavlink_fd)
|
|||
}
|
||||
|
||||
if (res == OK) {
|
||||
|
||||
/* measurements completed successfully, rotate calibration values */
|
||||
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
|
||||
int32_t board_rotation_int;
|
||||
|
|
@ -208,42 +220,58 @@ int do_accel_calibration(int mavlink_fd)
|
|||
math::Matrix<3, 3> board_rotation;
|
||||
get_rot_matrix(board_rotation_id, &board_rotation);
|
||||
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
|
||||
math::Vector<3> accel_offs_vec(&accel_offs[0]);
|
||||
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
|
||||
math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
|
||||
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
|
||||
|
||||
accel_scale.x_offset = accel_offs_rotated(0);
|
||||
accel_scale.x_scale = accel_T_rotated(0, 0);
|
||||
accel_scale.y_offset = accel_offs_rotated(1);
|
||||
accel_scale.y_scale = accel_T_rotated(1, 1);
|
||||
accel_scale.z_offset = accel_offs_rotated(2);
|
||||
accel_scale.z_scale = accel_T_rotated(2, 2);
|
||||
for (unsigned i = 0; i < max_sens; i++) {
|
||||
|
||||
/* set parameters */
|
||||
if (param_set(param_find("CAL_ACC0_XOFF"), &(accel_scale.x_offset))
|
||||
|| param_set(param_find("CAL_ACC0_YOFF"), &(accel_scale.y_offset))
|
||||
|| param_set(param_find("CAL_ACC0_ZOFF"), &(accel_scale.z_offset))
|
||||
|| param_set(param_find("CAL_ACC0_XSCALE"), &(accel_scale.x_scale))
|
||||
|| param_set(param_find("CAL_ACC0_YSCALE"), &(accel_scale.y_scale))
|
||||
|| param_set(param_find("CAL_ACC0_ZSCALE"), &(accel_scale.z_scale))) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||
res = ERROR;
|
||||
}
|
||||
/* handle individual sensors, one by one */
|
||||
math::Vector<3> accel_offs_vec(&accel_offs[i][0]);
|
||||
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
|
||||
math::Matrix<3, 3> accel_T_mat(&accel_T[i][0][0]);
|
||||
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
|
||||
|
||||
if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) {
|
||||
accel_scale.x_offset = accel_offs_rotated(0);
|
||||
accel_scale.x_scale = accel_T_rotated(0, 0);
|
||||
accel_scale.y_offset = accel_offs_rotated(1);
|
||||
accel_scale.y_scale = accel_T_rotated(1, 1);
|
||||
accel_scale.z_offset = accel_offs_rotated(2);
|
||||
accel_scale.z_scale = accel_T_rotated(2, 2);
|
||||
|
||||
bool failed = false;
|
||||
|
||||
/* set parameters */
|
||||
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
|
||||
failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset)));
|
||||
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
|
||||
failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset)));
|
||||
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
|
||||
failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset)));
|
||||
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
|
||||
failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale)));
|
||||
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
|
||||
failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale)));
|
||||
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
|
||||
failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale)));
|
||||
(void)sprintf(str, "CAL_ACC%u_ID", i);
|
||||
failed |= (OK != param_set(param_find(str), &(device_id[i])));
|
||||
|
||||
if (failed) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* apply new scaling and offsets */
|
||||
fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
close(fd);
|
||||
for (unsigned s = 0; s < max_sens; s++) {
|
||||
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
|
||||
fd = open(str, 0);
|
||||
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -266,14 +294,27 @@ int do_accel_calibration(int mavlink_fd)
|
|||
return res;
|
||||
}
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens][3], float accel_T[max_sens][3][3])
|
||||
{
|
||||
const int samples_num = 2500;
|
||||
float accel_ref[6][3];
|
||||
const unsigned samples_num = 2500;
|
||||
|
||||
float accel_ref[max_sens][6][3];
|
||||
bool data_collected[6] = { false, false, false, false, false, false };
|
||||
const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
|
||||
|
||||
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
int subs[max_sens];
|
||||
|
||||
uint64_t timestamps[max_sens];
|
||||
|
||||
unsigned active_sensors = 0;
|
||||
|
||||
for (unsigned i = 0; i < max_sens; i++) {
|
||||
subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
|
||||
/* store initial timestamp - used to infer which sensors are active */
|
||||
struct accel_report arp = {};
|
||||
(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
|
||||
timestamps[i] = arp.timestamp;
|
||||
}
|
||||
|
||||
unsigned done_count = 0;
|
||||
int res = OK;
|
||||
|
|
@ -312,7 +353,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
/* allow user enough time to read the message */
|
||||
sleep(3);
|
||||
|
||||
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
|
||||
int orient = detect_orientation(mavlink_fd, &subs[0]);
|
||||
|
||||
if (orient < 0) {
|
||||
mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
|
||||
|
|
@ -329,53 +370,70 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
|
||||
mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
|
||||
sleep(1);
|
||||
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
|
||||
read_accelerometer_avg(subs, accel_ref, orient, samples_num);
|
||||
mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
|
||||
(double)accel_ref[orient][0],
|
||||
(double)accel_ref[orient][1],
|
||||
(double)accel_ref[orient][2]);
|
||||
(double)accel_ref[0][orient][0],
|
||||
(double)accel_ref[0][orient][1],
|
||||
(double)accel_ref[0][orient][2]);
|
||||
|
||||
data_collected[orient] = true;
|
||||
tune_neutral(true);
|
||||
}
|
||||
|
||||
close(sensor_combined_sub);
|
||||
/* close all subscriptions */
|
||||
for (unsigned i = 0; i < max_sens; i++) {
|
||||
/* figure out which sensors were active */
|
||||
struct accel_report arp = {};
|
||||
(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
|
||||
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
|
||||
active_sensors++;
|
||||
}
|
||||
close(subs[i]);
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* calculate offsets and transform matrix */
|
||||
res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
|
||||
for (unsigned i = 0; i < active_sensors; i++) {
|
||||
res = calculate_calibration_values(accel_ref[i], accel_T[i], accel_offs[i], CONSTANTS_ONE_G);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
|
||||
if (res != OK) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Wait for vehicle become still and detect it's orientation.
|
||||
*
|
||||
* @param mavlink_fd the MAVLink file descriptor to print output to
|
||||
* @param subs the accelerometer subscriptions. Only the first one will be used.
|
||||
*
|
||||
* @return 0..5 according to orientation when vehicle is still and ready for measurements,
|
||||
* ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
|
||||
*/
|
||||
int detect_orientation(int mavlink_fd, int sub_sensor_combined)
|
||||
int detect_orientation(int mavlink_fd, int subs[max_sens])
|
||||
{
|
||||
struct sensor_combined_s sensor;
|
||||
const unsigned ndim = 3;
|
||||
|
||||
struct accel_report sensor;
|
||||
/* exponential moving average of accel */
|
||||
float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
|
||||
float accel_ema[ndim] = { 0.0f };
|
||||
/* max-hold dispersion of accel */
|
||||
float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
|
||||
/* EMA time constant in seconds*/
|
||||
float ema_len = 0.5f;
|
||||
/* set "still" threshold to 0.25 m/s^2 */
|
||||
float still_thr2 = pow(0.25f, 2);
|
||||
float still_thr2 = powf(0.25f, 2);
|
||||
/* set accel error threshold to 5m/s^2 */
|
||||
float accel_err_thr = 5.0f;
|
||||
/* still time required in us */
|
||||
hrt_abstime still_time = 2000000;
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_sensor_combined;
|
||||
fds[0].fd = subs[0];
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
hrt_abstime t_start = hrt_absolute_time();
|
||||
|
|
@ -393,14 +451,14 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
|
|||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
|
||||
orb_copy(ORB_ID(sensor_accel), subs[0], &sensor);
|
||||
t = hrt_absolute_time();
|
||||
float dt = (t - t_prev) / 1000000.0f;
|
||||
t_prev = t;
|
||||
float w = dt / ema_len;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
float d = sensor.accelerometer_m_s2[i] - accel_ema[i];
|
||||
for (unsigned i = 0; i < ndim; i++) {
|
||||
float d = ((float*)&sensor.x)[i] - accel_ema[i];
|
||||
accel_ema[i] += d * w;
|
||||
d = d * d;
|
||||
accel_disp[i] = accel_disp[i] * (1.0f - w);
|
||||
|
|
@ -502,29 +560,43 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
|
|||
/*
|
||||
* Read specified number of accelerometer samples, calculate average and dispersion.
|
||||
*/
|
||||
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num)
|
||||
int read_accelerometer_avg(int subs[max_sens], float accel_avg[max_sens][6][3], unsigned orient, unsigned samples_num)
|
||||
{
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sensor_combined_sub;
|
||||
fds[0].events = POLLIN;
|
||||
int count = 0;
|
||||
float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
|
||||
struct pollfd fds[max_sens];
|
||||
|
||||
int errcount = 0;
|
||||
for (unsigned i = 0; i < max_sens; i++) {
|
||||
fds[i].fd = subs[i];
|
||||
fds[i].events = POLLIN;
|
||||
}
|
||||
|
||||
while (count < samples_num) {
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
unsigned counts[max_sens] = { 0 };
|
||||
float accel_sum[max_sens][3] = { 0.0f };
|
||||
|
||||
if (poll_ret == 1) {
|
||||
struct sensor_combined_s sensor;
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
|
||||
unsigned errcount = 0;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_sum[i] += sensor.accelerometer_m_s2[i];
|
||||
/* use the first sensor to pace the readout, but do per-sensor counts */
|
||||
while (counts[0] < samples_num) {
|
||||
int poll_ret = poll(&fds[0], max_sens, 1000);
|
||||
|
||||
if (poll_ret > 0) {
|
||||
|
||||
for (unsigned s = 0; s < max_sens; s++) {
|
||||
bool changed;
|
||||
orb_check(subs[s], &changed);
|
||||
|
||||
if (changed) {
|
||||
|
||||
struct accel_report arp;
|
||||
orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_sum[s][i] += ((float*)&arp.x)[i];
|
||||
}
|
||||
|
||||
counts[s]++;
|
||||
}
|
||||
}
|
||||
|
||||
count++;
|
||||
|
||||
} else {
|
||||
errcount++;
|
||||
continue;
|
||||
|
|
@ -535,8 +607,10 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
|
|||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_avg[i] = accel_sum[i] / count;
|
||||
for (unsigned s = 0; s < max_sens; s++) {
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
|
||||
}
|
||||
}
|
||||
|
||||
return OK;
|
||||
|
|
|
|||
|
|
@ -89,7 +89,7 @@ int do_airspeed_calibration(int mavlink_fd)
|
|||
};
|
||||
|
||||
bool paramreset_successful = false;
|
||||
int fd = open(AIRSPEED_DEVICE_PATH, 0);
|
||||
int fd = open(AIRSPEED0_DEVICE_PATH, 0);
|
||||
|
||||
if (fd > 0) {
|
||||
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
|
||||
|
|
@ -157,7 +157,7 @@ int do_airspeed_calibration(int mavlink_fd)
|
|||
|
||||
if (isfinite(diff_pres_offset)) {
|
||||
|
||||
int fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
|
||||
int fd_scale = open(AIRSPEED0_DEVICE_PATH, 0);
|
||||
airscale.offset_pa = diff_pres_offset;
|
||||
if (fd_scale > 0) {
|
||||
if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
|
||||
|
|
|
|||
|
|
@ -100,7 +100,7 @@ int buzzer_init()
|
|||
tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
|
||||
tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
|
||||
|
||||
buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
|
||||
buzzer = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
|
||||
|
||||
if (buzzer < 0) {
|
||||
warnx("Buzzer: open fail\n");
|
||||
|
|
@ -201,7 +201,7 @@ int led_init()
|
|||
blink_msg_end = 0;
|
||||
|
||||
/* first open normal LEDs */
|
||||
leds = open(LED_DEVICE_PATH, 0);
|
||||
leds = open(LED0_DEVICE_PATH, 0);
|
||||
|
||||
if (leds < 0) {
|
||||
warnx("LED: open fail\n");
|
||||
|
|
@ -224,10 +224,10 @@ int led_init()
|
|||
led_off(LED_AMBER);
|
||||
|
||||
/* then try RGB LEDs, this can fail on FMUv1*/
|
||||
rgbleds = open(RGBLED_DEVICE_PATH, 0);
|
||||
rgbleds = open(RGBLED0_DEVICE_PATH, 0);
|
||||
|
||||
if (rgbleds == -1) {
|
||||
warnx("No RGB LED found at " RGBLED_DEVICE_PATH);
|
||||
warnx("No RGB LED found at " RGBLED0_DEVICE_PATH);
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@
|
|||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
|
|
@ -63,20 +64,23 @@ static const char *sensor_name = "gyro";
|
|||
|
||||
int do_gyro_calibration(int mavlink_fd)
|
||||
{
|
||||
int32_t device_id;
|
||||
const unsigned max_gyros = 3;
|
||||
|
||||
int32_t device_id[3];
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_log_info(mavlink_fd, "HOLD STILL");
|
||||
|
||||
/* wait for the user to respond */
|
||||
sleep(2);
|
||||
|
||||
struct gyro_scale gyro_scale = {
|
||||
struct gyro_scale gyro_scale[max_gyros] = { {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
}
|
||||
};
|
||||
|
||||
int res = OK;
|
||||
|
|
@ -86,47 +90,75 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
mcu_unique_id(&mcu_id[0]);
|
||||
|
||||
/* store last 32bit number - not unique, but unique in a given set */
|
||||
param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
|
||||
(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
|
||||
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
int fd = open(GYRO_DEVICE_PATH, 0);
|
||||
char str[30];
|
||||
|
||||
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
|
||||
for (unsigned s = 0; s < max_gyros; s++) {
|
||||
|
||||
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
|
||||
close(fd);
|
||||
/* ensure all scale fields are initialized tha same as the first struct */
|
||||
(void)memcpy(&gyro_scale[s], &gyro_scale[0], sizeof(gyro_scale[0]));
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
int fd = open(str, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
|
||||
|
||||
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned calibration_counter[max_gyros] = { 0 };
|
||||
const unsigned calibration_count = 5000;
|
||||
|
||||
if (res == OK) {
|
||||
/* determine gyro mean values */
|
||||
const unsigned calibration_count = 5000;
|
||||
unsigned calibration_counter = 0;
|
||||
unsigned poll_errcount = 0;
|
||||
|
||||
/* subscribe to gyro sensor topic */
|
||||
int sub_sensor_gyro = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
|
||||
int sub_sensor_gyro[max_gyros];
|
||||
struct pollfd fds[max_gyros];
|
||||
|
||||
for (unsigned s = 0; s < max_gyros; s++) {
|
||||
sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
|
||||
fds[s].fd = sub_sensor_gyro[s];
|
||||
fds[s].events = POLLIN;
|
||||
}
|
||||
|
||||
struct gyro_report gyro_report;
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
/* use first gyro to pace, but count correctly per-gyro for statistics */
|
||||
while (calibration_counter[0] < calibration_count) {
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_sensor_gyro;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret > 0) {
|
||||
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
|
||||
gyro_scale.x_offset += gyro_report.x;
|
||||
gyro_scale.y_offset += gyro_report.y;
|
||||
gyro_scale.z_offset += gyro_report.z;
|
||||
calibration_counter++;
|
||||
|
||||
if (calibration_counter % (calibration_count / 20) == 0) {
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
|
||||
for (unsigned s = 0; s < max_gyros; s++) {
|
||||
bool changed;
|
||||
orb_check(sub_sensor_gyro[s], &changed);
|
||||
|
||||
if (changed) {
|
||||
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
|
||||
gyro_scale[s].x_offset += gyro_report.x;
|
||||
gyro_scale[s].y_offset += gyro_report.y;
|
||||
gyro_scale[s].z_offset += gyro_report.z;
|
||||
calibration_counter[s]++;
|
||||
}
|
||||
|
||||
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -140,16 +172,18 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
}
|
||||
}
|
||||
|
||||
close(sub_sensor_gyro);
|
||||
for (unsigned s = 0; s < max_gyros; s++) {
|
||||
close(sub_sensor_gyro[s]);
|
||||
|
||||
gyro_scale.x_offset /= calibration_count;
|
||||
gyro_scale.y_offset /= calibration_count;
|
||||
gyro_scale.z_offset /= calibration_count;
|
||||
gyro_scale[s].x_offset /= calibration_counter[s];
|
||||
gyro_scale[s].y_offset /= calibration_counter[s];
|
||||
gyro_scale[s].z_offset /= calibration_counter[s];
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* check offsets */
|
||||
if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
|
||||
if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
|
||||
res = ERROR;
|
||||
}
|
||||
|
|
@ -157,9 +191,41 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
|
||||
if (res == OK) {
|
||||
/* set offset parameters to new values */
|
||||
if (param_set(param_find("CAL_GYRO0_XOFF"), &(gyro_scale.x_offset))
|
||||
|| param_set(param_find("CAL_GYRO0_YOFF"), &(gyro_scale.y_offset))
|
||||
|| param_set(param_find("CAL_GYRO0_ZOFF"), &(gyro_scale.z_offset))) {
|
||||
bool failed = false;
|
||||
|
||||
for (unsigned s = 0; s < max_gyros; s++) {
|
||||
|
||||
/* if any reasonable amount of data is missing, skip */
|
||||
if (calibration_counter[s] < calibration_count / 2) {
|
||||
continue;
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
|
||||
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset)));
|
||||
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
|
||||
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset)));
|
||||
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
|
||||
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset)));
|
||||
(void)sprintf(str, "CAL_GYRO%u_ID", s);
|
||||
failed |= (OK != param_set(param_find(str), &(device_id[s])));
|
||||
|
||||
/* apply new scaling and offsets */
|
||||
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
|
||||
int fd = open(str, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
if (failed) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
|
||||
res = ERROR;
|
||||
}
|
||||
|
|
@ -279,8 +345,6 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
mavlink_log_info(mavlink_fd, "gyro scale calibration done");
|
||||
tune_neutral();
|
||||
|
||||
#endif
|
||||
|
||||
if (res == OK) {
|
||||
/* set scale parameters to new values */
|
||||
if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale))
|
||||
|
|
@ -289,21 +353,9 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
|
||||
res = ERROR;
|
||||
}
|
||||
if (param_set(param_find("CAL_GYRO0_ID"), &(device_id))) {
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* apply new scaling and offsets */
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (res == OK) {
|
||||
/* auto-save to EEPROM */
|
||||
|
|
|
|||
|
|
@ -63,12 +63,78 @@ static const int ERROR = -1;
|
|||
|
||||
static const char *sensor_name = "mag";
|
||||
|
||||
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id);
|
||||
|
||||
int do_mag_calibration(int mavlink_fd)
|
||||
{
|
||||
int32_t device_id;
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_log_info(mavlink_fd, "don't move system");
|
||||
const unsigned max_mags = 3;
|
||||
|
||||
int32_t device_id[max_mags];
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
sleep(1);
|
||||
|
||||
struct mag_scale mscale_null[max_mags] = {
|
||||
{
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
}
|
||||
} ;
|
||||
|
||||
int res = ERROR;
|
||||
|
||||
char str[30];
|
||||
|
||||
for (unsigned s = 0; s < max_mags; s++) {
|
||||
|
||||
mavlink_log_info(mavlink_fd, "Magnetometer #%u", s);
|
||||
sleep(3);
|
||||
|
||||
/* erase old calibration */
|
||||
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
|
||||
int fd = open(str, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
|
||||
|
||||
/* ensure all scale fields are initialized tha same as the first struct */
|
||||
(void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0]));
|
||||
|
||||
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* calibrate range */
|
||||
res = ioctl(fd, MAGIOCCALIBRATE, fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_info(mavlink_fd, "Skipped scale calibration");
|
||||
/* this is non-fatal - mark it accordingly */
|
||||
res = OK;
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
if (res == OK) {
|
||||
res = calibrate_instance(mavlink_fd, s, device_id[s]);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
|
||||
{
|
||||
/* 45 seconds */
|
||||
uint64_t calibration_interval = 45 * 1000 * 1000;
|
||||
|
||||
|
|
@ -76,80 +142,42 @@ int do_mag_calibration(int mavlink_fd)
|
|||
const unsigned int calibration_maxcount = 240;
|
||||
unsigned int calibration_counter;
|
||||
|
||||
struct mag_scale mscale_null = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
};
|
||||
|
||||
int res = OK;
|
||||
|
||||
/* erase old calibration */
|
||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
|
||||
|
||||
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* calibrate range */
|
||||
res = ioctl(fd, MAGIOCCALIBRATE, fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
|
||||
/* this is non-fatal - mark it accordingly */
|
||||
res = OK;
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
float *x = NULL;
|
||||
float *y = NULL;
|
||||
float *z = NULL;
|
||||
|
||||
if (res == OK) {
|
||||
/* allocate memory */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
|
||||
char str[30];
|
||||
int res = ERROR;
|
||||
|
||||
/* allocate memory */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
|
||||
|
||||
x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
|
||||
if (x == NULL || y == NULL || z == NULL) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
|
||||
if (x == NULL || y == NULL || z == NULL) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
|
||||
|
||||
/* clean up */
|
||||
if (x != NULL) {
|
||||
free(x);
|
||||
}
|
||||
|
||||
if (y != NULL) {
|
||||
free(y);
|
||||
}
|
||||
|
||||
if (z != NULL) {
|
||||
free(z);
|
||||
}
|
||||
|
||||
res = ERROR;
|
||||
return res;
|
||||
/* clean up */
|
||||
if (x != NULL) {
|
||||
free(x);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* exit */
|
||||
return ERROR;
|
||||
if (y != NULL) {
|
||||
free(y);
|
||||
}
|
||||
|
||||
if (z != NULL) {
|
||||
free(z);
|
||||
}
|
||||
|
||||
res = ERROR;
|
||||
return res;
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
|
||||
int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);
|
||||
|
||||
if (sub_mag < 0) {
|
||||
mavlink_log_critical(mavlink_fd, "No mag found, abort");
|
||||
|
|
@ -239,8 +267,8 @@ int do_mag_calibration(int mavlink_fd)
|
|||
if (res == OK) {
|
||||
/* apply calibration and set parameters */
|
||||
struct mag_scale mscale;
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
|
||||
int fd = open(str, 0);
|
||||
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
|
||||
|
||||
if (res != OK) {
|
||||
|
|
@ -262,35 +290,26 @@ int do_mag_calibration(int mavlink_fd)
|
|||
close(fd);
|
||||
|
||||
if (res == OK) {
|
||||
|
||||
bool failed = false;
|
||||
/* set parameters */
|
||||
if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) {
|
||||
res = ERROR;
|
||||
}
|
||||
if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) {
|
||||
res = ERROR;
|
||||
}
|
||||
(void)sprintf(str, "CAL_MAG%u_ID", s);
|
||||
failed |= (OK != param_set(param_find(str), &(device_id)));
|
||||
(void)sprintf(str, "CAL_MAG%u_XOFF", s);
|
||||
failed |= (OK != param_set(param_find(str), &(mscale.x_offset)));
|
||||
(void)sprintf(str, "CAL_MAG%u_YOFF", s);
|
||||
failed |= (OK != param_set(param_find(str), &(mscale.y_offset)));
|
||||
(void)sprintf(str, "CAL_MAG%u_ZOFF", s);
|
||||
failed |= (OK != param_set(param_find(str), &(mscale.z_offset)));
|
||||
(void)sprintf(str, "CAL_MAG%u_XSCALE", s);
|
||||
failed |= (OK != param_set(param_find(str), &(mscale.x_scale)));
|
||||
(void)sprintf(str, "CAL_MAG%u_YSCALE", s);
|
||||
failed |= (OK != param_set(param_find(str), &(mscale.y_scale)));
|
||||
(void)sprintf(str, "CAL_MAG%u_ZSCALE", s);
|
||||
failed |= (OK != param_set(param_find(str), &(mscale.z_scale)));
|
||||
|
||||
if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
|
||||
if (failed) {
|
||||
res = ERROR;
|
||||
}
|
||||
|
||||
if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) {
|
||||
res = ERROR;
|
||||
}
|
||||
|
||||
if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) {
|
||||
res = ERROR;
|
||||
}
|
||||
|
||||
if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) {
|
||||
res = ERROR;
|
||||
}
|
||||
|
||||
if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) {
|
||||
res = ERROR;
|
||||
}
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -652,7 +652,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
int ret;
|
||||
bool failed = false;
|
||||
|
||||
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||
int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
|
||||
|
|
|
|||
Loading…
Reference in New Issue