Browse Source

AP_InertialSensor: removed INS_CALSENSFRAME

it is no longer needed as we have shifted the accel cal indexes
mission-4.1.18
Andrew Tridgell 10 years ago
parent
commit
f3314791f2
  1. 12
      libraries/AP_InertialSensor/AP_InertialSensor.cpp
  2. 3
      libraries/AP_InertialSensor/AP_InertialSensor.h
  3. 15
      libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

12
libraries/AP_InertialSensor/AP_InertialSensor.cpp

@ -45,6 +45,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = { @@ -45,6 +45,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
ACC2OFFS : 6
ACC3SCAL : 8
ACC3OFFS : 9
CALSENSFRAME : 11
*/
// @Param: GYROFFS_X
@ -116,13 +117,6 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = { @@ -116,13 +117,6 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
AP_GROUPINFO("GYR3OFFS", 10, AP_InertialSensor, _gyro_offset[2], 0),
#endif
// @Param: INS_CALSENSFRAME
// @DisplayName: Calibration is in sensor frame
// @Description: This is an internal parameter that records whether accelerometer calibration was done in sensor frame. It is used to detect an old accel calibration which was done in body frame. This parameter is automatically set during accelerometer calibration and should not be changed by the user.
// @Values: 0:BoardFrame,1:SensorFrame
// @User: Advanced
AP_GROUPINFO("CALSENSFRAME", 11, AP_InertialSensor, _cal_sensor_frame, 0),
// @Param: ACCSCAL_X
// @DisplayName: Accelerometer scaling of X axis
// @Description: Accelerometer scaling of X axis. Calculated during acceleration calibration routine
@ -548,10 +542,6 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact @@ -548,10 +542,6 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
_board_orientation = saved_orientation;
// now set and save the INS_CALSENSFRAME parameter so we know
// the calibration was done in sensor frame
_cal_sensor_frame.set_and_save(1);
return true;
}

3
libraries/AP_InertialSensor/AP_InertialSensor.h

@ -268,9 +268,6 @@ private: @@ -268,9 +268,6 @@ private:
// filtering frequency (0 means default)
AP_Int8 _mpu6000_filter;
// was the accel cal done in sensor frame?
AP_Int8 _cal_sensor_frame;
// board orientation from AHRS
enum Rotation _board_orientation;

15
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@ -12,14 +12,10 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) : @@ -12,14 +12,10 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel)
{
/*
we rotate before or after offset and scaling based on the
INS_CALSENSFRAME parameter. This allows us to use older
calibrations, while making all new calibrations operate in
sensor frame, and thus be independent of AHRS_ORIENTATION
accel calibration is always done in sensor frame with this
version of the code. That means we apply the rotation after the
offsets and scaling.
*/
if (_imu._cal_sensor_frame == 0) {
accel.rotate(_imu._board_orientation);
}
// apply scaling
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
@ -30,9 +26,8 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect @@ -30,9 +26,8 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
// apply offsets
accel -= _imu._accel_offset[instance];
if (_imu._cal_sensor_frame != 0) {
accel.rotate(_imu._board_orientation);
}
// rotate to body frame
accel.rotate(_imu._board_orientation);
}
void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro)

Loading…
Cancel
Save