diff options
author | Julian Oes <julian@oes.ch> | 2014-07-02 14:59:43 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-07-02 14:59:43 +0200 |
commit | fdceb8b0620c347c9f7f477dbf295dcfff12012c (patch) | |
tree | 4b41752c9cb81cb035034b3893872476495a3acd /src/modules/sensors | |
parent | 40780e29164f323d4896d75d9f585434375d83b0 (diff) | |
parent | 28a31708f98eefa4ceb04617f2da3dd7892c99fa (diff) | |
download | px4-firmware-fdceb8b0620c347c9f7f477dbf295dcfff12012c.tar.gz px4-firmware-fdceb8b0620c347c9f7f477dbf295dcfff12012c.tar.bz2 px4-firmware-fdceb8b0620c347c9f7f477dbf295dcfff12012c.zip |
Merge branch 'master' into navigator_rewrite_offboard2_merge
Conflicts:
mavlink/include/mavlink/v1.0/common/common.h
src/modules/mavlink/mavlink_receiver.cpp
Diffstat (limited to 'src/modules/sensors')
-rw-r--r-- | src/modules/sensors/sensor_params.c | 30 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 23 |
2 files changed, 52 insertions, 1 deletions
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dad81d8e1..5deb471d6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -243,6 +243,36 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** + * Board rotation Y (Pitch) offset + * + * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); + +/** + * Board rotation X (Roll) offset + * + * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); + +/** + * Board rotation Z (YAW) offset + * + * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); + +/** * 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 1a1d153f7..cc96f8a53 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,7 +229,7 @@ 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 */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -252,6 +252,8 @@ private: int board_rotation; int external_mag_rotation; + + float board_offset[3]; int rc_map_roll; int rc_map_pitch; @@ -346,6 +348,8 @@ private: param_t board_rotation; param_t external_mag_rotation; + + param_t board_offset[3]; } _parameter_handles; /**< handles for interesting parameters */ @@ -594,6 +598,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_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(); @@ -806,6 +815,18 @@ 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])); + + /** 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; } |