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

Commander: Implement calibration routines for multi-sensor setups

sbg
Lorenz Meier 10 years ago
parent
ac155b0fac
commit
807cf7bd16
6 changed files with 363 additions and 218 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 240
      src/modules/commander/accelerometer_calibration.cpp
  2. 4
      src/modules/commander/airspeed_calibration.cpp
  3. 8
      src/modules/commander/commander_helper.cpp
  4. 148
      src/modules/commander/gyro_calibration.cpp
  5. 179
      src/modules/commander/mag_calibration.cpp
  6. 2
      src/modules/commander/state_machine_helper.cpp

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

@ -134,7 +134,6 @@ @@ -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; @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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 @@ -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) @@ -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) @@ -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 @@ -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;

4
src/modules/commander/airspeed_calibration.cpp
Unescape View File

@ -89,7 +89,7 @@ int do_airspeed_calibration(int mavlink_fd) @@ -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) @@ -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)) {

8
src/modules/commander/commander_helper.cpp
Unescape View File

@ -100,7 +100,7 @@ int buzzer_init() @@ -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() @@ -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() @@ -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;

148
src/modules/commander/gyro_calibration.cpp
Unescape View File

@ -45,6 +45,7 @@ @@ -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"; @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 */

179
src/modules/commander/mag_calibration.cpp
Unescape View File

@ -63,93 +63,121 @@ static const int ERROR = -1; @@ -63,93 +63,121 @@ 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");
/* 45 seconds */
uint64_t calibration_interval = 45 * 1000 * 1000;
const unsigned max_mags = 3;
/* maximum 500 values */
const unsigned int calibration_maxcount = 240;
unsigned int calibration_counter;
int32_t device_id[max_mags];
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
sleep(1);
struct mag_scale mscale_null = {
struct mag_scale mscale_null[max_mags] = {
{
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
}
} ;
int res = OK;
int res = ERROR;
/* erase old calibration */
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
char str[30];
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
for (unsigned s = 0; s < max_mags; s++) {
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
mavlink_log_info(mavlink_fd, "Magnetometer #%u", s);
sleep(3);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
/* erase old calibration */
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
int fd = open(str, O_RDONLY);
if (res == OK) {
/* calibrate range */
res = ioctl(fd, MAGIOCCALIBRATE, fd);
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, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
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]);
}
}
close(fd);
return res;
}
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
{
/* 45 seconds */
uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 500 values */
const unsigned int calibration_maxcount = 240;
unsigned int calibration_counter;
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);
}
/* clean up */
if (x != NULL) {
free(x);
}
if (z != NULL) {
free(z);
}
if (y != NULL) {
free(y);
}
res = ERROR;
return res;
if (z != NULL) {
free(z);
}
} else {
/* exit */
return ERROR;
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) @@ -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) @@ -262,35 +290,26 @@ int do_mag_calibration(int mavlink_fd)
close(fd);
if (res == OK) {
/* 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;
}
if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
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))) {
bool failed = false;
/* set parameters */
(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 (failed) {
res = ERROR;
}
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
}

2
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -652,7 +652,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) @@ -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");

Write Preview
Loading…
Cancel
Save