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

Fixed calibration routines to ignore previous offsets during calibration, added scale compensation for MPU-6000

This commit is contained in:
Lorenz Meier 2012-09-11 23:35:01 +02:00
parent 31d028828c
commit a74a455ab5
3 changed files with 155 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
apps/commander/commander.c
View File

@ -74,6 +74,16 @@
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <arch/board/up_cpuload.h>
extern struct system_load_s system_load;
@ -294,7 +304,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
mag_minima[2][i] = FLT_MAX;
}
mavlink_log_info(mavlink_fd, "[commander] Please rotate in all directions.");
int fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null))
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes.");
uint64_t calibration_start = hrt_absolute_time();
while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
@ -443,6 +466,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
}
}
fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale = {
mag_offset[0],
1.0f,
mag_offset[1],
1.0f,
mag_offset[2],
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
free(mag_maxima[0]);
free(mag_maxima[1]);
free(mag_maxima[2]);
@ -468,6 +504,20 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
int calibration_counter = 0;
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
/* set offsets to zero */
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@ -502,6 +552,20 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
1.0f,
gyro_offset[1],
1.0f,
gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* exit to gyro calibration mode */
status->flag_preflight_gyro_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
@ -531,6 +595,19 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
int calibration_counter = 0;
float accel_offset[3] = {0.0f, 0.0f, 0.0f};
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@ -556,20 +633,48 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
/* add the removed length from x / y to z, since we induce a scaling issue else */
float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]);
/* if length is correct, zero results here */
accel_offset[2] = accel_offset[2] + total_len;
if (param_set(param_find("SENSOR_ACC_XOFF"), &(accel_offset[0]))) {
float scale = 9.80665f / total_len;
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
}
if (param_set(param_find("SENSOR_ACC_YOFF"), &(accel_offset[1]))) {
if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
}
if (param_set(param_find("SENSOR_ACC_ZOFF"), &(accel_offset[2]))) {
if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
}
if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
}
if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
}
if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
}
fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offset[0],
scale,
accel_offset[1],
scale,
accel_offset[2],
scale,
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* exit to gyro calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);

22
apps/sensors/sensor_params.c
View File

@ -44,17 +44,21 @@
#include <systemlib/param/param.h>
PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_MAG_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_MAG_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_ACC_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_ACC_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_ACC_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 0.0f);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);

58
apps/sensors/sensors.cpp
View File

@ -182,7 +182,8 @@ private:
float gyro_offset[3];
float mag_offset[3];
float acc_offset[3];
float accel_offset[3];
float accel_scale[3];
int rc_type;
@ -203,8 +204,9 @@ private:
param_t rc_type;
param_t gyro_offset[3];
param_t accel_offset[3];
param_t accel_scale[3];
param_t mag_offset[3];
param_t acc_offset[3];
param_t rc_map_roll;
param_t rc_map_pitch;
@ -383,19 +385,22 @@ Sensors::Sensors() :
_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENSOR_GYRO_XOFF");
_parameter_handles.gyro_offset[1] = param_find("SENSOR_GYRO_YOFF");
_parameter_handles.gyro_offset[2] = param_find("SENSOR_GYRO_ZOFF");
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
_parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
/* accel offsets */
_parameter_handles.acc_offset[0] = param_find("SENSOR_ACC_XOFF");
_parameter_handles.acc_offset[1] = param_find("SENSOR_ACC_YOFF");
_parameter_handles.acc_offset[2] = param_find("SENSOR_ACC_ZOFF");
_parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
_parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF");
_parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF");
_parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE");
_parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE");
_parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE");
/* mag offsets */
_parameter_handles.mag_offset[0] = param_find("SENSOR_MAG_XOFF");
_parameter_handles.mag_offset[1] = param_find("SENSOR_MAG_YOFF");
_parameter_handles.mag_offset[2] = param_find("SENSOR_MAG_ZOFF");
_parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF");
_parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF");
_parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
@ -493,9 +498,12 @@ Sensors::parameters_update()
param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
/* accel offsets */
param_get(_parameter_handles.acc_offset[0], &(_parameters.acc_offset[0]));
param_get(_parameter_handles.acc_offset[1], &(_parameters.acc_offset[1]));
param_get(_parameter_handles.acc_offset[2], &(_parameters.acc_offset[2]));
param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0]));
param_get(_parameter_handles.accel_offset[1], &(_parameters.accel_offset[1]));
param_get(_parameter_handles.accel_offset[2], &(_parameters.accel_offset[2]));
param_get(_parameter_handles.accel_scale[0], &(_parameters.accel_scale[0]));
param_get(_parameter_handles.accel_scale[1], &(_parameters.accel_scale[1]));
param_get(_parameter_handles.accel_scale[2], &(_parameters.accel_scale[2]));
/* mag offsets */
param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0]));
@ -643,9 +651,9 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
const float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.x = (((accel_report.x_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.y = (((accel_report.y_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.z = (((accel_report.z_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_counter++;
} else {
@ -822,12 +830,12 @@ Sensors::parameter_update_poll(bool forced)
fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
_parameters.acc_offset[0],
1.0f,
_parameters.acc_offset[1],
1.0f,
_parameters.acc_offset[2],
1.0f,
_parameters.accel_offset[0],
_parameters.accel_scale[0],
_parameters.accel_offset[1],
_parameters.accel_scale[1],
_parameters.accel_offset[2],
_parameters.accel_scale[2],
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
@ -1004,7 +1012,7 @@ Sensors::task_main()
parameter_update_poll(true /* forced */);
/* advertise the sensor_combined topic and make the initial publication */
/* advertise the SENS_combined topic and make the initial publication */
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* advertise the manual_control topic */
@ -1092,7 +1100,7 @@ Sensors::start()
ASSERT(_sensors_task == -1);
/* start the task */
_sensors_task = task_create("sensor_task",
_sensors_task = task_create("SENS_task",
SCHED_PRIORITY_MAX - 5,
4096, /* XXX may be excesssive */
(main_t)&Sensors::task_main_trampoline,