diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-03 13:47:46 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-09 22:56:23 +0100 |
commit | 807cf7bd16536cbfb9632eb908faf22e44fa9233 (patch) | |
tree | 8edbb847d1d4ee077d9fca2e42751c1bf6d2f348 | |
parent | ac155b0faca7e9c378e9059d318e1f5151c61561 (diff) | |
download | px4-firmware-807cf7bd16536cbfb9632eb908faf22e44fa9233.tar.gz px4-firmware-807cf7bd16536cbfb9632eb908faf22e44fa9233.tar.bz2 px4-firmware-807cf7bd16536cbfb9632eb908faf22e44fa9233.zip |
Commander: Implement calibration routines for multi-sensor setups
-rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp | 240 | ||||
-rw-r--r-- | src/modules/commander/airspeed_calibration.cpp | 4 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.cpp | 8 | ||||
-rw-r--r-- | src/modules/commander/gyro_calibration.cpp | 148 | ||||
-rw-r--r-- | src/modules/commander/mag_calibration.cpp | 179 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 2 |
6 files changed, 363 insertions, 218 deletions
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d9e7e21fc..10fa3eaa3 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -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; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index cae1d7684..747d915ff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -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)) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 8a4451100..60e8be23e 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -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; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index e941e327c..dfd1657c5 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -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"); 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 */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7147fb283..1d2f38437 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -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) 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) { - /* 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); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 40da9c77b..40aa4a2f0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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"); |