From 3d195bc7cc41885e3328ae38c8a1afc53e0d7893 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Feb 2015 21:59:12 +0100 Subject: Proper mag rotation handling --- src/modules/sensors/sensor_params.c | 21 +++++++++++ src/modules/sensors/sensors.cpp | 70 +++++++++++++++++-------------------- 2 files changed, 53 insertions(+), 38 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5e04241fe..a80ca4eb6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -119,6 +119,13 @@ PARAM_DEFINE_FLOAT(CAL_GYRO0_ZSCALE, 1.0f); */ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); +/** + * Rotation of magnetometer 0 relative to airframe. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG0_ROT, 0); + /** * Magnetometer X-axis offset * @@ -284,6 +291,13 @@ PARAM_DEFINE_FLOAT(CAL_GYRO1_ZSCALE, 1.0f); */ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); +/** + * Rotation of magnetometer 0 relative to airframe. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG1_ROT, 0); + /** * Magnetometer X-axis offset * @@ -449,6 +463,13 @@ PARAM_DEFINE_FLOAT(CAL_GYRO2_ZSCALE, 1.0f); */ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); +/** + * Rotation of magnetometer 0 relative to airframe. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG2_ROT, 0); + /** * Magnetometer X-axis offset * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b93b720be..f9a607d77 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -253,9 +253,8 @@ private: struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; - math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ - bool _mag_is_external; /**< true if the active mag is on an external board */ + math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -273,7 +272,6 @@ private: int board_rotation; int flow_rotation; - int external_mag_rotation; float board_offset[3]; @@ -374,7 +372,6 @@ private: param_t board_rotation; param_t flow_rotation; - param_t external_mag_rotation; param_t board_offset[3]; @@ -532,7 +529,9 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _mag_is_external(false), + _board_rotation{}, + _mag_rotation{}, + _battery_discharged(0), _battery_current_timestamp(0) { @@ -618,7 +617,6 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); - _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); @@ -820,7 +818,6 @@ Sensors::parameters_update() param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); - param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); /* set px4flow rotation */ int flowfd; @@ -839,7 +836,6 @@ Sensors::parameters_update() } get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); - get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); @@ -962,21 +958,6 @@ Sensors::mag_init() } } - - - ret = ioctl(fd, MAGIOCGEXTERNAL, 0); - - if (ret < 0) { - warnx("FATAL: unknown if magnetometer is external or onboard"); - return ERROR; - - } else if (ret == 1) { - _mag_is_external = true; - - } else { - _mag_is_external = false; - } - close(fd); return OK; @@ -1171,14 +1152,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - // XXX we need device-id based handling here - - if (_mag_is_external) { - vect = _external_mag_rotation * vect; - - } else { - vect = _board_rotation * vect; - } + vect = _mag_rotation[0] * vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); @@ -1201,9 +1175,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - // XXX presume internal mag - - vect = _board_rotation * vect; + vect = _mag_rotation[1] * vect; raw.magnetometer1_ga[0] = vect(0); raw.magnetometer1_ga[1] = vect(1); @@ -1226,9 +1198,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - // XXX presume internal mag - - vect = _board_rotation * vect; + vect = _mag_rotation[2] * vect; raw.magnetometer2_ga[0] = vect(0); raw.magnetometer2_ga[1] = vect(1); @@ -1542,6 +1512,30 @@ Sensors::parameter_update_poll(bool forced) (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); failed |= (OK != param_get(param_find(str), &gscale.z_scale)); + if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { + /* mag is internal */ + _mag_rotation[s] = _board_rotation; + } else { + + int32_t mag_rot = 0; + (void)sprintf(str, "CAL_MAG%u_ROT", i); + param_get(param_find(str), &mag_rot); + + /* handling of old setups, will be removed */ + if (s == 0) { + int32_t deprecated_mag_rot = 0; + param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); + + /* if the old param is non-zero, set the new one to the same value */ + if ((deprecated_mag_rot != 0) && (mag_rot == 0)) { + mag_rot = deprecated_mag_rot; + param_set(param_find("CAL_MAG0_ROT"), &mag_rot); + } + } + + get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[s]); + } + if (failed) { warnx("%s: mag #%u", CAL_FAILED_APPLY_CAL_MSG, mag_count); } else { -- cgit v1.2.3