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

AP_InertialSensor: remove custom orentations

This commit is contained in:
Iampete1 2021-11-05 16:11:46 +00:00 committed by Andrew Tridgell
parent 209ad965be
commit b77476caa1
3 changed files with 5 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -2133,11 +2133,7 @@ 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);
if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
ret = *_custom_rotation * ret;
} else {
ret.rotate(_board_orientation);
}
ret.rotate(_board_orientation);
return true;
}
@ -2162,11 +2158,7 @@ bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vec
}
avg /= count;
ret = avg;
if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
ret = *_custom_rotation * ret;
} else {
ret.rotate(_board_orientation);
}
ret.rotate(_board_orientation);
return true;
}

4
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -224,9 +224,8 @@ public:
static const struct AP_Param::GroupInfo var_info[];
// set overall board orientation
void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) {
void set_board_orientation(enum Rotation orientation) {
_board_orientation = orientation;
_custom_rotation = custom_rotation;
}
// return the selected loop rate at which samples are made avilable
@ -595,7 +594,6 @@ 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

@ -126,11 +126,7 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
}
// rotate to body frame
if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) {
accel = *_imu._custom_rotation * accel;
} else {
accel.rotate(_imu._board_orientation);
}
accel.rotate(_imu._board_orientation);
}
void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro)
@ -155,11 +151,7 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
gyro -= _imu._gyro_offset[instance];
}
if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) {
gyro = *_imu._custom_rotation * gyro;
} else {
gyro.rotate(_imu._board_orientation);
}
gyro.rotate(_imu._board_orientation);
}
/*