dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.

This commit is contained in:
Darryl Taylor 2014-06-18 20:27:35 +08:00
parent 0f41d8b507
commit 90a89ff2cd
2 changed files with 47 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/sensors/sensor_params.c
View File

@ -242,6 +242,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
*

26
src/modules/sensors/sensors.cpp
View File

@ -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);