dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: rework IMU cal for compatibility with temperature compensation

This commit is contained in:
Paul Riseborough 2017-01-20 22:10:20 +11:00 committed by Lorenz Meier
parent add298c0b5
commit 62694d92d2
2 changed files with 87 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -147,6 +147,7 @@
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_correction.h>
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<max_accel_sens; i++) {
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]);
}
}
orb_unsubscribe(worker_data.sensor_correction_sub);
if (result == calibrate_return_ok) {
/* 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.
*/
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 */
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;
/* 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 */
while (counts[0] < samples_num) {
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;
orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
accel_sum[s][0] += arp.x;
accel_sum[s][1] += arp.y;
accel_sum[s][2] += arp.z;
// Apply thermal corrections
if (s == 0) {
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]++;
}
@ -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++) {
math::Vector<3> accel_sum_vec(&accel_sum[i][0]);
accel_sum_vec = board_rotation * accel_sum_vec;

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

@ -54,6 +54,7 @@
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_correction.h>
#include <drivers/drv_gyro.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
@ -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);