From 62694d92d25a6d928af474f14ad8788e645d8ff3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Jan 2017 22:10:20 +1100 Subject: [PATCH] commander: rework IMU cal for compatibility with temperature compensation --- .../commander/accelerometer_calibration.cpp | 48 +++++++++++++++--- src/modules/commander/gyro_calibration.cpp | 49 +++++++++++++++++-- 2 files changed, 87 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4a5a35903c..8ee4a9f960 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -147,6 +147,7 @@ #include #include #include +#include static const char *sensor_name = "accel"; @@ -155,7 +156,7 @@ static int device_prio_max = 0; static int32_t device_id_primary = 0; calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); -calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); +calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); calibrate_return 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); @@ -165,6 +166,7 @@ typedef struct { unsigned done_count; int subs[max_accel_sens]; float accel_ref[max_accel_sens][detect_orientation_side_count][3]; + int sensor_correction_sub; } accel_worker_data_t; int do_accel_calibration(orb_advert_t *mavlink_log_pub) @@ -371,7 +373,7 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); - read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num); + read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation, samples_num); calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation), (double)worker_data->accel_ref[0][orientation][0], @@ -397,6 +399,10 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false }; + // Initialise sub to sensor thermal compensation data + worker_data.sensor_correction_sub = -1; + worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); + // Initialize subs to error condition so we know which ones are open and which are not for (size_t i=0; i accel_sum_vec(&accel_sum[i][0]); accel_sum_vec = board_rotation * accel_sum_vec; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8c72384c26..bae483d45f 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +69,7 @@ typedef struct { orb_advert_t *mavlink_log_pub; int32_t device_id[max_gyros]; int gyro_sensor_sub[max_gyros]; + int sensor_correction_sub; struct gyro_calibration_s gyro_scale[max_gyros]; struct gyro_report gyro_report_0; } gyro_worker_data_t; @@ -80,6 +82,10 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) struct gyro_report gyro_report; unsigned poll_errcount = 0; + // set up subscription to sensor thermal compensation data + struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */ + memset(&_sensor_correction, 0, sizeof(_sensor_correction)); + px4_pollfd_struct_t fds[max_gyros]; for (unsigned s = 0; s < max_gyros; s++) { fds[s].fd = worker_data->gyro_sensor_sub[s]; @@ -88,12 +94,21 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); + /* use first gyro to pace, but count correctly per-gyro for statistics */ while (calibration_counter[0] < calibration_count) { if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { return calibrate_return_cancelled; } + /* check if there are new thermal corrections */ + bool updated; + orb_check(worker_data->sensor_correction_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &_sensor_correction); + } + int poll_ret = px4_poll(&fds[0], max_gyros, 1000); if (poll_ret > 0) { @@ -106,13 +121,36 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report); if (s == 0) { + // take a working copy + worker_data->gyro_scale[s].x_offset += (gyro_report.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0]; + worker_data->gyro_scale[s].y_offset += (gyro_report.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1]; + worker_data->gyro_scale[s].z_offset += (gyro_report.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2]; + + // take a reference copy of the primary sensor including correction for thermal drift orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0); + worker_data->gyro_report_0.x = (gyro_report.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0]; + worker_data->gyro_report_0.y = (gyro_report.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1]; + worker_data->gyro_report_0.z = (gyro_report.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2]; + + } else if (s == 1) { + worker_data->gyro_scale[s].x_offset += (gyro_report.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0]; + worker_data->gyro_scale[s].y_offset += (gyro_report.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1]; + worker_data->gyro_scale[s].z_offset += (gyro_report.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2]; + + } else if (s == 2) { + worker_data->gyro_scale[s].x_offset += (gyro_report.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0]; + worker_data->gyro_scale[s].y_offset += (gyro_report.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1]; + worker_data->gyro_scale[s].z_offset += (gyro_report.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2]; + + } else { + worker_data->gyro_scale[s].x_offset += gyro_report.x; + worker_data->gyro_scale[s].y_offset += gyro_report.y; + worker_data->gyro_scale[s].z_offset += gyro_report.z; + } - worker_data->gyro_scale[s].x_offset += gyro_report.x; - worker_data->gyro_scale[s].y_offset += gyro_report.y; - worker_data->gyro_scale[s].z_offset += gyro_report.z; calibration_counter[s]++; + } if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { @@ -164,6 +202,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) int device_prio_max = 0; int32_t device_id_primary = 0; + worker_data.sensor_correction_sub = -1; + worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); + for (unsigned s = 0; s < max_gyros; s++) { char str[30]; @@ -382,6 +423,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); } + orb_unsubscribe(worker_data.sensor_correction_sub); + /* give this message enough time to propagate */ usleep(600000);