Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: add support for custom board orientations

This commit is contained in:
Jacob Walser 2018-03-08 21:26:39 -05:00 committed by Francisco Ferreira
parent 6699c59ad3
commit ad4e928e7d
3 changed files with 23 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1781,7 +1781,11 @@ bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vec
return false;
}
_accel_calibrator[_acc_body_aligned-1].get_sample_corrected(sample_num, ret);
ret.rotate(_board_orientation);
if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
ret = *_custom_rotation * ret;
} else {
ret.rotate(_board_orientation);
}
return true;
}
@ -1806,7 +1810,11 @@ bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vec
}
avg /= count;
ret = avg;
ret.rotate(_board_orientation);
if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
ret = *_custom_rotation * ret;
} else {
ret.rotate(_board_orientation);
}
return true;
}

4
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -187,8 +187,9 @@ public:
static const struct AP_Param::GroupInfo var_info[];
// set overall board orientation
void set_board_orientation(enum Rotation orientation) {
void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) {
_board_orientation = orientation;
_custom_rotation = custom_rotation;
}
// return the selected sample rate
@ -464,6 +465,7 @@ private:
// board orientation from AHRS
enum Rotation _board_orientation;
Matrix3f* _custom_rotation;
// per-sensor orientation to allow for board type defaults at runtime
enum Rotation _gyro_orientation[INS_MAX_INSTANCES];

12
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -105,7 +105,11 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
accel.z *= accel_scale.z;
// rotate to body frame
accel.rotate(_imu._board_orientation);
if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) {
accel = *_imu._custom_rotation * accel;
} else {
accel.rotate(_imu._board_orientation);
}
}
void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro)
@ -116,7 +120,11 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
// gyro calibration is always assumed to have been done in sensor frame
gyro -= _imu._gyro_offset[instance];
gyro.rotate(_imu._board_orientation);
if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) {
gyro = *_imu._custom_rotation * gyro;
} else {
gyro.rotate(_imu._board_orientation);
}
}
/*