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

AP_AHRS: allow setting of AHRS_TRIM_Z

This commit is contained in:
Andrew Tridgell 2021-08-17 12:52:01 +10:00
parent fd86cb0591
commit 3715ce6d1a
3 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -195,7 +195,7 @@ AP_AHRS::AP_AHRS(uint8_t flags) :
// 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_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, _last_trim.z);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
}
@ -238,7 +238,7 @@ void AP_AHRS::update_trim_rotation_matrices()
}
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, _last_trim.z);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
}
@ -3059,7 +3059,7 @@ void AP_AHRS::set_alt_measurement_noise(float noise)
}
/*
get the current views rotation, or ROTATION_NONE
get the current view's rotation, or ROTATION_NONE
*/
enum Rotation AP_AHRS::get_view_rotation(void) const
{

2
libraries/AP_AHRS/AP_AHRS.h
View File

@ -457,7 +457,7 @@ public:
_vehicle_class = vclass;
}
// get the views rotation, or ROTATION_NONE
// get the view's rotation, or ROTATION_NONE
enum Rotation get_view_rotation(void) const;
protected:

8
libraries/AP_AHRS/AP_AHRS_Backend.cpp
View File

@ -38,9 +38,11 @@ Vector3f AP_AHRS_Backend::get_gyro_latest(void) const
// set_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));
trim.y = constrain_float(new_trim.y, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
const Vector3f trim {
constrain_float(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)),
constrain_float(new_trim.y, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)),
constrain_float(new_trim.z, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT))
};
_trim.set_and_save(trim);
}