From 90a89ff2cdbb5268f4b8bee64b8da24db508f5a7 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 20:27:35 +0800 Subject: WIP: Support in-flight fine tuning of board alignment. Implemented by applying a user supplied rotation matrix via new SENS_BOARD_(PITCH, ROLL, YAW)_OFF params in the sensors app. Currently only tested in QGC. --- src/modules/sensors/sensors.cpp | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) (limited to 'src/modules/sensors/sensors.cpp') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36..1a6202d7d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -230,6 +230,8 @@ private: 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_offset; /**< rotation matrix for fine tuning board offset **/ + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -252,6 +254,8 @@ private: int board_rotation; int external_mag_rotation; + + float board_offset[3]; int rc_map_roll; int rc_map_pitch; @@ -341,6 +345,8 @@ private: param_t board_rotation; param_t external_mag_rotation; + + param_t board_offset[3]; } _parameter_handles; /**< handles for interesting parameters */ @@ -587,6 +593,11 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -791,6 +802,15 @@ 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])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + _board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + return OK; } @@ -970,7 +990,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -996,7 +1016,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1027,7 +1047,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) vect = _external_mag_rotation * vect; } else { - vect = _board_rotation * vect; + vect = _board_rotation * _board_rotation_offset * vect; } raw.magnetometer_ga[0] = vect(0); -- cgit v1.2.3 From 5ed1cf7e8d6201f1f2e0115f4941ac551f61d628 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 18 Jun 2014 21:34:38 +0800 Subject: Fixed too-long param names. --- src/modules/sensors/sensor_params.c | 10 +++++----- src/modules/sensors/sensors.cpp | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) (limited to 'src/modules/sensors/sensors.cpp') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c90633822..687efc49b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -248,23 +248,23 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * This parameter defines a pitch offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ - PARAM_DEFINE_FLOAT(SENS_BOARD_PITCH_OFF, 0); + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0); /** * Board rotation roll offset * - * This parameter defines a pitch offset from the board rotation. It allows the user + * This parameter defines a roll offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_ROLL_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0); /** * Board rotation YAW offset * - * This parameter defines a pitch offset from the board rotation. It allows the user + * This parameter defines a yaw offset from the board rotation. It allows the user * to fine tune the board offset in the event of misalignment. */ -PARAM_DEFINE_FLOAT(SENS_BOARD_YAW_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0); /** * External magnetometer rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1a6202d7d..fe741894b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -595,9 +595,9 @@ Sensors::Sensors() : _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ - _parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF"); - _parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF"); - _parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF"); + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); -- cgit v1.2.3 From 2475efe13d8f8d9d801276b73b98759676a64db6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Thu, 19 Jun 2014 13:22:45 +0800 Subject: Pre-compute board orientation offsets on param update. --- src/modules/sensors/sensors.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) (limited to 'src/modules/sensors/sensors.cpp') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index fe741894b..16fcb4c26 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,8 +229,6 @@ private: 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_offset; /**< rotation matrix for fine tuning board offset **/ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -807,10 +805,13 @@ Sensors::parameters_update() param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - _board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + + _board_rotation = _board_rotation * board_rotation_offset; return OK; } @@ -990,7 +991,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -1016,7 +1017,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1047,7 +1048,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) vect = _external_mag_rotation * vect; } else { - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; } raw.magnetometer_ga[0] = vect(0); -- cgit v1.2.3