dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Use new calibrate_from_orientation api

This commit is contained in:
Don Gagne 2015-03-23 19:01:53 -07:00 committed by Lorenz Meier
parent fdc053e883
commit 9d61e3a7db
1 changed files with 96 additions and 279 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

359
src/modules/commander/accelerometer_calibration.cpp
View File

@ -120,8 +120,11 @@
* @author Anton Babushkin <anton.babushkin@me.com> * @author Anton Babushkin <anton.babushkin@me.com>
*/ */
// FIXME: Can some of these headers move out with detect_ move?
#include "accelerometer_calibration.h" #include "accelerometer_calibration.h"
#include "calibration_messages.h" #include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h" #include "commander_helper.h"
#include <unistd.h> #include <unistd.h>
@ -149,18 +152,24 @@ static const int ERROR = -1;
static const char *sensor_name = "accel"; static const char *sensor_name = "accel";
static const unsigned max_sens = 3; int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors);
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 mat_invert3(float src[3][3], float dst[3][3]);
int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g); int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
int accel_calibration_worker(detect_orientation_return orientation, void* worker_data);
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
unsigned done_count;
int subs[max_accel_sens];
float accel_ref[max_accel_sens][detect_orientation_side_count][3];
} accel_worker_data_t;
int do_accel_calibration(int mavlink_fd) int do_accel_calibration(int mavlink_fd)
{ {
int fd; int fd;
int32_t device_id[max_sens]; int32_t device_id[max_accel_sens];
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
@ -178,7 +187,7 @@ int do_accel_calibration(int mavlink_fd)
char str[30]; char str[30];
/* reset all sensors */ /* reset all sensors */
for (unsigned s = 0; s < max_sens; s++) { for (unsigned s = 0; s < max_accel_sens; s++) {
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */ /* reset all offsets to zero and all scales to one */
fd = open(str, 0); fd = open(str, 0);
@ -193,12 +202,12 @@ int do_accel_calibration(int mavlink_fd)
close(fd); close(fd);
if (res != OK) { if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
} }
} }
float accel_offs[max_sens][3]; float accel_offs[max_accel_sens][3];
float accel_T[max_sens][3][3]; float accel_T[max_accel_sens][3][3];
unsigned active_sensors; unsigned active_sensors;
if (res == OK) { if (res == OK) {
@ -239,22 +248,22 @@ int do_accel_calibration(int mavlink_fd)
/* set parameters */ /* set parameters */
(void)sprintf(str, "CAL_ACC%u_XOFF", i); (void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset))); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", i); (void)sprintf(str, "CAL_ACC%u_YOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset))); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", i); (void)sprintf(str, "CAL_ACC%u_ZOFF", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset))); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", i); (void)sprintf(str, "CAL_ACC%u_XSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale))); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", i); (void)sprintf(str, "CAL_ACC%u_YSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale))); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale))); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", i); (void)sprintf(str, "CAL_ACC%u_ID", i);
failed |= (OK != param_set(param_find(str), &(device_id[i]))); failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
if (failed) { if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i);
return ERROR; return ERROR;
} }
@ -270,7 +279,7 @@ int do_accel_calibration(int mavlink_fd)
} }
if (res != OK) { if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i);
} }
} }
@ -291,309 +300,117 @@ int do_accel_calibration(int mavlink_fd)
return res; return res;
} }
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors) int accel_calibration_worker(detect_orientation_return orientation, void* data)
{ {
const unsigned samples_num = 3000; const unsigned samples_num = 3000;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation));
sleep(1);
read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count);
return OK;
}
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
int result = OK;
*active_sensors = 0; *active_sensors = 0;
float accel_ref[max_sens][6][3]; accel_worker_data_t worker_data;
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
int subs[max_sens]; worker_data.mavlink_fd = mavlink_fd;
worker_data.done_count = 0;
uint64_t timestamps[max_sens]; bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
for (unsigned i = 0; i < max_sens; i++) { // Initialize subs to error condition so we know which ones are open and which are not
subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); for (size_t i=0; i<max_accel_sens; i++) {
/* store initial timestamp - used to infer which sensors are active */ worker_data.subs[i] = -1;
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
timestamps[i] = arp.timestamp;
} }
unsigned done_count = 0; uint64_t timestamps[max_accel_sens];
int res = OK;
while (true) { for (unsigned i = 0; i < max_accel_sens; i++) {
bool done = true; worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
unsigned old_done_count = done_count; if (worker_data.subs[i] < 0) {
done_count = 0; result = ERROR;
for (int i = 0; i < 6; i++) {
if (data_collected[i]) {
done_count++;
} else {
done = false;
}
}
if (old_done_count != done_count) {
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
}
if (done) {
break; break;
} }
/* inform user which axes are still needed */ /* store initial timestamp - used to infer which sensors are active */
mavlink_and_console_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", struct accel_report arp = {};
(!data_collected[5]) ? "down " : "", (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
(!data_collected[0]) ? "back " : "", timestamps[i] = arp.timestamp;
(!data_collected[1]) ? "front " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "");
/* allow user enough time to read the message */
sleep(1);
int orient = detect_orientation(mavlink_fd, subs);
if (orient < 0) {
mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still...");
sleep(2);
continue;
} }
/* inform user about already handled side */ if (result == OK) {
if (data_collected[orient]) { result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
sleep(1);
continue;
}
mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
sleep(1);
read_accelerometer_avg(subs, accel_ref, orient, samples_num);
mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
usleep(100000);
mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %8.4f %8.4f %8.4f ]", orientation_strs[orient],
(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 all subscriptions */ /* close all subscriptions */
for (unsigned i = 0; i < max_sens; i++) { for (unsigned i = 0; i < max_accel_sens; i++) {
if (worker_data.subs[i] >= 0) {
/* figure out which sensors were active */ /* figure out which sensors were active */
struct accel_report arp = {}; struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp); (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++; (*active_sensors)++;
} }
close(subs[i]); close(worker_data.subs[i]);
}
} }
if (res == OK) { if (result == OK) {
/* calculate offsets and transform matrix */ /* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) { for (unsigned i = 0; i < (*active_sensors); i++) {
res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (res != OK) { if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
break; break;
} }
} }
} }
return res; return result;
}
/**
* 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 (&subs)[max_sens])
{
const unsigned ndim = 3;
struct accel_report sensor;
/* exponential moving average of accel */
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 = 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 = subs[0];
fds[0].events = POLLIN;
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
hrt_abstime timeout = 30000000;
hrt_abstime t_timeout = t_start + timeout;
hrt_abstime t = t_start;
hrt_abstime t_prev = t_start;
hrt_abstime t_still = 0;
unsigned poll_errcount = 0;
while (true) {
/* wait blocking for new data */
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
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 (unsigned i = 0; i < ndim; i++) {
float di = 0.0f;
switch (i) {
case 0:
di = sensor.x;
break;
case 1:
di = sensor.y;
break;
case 2:
di = sensor.z;
break;
}
float d = di - accel_ema[i];
accel_ema[i] += d * w;
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
if (accel_disp[0] < still_thr2 &&
accel_disp[1] < still_thr2 &&
accel_disp[2] < still_thr2) {
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
} else {
/* still since t_still */
if (t > t_still + still_time) {
/* vehicle is still, exit from the loop to detection of its orientation */
break;
}
}
} else if (accel_disp[0] > still_thr2 * 4.0f ||
accel_disp[1] > still_thr2 * 4.0f ||
accel_disp[2] > still_thr2 * 4.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
sleep(3);
t_still = 0;
}
}
} else if (poll_ret == 0) {
poll_errcount++;
}
if (t > t_timeout) {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
return ERROR;
}
}
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return 0; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return 1; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return 2; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return 3; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return 4; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return 5; // [ 0, 0, -g ]
}
mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
return ERROR; // Can't detect orientation
} }
/* /*
* Read specified number of accelerometer samples, calculate average and dispersion. * Read specified number of accelerometer samples, calculate average and dispersion.
*/ */
int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num) int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{ {
struct pollfd fds[max_sens]; struct pollfd fds[max_accel_sens];
for (unsigned i = 0; i < max_sens; i++) { for (unsigned i = 0; i < max_accel_sens; i++) {
fds[i].fd = subs[i]; fds[i].fd = subs[i];
fds[i].events = POLLIN; fds[i].events = POLLIN;
} }
unsigned counts[max_sens] = { 0 }; unsigned counts[max_accel_sens] = { 0 };
float accel_sum[max_sens][3]; float accel_sum[max_accel_sens][3];
memset(accel_sum, 0, sizeof(accel_sum)); memset(accel_sum, 0, sizeof(accel_sum));
unsigned errcount = 0; unsigned errcount = 0;
/* use the first sensor to pace the readout, but do per-sensor counts */ /* use the first sensor to pace the readout, but do per-sensor counts */
while (counts[0] < samples_num) { while (counts[0] < samples_num) {
int poll_ret = poll(&fds[0], max_sens, 1000); int poll_ret = poll(&fds[0], max_accel_sens, 1000);
if (poll_ret > 0) { if (poll_ret > 0) {
for (unsigned s = 0; s < max_sens; s++) { for (unsigned s = 0; s < max_accel_sens; s++) {
bool changed; bool changed;
orb_check(subs[s], &changed); orb_check(subs[s], &changed);
@ -620,7 +437,7 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6
} }
} }
for (unsigned s = 0; s < max_sens; s++) { for (unsigned s = 0; s < max_accel_sens; s++) {
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
accel_avg[s][orient][i] = accel_sum[s][i] / counts[s]; accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
} }
@ -652,7 +469,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
return OK; return OK;
} }
int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g) int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
{ {
/* calculate offsets */ /* calculate offsets */
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {