From a74a455ab5ae3e60753edfd82235e856db0b2de5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Sep 2012 23:35:01 +0200 Subject: [PATCH] Fixed calibration routines to ignore previous offsets during calibration, added scale compensation for MPU-6000 --- apps/commander/commander.c | 113 +++++++++++++++++++++++++++++++++-- apps/sensors/sensor_params.c | 22 ++++--- apps/sensors/sensors.cpp | 58 ++++++++++-------- 3 files changed, 155 insertions(+), 38 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 022003ee58..d8e871eb06 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -74,6 +74,16 @@ #include #include +#include + +/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ +#include +#include +#include +#include + + + #include 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); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 6e36002dd4..fcd0608a42 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -44,17 +44,21 @@ #include -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); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d388a6d5a1..7c64376083 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -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,