Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS: limit trim to 10 degrees

Browse Source
This commit is contained in:
Randy Mackay 2013-02-19 18:50:57 +09:00
parent 2ecf34e509
commit 3321db8dde
2 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -115,14 +115,23 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
return false;
}
// set_trim
void AP_AHRS::set_trim(Vector3f new_trim)
{
Vector3f trim;
trim.x = constrain(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
trim.y = constrain(new_trim.y, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
_trim.set_and_save(trim);
}
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
{
Vector3f trim = _trim.get();
// add new trim
trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0f), ToRad(10.0f));
trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0f), ToRad(10.0f));
trim.x = constrain(trim.x + roll_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
trim.y = constrain(trim.y + pitch_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
// set new trim values
_trim.set(trim);

8
libraries/AP_AHRS/AP_AHRS.h
View File

@ -20,6 +20,8 @@
#include <AP_Baro.h>
#include <AP_Param.h>
#define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees
class AP_AHRS
{
public:
@ -145,11 +147,11 @@ public:
_fast_ground_gains = setting;
}
// get strim
// get trim
Vector3f get_trim() { return _trim; }
// set_trim
virtual void set_trim(Vector3f new_trim) { _trim.set_and_save(new_trim); }
// set trim
virtual void set_trim(Vector3f new_trim);
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
virtual void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);