Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: allow setting of AHRS_TRIM_Z

Browse Source
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: // initialise the controller-to-autopilot-body trim state:
_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_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(); _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(); _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(); _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 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; _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; enum Rotation get_view_rotation(void) const;
protected: 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 // set_trim
void AP_AHRS::set_trim(const Vector3f &new_trim) void AP_AHRS::set_trim(const Vector3f &new_trim)
{ {
Vector3f trim; const Vector3f trim {
trim.x = constrain_float(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); 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)); 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); _trim.set_and_save(trim);
} }