aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: move trim-related functionality to front end

Browse Source
This commit is contained in:
Peter Barker 2021-07-31 16:24:05 +10:00 committed by Andrew Tridgell
parent 7dea029626
commit ecd488b3c3
5 changed files with 59 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -191,6 +191,11 @@ AP_AHRS::AP_AHRS(uint8_t flags) :
_ekf_flags |= AP_AHRS::FLAG_ALWAYS_USE_EKF;
#endif
_dcm_matrix.identity();
// initialise the controller-to-autopilot-body trim state:
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
}
// init sets up INS board orientation
@ -222,6 +227,20 @@ void AP_AHRS::init()
#endif
}
// updates matrices responsible for rotating vectors from vehicle body
// frame to autopilot body frame from _trim variables
void AP_AHRS::update_trim_rotation_matrices()
{
if (_last_trim == _trim.get()) {
// nothing to do
return;
}
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
}
// return the smoothed gyro vector corrected for drift
const Vector3f &AP_AHRS::get_gyro(void) const
{
@ -281,6 +300,9 @@ void AP_AHRS::update(bool skip_ins_update)
// calling delay_microseconds_boost()
hal.scheduler->boost_end();
// update autopilot-body-to-vehicle-body from _trim parameters:
update_trim_rotation_matrices();
update_DCM(skip_ins_update);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

34
libraries/AP_AHRS/AP_AHRS.h
View File

@ -337,6 +337,23 @@ public:
// return SSA
float getSSA(void) const { return _SSA; }
/*
* trim-related functions
*/
// get trim
const Vector3f &get_trim() const { return _trim.get(); }
// set trim
void set_trim(const Vector3f &new_trim);
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
// trim rotation matrices:
const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
/*
* home-related functionality
*/
@ -464,6 +481,23 @@ private:
void update_external(void);
#endif
/*
* trim-related state and private methods:
*/
// a vector to capture the difference between the controller and body frames
AP_Vector3f _trim;
// cached trim rotations
Vector3f _last_trim;
Matrix3f _rotation_autopilot_body_to_vehicle_body;
Matrix3f _rotation_vehicle_body_to_autopilot_body;
// updates matrices responsible for rotating vectors from vehicle body
// frame to autopilot body frame from _trim variables
void update_trim_rotation_matrices();
#if HAL_NMEA_OUTPUT_ENABLED
class AP_NMEA_Output* _nmea_out;
#endif

10
libraries/AP_AHRS/AP_AHRS_Backend.cpp
View File

@ -36,7 +36,7 @@ Vector3f AP_AHRS_Backend::get_gyro_latest(void) const
}
// set_trim
void AP_AHRS_Backend::set_trim(const Vector3f &new_trim)
void AP_AHRS::set_trim(const Vector3f &new_trim)
{
Vector3f trim;
trim.x = constrain_float(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
@ -45,7 +45,7 @@ void AP_AHRS_Backend::set_trim(const Vector3f &new_trim)
}
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
void AP_AHRS_Backend::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
{
Vector3f trim = _trim.get();
@ -194,12 +194,6 @@ void AP_AHRS_Backend::calc_trig(const Matrix3f &rot,
// should be called after _dcm_matrix is updated
void AP_AHRS_Backend::update_trig(void)
{
if (_last_trim != _trim.get()) {
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
}
calc_trig(get_rotation_body_to_ned(),
_cos_roll, _cos_pitch, _cos_yaw,
_sin_roll, _sin_pitch, _sin_yaw);

28
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -50,10 +50,6 @@ public:
AP_AHRS_Backend() {
// enable centrifugal correction by default
_flags.correct_centrifugal = true;
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
}
// empty virtual destructor
@ -199,9 +195,6 @@ public:
quat.from_rotation_matrix(get_rotation_body_to_ned());
}
const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
// get rotation matrix specifically from DCM backend (used for compass calibrator)
virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0;
@ -337,17 +330,6 @@ public:
return _flags.correct_centrifugal;
}
// get trim
const Vector3f &get_trim() const {
return _trim.get();
}
// set trim
void set_trim(const Vector3f &new_trim);
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
// helper trig value accessors
float cos_roll() const {
return _cos_roll;
@ -574,16 +556,6 @@ protected:
// update takeoff/touchdown flags
void update_flags();
// pointer to airspeed object, if available
// a vector to capture the difference between the controller and body frames
AP_Vector3f _trim;
// cached trim rotations
Vector3f _last_trim;
Matrix3f _rotation_autopilot_body_to_vehicle_body;
Matrix3f _rotation_vehicle_body_to_autopilot_body;
// accelerometer values in the earth frame in m/s/s
Vector3f _accel_ef[INS_MAX_INSTANCES];
Vector3f _accel_ef_blended;

2
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1012,7 +1012,7 @@ void AP_AHRS_DCM::estimate_wind(void)
void
AP_AHRS_DCM::euler_angles(void)
{
_body_dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body();
_body_dcm_matrix = _dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
_body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
update_cd_values();