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

commander: rework IMU cal for compatibility with temperature compensation

sbg
Paul Riseborough 8 years ago committed by Lorenz Meier
parent
add298c0b5
commit
62694d92d2
2 changed files with 87 additions and 10 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. 48
      src/modules/commander/accelerometer_calibration.cpp
  2. 49
      src/modules/commander/gyro_calibration.cpp

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

@ -147,6 +147,7 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/mavlink_log.h> #include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_correction.h>
static const char *sensor_name = "accel"; static const char *sensor_name = "accel";
@ -155,7 +156,7 @@ static int device_prio_max = 0;
static int32_t device_id_primary = 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 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]); 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); 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; unsigned done_count;
int subs[max_accel_sens]; int subs[max_accel_sens];
float accel_ref[max_accel_sens][detect_orientation_side_count][3]; float accel_ref[max_accel_sens][detect_orientation_side_count][3];
int sensor_correction_sub;
} accel_worker_data_t; } accel_worker_data_t;
int do_accel_calibration(orb_advert_t *mavlink_log_pub) 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)); 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), 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], (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 }; 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 // Initialize subs to error condition so we know which ones are open and which are not
for (size_t i=0; i<max_accel_sens; i++) { for (size_t i=0; i<max_accel_sens; i++) {
worker_data.subs[i] = -1; worker_data.subs[i] = -1;
@ -458,6 +464,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
px4_close(worker_data.subs[i]); px4_close(worker_data.subs[i]);
} }
} }
orb_unsubscribe(worker_data.sensor_correction_sub);
if (result == calibrate_return_ok) { if (result == calibrate_return_ok) {
/* calculate offsets and transform matrix */ /* calculate offsets and transform matrix */
@ -477,7 +484,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
/* /*
* Read specified number of accelerometer samples, calculate average and dispersion. * Read specified number of accelerometer samples, calculate average and dispersion.
*/ */
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)
{ {
/* get total sensor board rotation matrix */ /* get total sensor board rotation matrix */
param_t board_rotation_h = param_find("SENS_BOARD_ROT"); param_t board_rotation_h = param_find("SENS_BOARD_ROT");
@ -517,6 +524,18 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc
unsigned errcount = 0; unsigned errcount = 0;
/* Define struct containing sensor thermal compensation data and set default values */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i=0; i<3; i++) {
sensor_correction.accel_scale_0[i] = 1.0f;
sensor_correction.accel_scale_1[i] = 1.0f;
sensor_correction.accel_scale_2[i] = 1.0f;
}
/* get latest thermal corrections */
orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
/* 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 = px4_poll(&fds[0], max_accel_sens, 1000); int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000);
@ -532,9 +551,24 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc
struct accel_report arp; struct accel_report arp;
orb_copy(ORB_ID(sensor_accel), subs[s], &arp); orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
accel_sum[s][0] += arp.x; // Apply thermal corrections
accel_sum[s][1] += arp.y; if (s == 0) {
accel_sum[s][2] += arp.z; accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]) * sensor_correction.accel_scale_0[0];
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]) * sensor_correction.accel_scale_0[1];
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]) * sensor_correction.accel_scale_0[2];
} else if (s == 1) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]) * sensor_correction.accel_scale_1[0];
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]) * sensor_correction.accel_scale_1[1];
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]) * sensor_correction.accel_scale_1[2];
} else if (s == 2) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]) * sensor_correction.accel_scale_2[0];
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]) * sensor_correction.accel_scale_2[1];
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]) * sensor_correction.accel_scale_2[2];
} else {
accel_sum[s][0] += arp.x;
accel_sum[s][1] += arp.y;
accel_sum[s][2] += arp.z;
}
counts[s]++; counts[s]++;
} }
@ -550,7 +584,7 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc
} }
} }
// rotate sensor measurements from body frame into sensor frame using board rotation matrix // rotate sensor measurements from sensor to body frame using board rotation matrix
for (unsigned i = 0; i < max_accel_sens; i++) { for (unsigned i = 0; i < max_accel_sens; i++) {
math::Vector<3> accel_sum_vec(&accel_sum[i][0]); math::Vector<3> accel_sum_vec(&accel_sum[i][0]);
accel_sum_vec = board_rotation * accel_sum_vec; accel_sum_vec = board_rotation * accel_sum_vec;

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

@ -54,6 +54,7 @@
#include <string.h> #include <string.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_correction.h>
#include <drivers/drv_gyro.h> #include <drivers/drv_gyro.h>
#include <systemlib/mavlink_log.h> #include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
@ -68,6 +69,7 @@ typedef struct {
orb_advert_t *mavlink_log_pub; orb_advert_t *mavlink_log_pub;
int32_t device_id[max_gyros]; int32_t device_id[max_gyros];
int gyro_sensor_sub[max_gyros]; int gyro_sensor_sub[max_gyros];
int sensor_correction_sub;
struct gyro_calibration_s gyro_scale[max_gyros]; struct gyro_calibration_s gyro_scale[max_gyros];
struct gyro_report gyro_report_0; struct gyro_report gyro_report_0;
} gyro_worker_data_t; } gyro_worker_data_t;
@ -80,6 +82,10 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
struct gyro_report gyro_report; struct gyro_report gyro_report;
unsigned poll_errcount = 0; 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]; px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) { for (unsigned s = 0; s < max_gyros; s++) {
fds[s].fd = worker_data->gyro_sensor_sub[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)); 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 */ /* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) { while (calibration_counter[0] < calibration_count) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
return calibrate_return_cancelled; 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); int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
if (poll_ret > 0) { 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); orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
if (s == 0) { 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); 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]++; calibration_counter[s]++;
} }
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { 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; int device_prio_max = 0;
int32_t device_id_primary = 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++) { for (unsigned s = 0; s < max_gyros; s++) {
char str[30]; 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); 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 */ /* give this message enough time to propagate */
usleep(600000); usleep(600000);

Write Preview
Loading…
Cancel
Save