diff options
author | Darryl Taylor <darryl.c.taylor@gmail.com> | 2014-06-18 20:27:35 +0800 |
---|---|---|
committer | Darryl Taylor <darryl.c.taylor@gmail.com> | 2014-06-18 20:27:35 +0800 |
commit | 90a89ff2cdbb5268f4b8bee64b8da24db508f5a7 (patch) | |
tree | 8522638a03e77e930d056902316dc72adb46be09 | |
parent | 0f41d8b5077856cf8d00f30acf9f4bc52f6e41a6 (diff) | |
download | px4-firmware-90a89ff2cdbb5268f4b8bee64b8da24db508f5a7.tar.gz px4-firmware-90a89ff2cdbb5268f4b8bee64b8da24db508f5a7.tar.bz2 px4-firmware-90a89ff2cdbb5268f4b8bee64b8da24db508f5a7.zip |
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.
-rw-r--r-- | src/modules/sensors/sensor_params.c | 24 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 26 |
2 files changed, 47 insertions, 3 deletions
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 999cf8bb3..c90633822 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -243,6 +243,30 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** + * Board rotation pitch offset + * + * 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); + +/** + * Board rotation roll offset + * + * 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_ROLL_OFF, 0); + +/** + * Board rotation YAW offset + * + * 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_YAW_OFF, 0); + +/** * External magnetometer rotation * * This parameter defines the rotation of the external magnetometer relative 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); |