zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander: Fix new-style accel calibration

sbg
Lorenz Meier 10 years ago
parent
8fde33bcf7
commit
66fced90de
1 changed files with 78 additions and 76 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 154
      src/modules/commander/accelerometer_calibration.cpp

154
src/modules/commander/accelerometer_calibration.cpp
Unescape View File

@ -151,11 +151,11 @@ static const char *sensor_name = "accel";
static const unsigned max_sens = 3; 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 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 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 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(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); 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 do_accel_calibration(int mavlink_fd)
{ {
@ -204,74 +204,78 @@ int do_accel_calibration(int mavlink_fd)
float accel_offs[max_sens][3]; float accel_offs[max_sens][3];
float accel_T[max_sens][3][3]; float accel_T[max_sens][3][3];
unsigned active_sensors;
if (res == OK) { if (res == OK) {
/* measure and calculate offsets & scales */ /* measure and calculate offsets & scales */
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
} }
if (res == OK) { if (res != OK || active_sensors == 0) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
return ERROR;
}
/* measurements completed successfully, rotate calibration values */ /* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT"); param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int; int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int)); param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation; math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation); get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
for (unsigned i = 0; i < max_sens; i++) { for (unsigned i = 0; i < active_sensors; i++) {
/* handle individual sensors, one by one */ /* handle individual sensors, one by one */
math::Vector<3> accel_offs_vec(&accel_offs[i][0]); math::Vector<3> accel_offs_vec(&accel_offs[i][0]);
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec; 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_mat(&accel_T[i][0][0]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; 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_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0); accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1); accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1); accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2); accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2); accel_scale.z_scale = accel_T_rotated(2, 2);
bool failed = false; bool failed = false;
/* 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(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(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(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(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(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(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(param_find(str), &(device_id[i])));
if (failed) { if (failed) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
res = ERROR; return ERROR;
}
} }
}
if (res == OK) { sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
/* apply new scaling and offsets */ fd = open(str, 0);
for (unsigned s = 0; s < max_sens; s++) {
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); if (fd < 0) {
fd = open(str, 0); mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
res = ERROR;
} else {
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd); close(fd);
}
if (res != OK) { if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
}
} }
} }
@ -280,23 +284,22 @@ int do_accel_calibration(int mavlink_fd)
res = param_save_default(); res = param_save_default();
if (res != OK) { if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
} }
}
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else { } else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
} }
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]) int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors)
{ {
const unsigned samples_num = 2500; const unsigned samples_num = 3000;
*active_sensors = 0;
float accel_ref[max_sens][6][3]; float accel_ref[max_sens][6][3];
bool data_collected[6] = { false, false, false, false, false, false }; bool data_collected[6] = { false, false, false, false, false, false };
@ -306,8 +309,6 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens]
uint64_t timestamps[max_sens]; uint64_t timestamps[max_sens];
unsigned active_sensors = 0;
for (unsigned i = 0; i < max_sens; i++) { for (unsigned i = 0; i < max_sens; i++) {
subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
/* store initial timestamp - used to infer which sensors are active */ /* store initial timestamp - used to infer which sensors are active */
@ -353,7 +354,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens]
/* allow user enough time to read the message */ /* allow user enough time to read the message */
sleep(3); sleep(3);
int orient = detect_orientation(mavlink_fd, &subs[0]); int orient = detect_orientation(mavlink_fd, subs);
if (orient < 0) { if (orient < 0) {
mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
@ -386,18 +387,18 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens]
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), 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(subs[i]);
} }
if (res == OK) { if (res == 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(accel_ref[i], accel_T[i], accel_offs[i], CONSTANTS_ONE_G); res = calculate_calibration_values(accel_ref[i], accel_T[i], accel_offs[i], CONSTANTS_ONE_G);
if (res != OK) { if (res != OK) {
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); mavlink_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
break; break;
} }
} }
@ -415,7 +416,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens]
* @return 0..5 according to orientation when vehicle is still and ready for measurements, * @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 * 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]) int detect_orientation(int mavlink_fd, int (&subs)[max_sens])
{ {
const unsigned ndim = 3; const unsigned ndim = 3;
@ -560,7 +561,7 @@ int detect_orientation(int mavlink_fd, int subs[max_sens])
/* /*
* 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_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num)
{ {
struct pollfd fds[max_sens]; struct pollfd fds[max_sens];
@ -610,6 +611,7 @@ int read_accelerometer_avg(int subs[max_sens], float accel_avg[max_sens][6][3],
for (unsigned s = 0; s < max_sens; s++) { for (unsigned s = 0; s < max_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];
warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]);
} }
} }
@ -639,7 +641,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
return OK; return OK;
} }
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) int calculate_calibration_values(float (&accel_ref)[6][3], float (&accel_T)[3][3], float (&accel_offs)[3], float g)
{ {
/* calculate offsets */ /* calculate offsets */
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {

Write Preview
Loading…
Cancel
Save