Commit 807cf7bd authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Commander: Implement calibration routines for multi-sensor setups

parent ac155b0f
......@@ -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];
/* 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);
if (fd < 0) {
continue;
}
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
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);
/* 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;
}
if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) {
for (unsigned i = 0; i < max_sens; i++) {
/* 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;
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);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
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);
}
}
}
......@@ -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,28 +560,42 @@ 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);
count++;
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]++;
}
}
} else {
errcount++;
......@@ -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]));
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);
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");