mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: restore auto-trim method but now use AHRS.add_trim
This commit is contained in:
parent
ae72238beb
commit
e13cea03ea
|
@ -876,8 +876,8 @@ static uint8_t auto_disarming_counter;
|
||||||
// prevents duplicate GPS messages from entering system
|
// prevents duplicate GPS messages from entering system
|
||||||
static uint32_t last_gps_time;
|
static uint32_t last_gps_time;
|
||||||
|
|
||||||
// Used to auto exit the roll_pitch_trim saving function
|
// Used to exit the roll and pitch auto trim function
|
||||||
static uint8_t save_trim_counter;
|
static uint8_t auto_trim_counter;
|
||||||
|
|
||||||
// Reference to the AP relay object - APM1 only
|
// Reference to the AP relay object - APM1 only
|
||||||
AP_Relay relay;
|
AP_Relay relay;
|
||||||
|
@ -1095,8 +1095,8 @@ static void medium_loop()
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// save_trim - stores roll and pitch radio inputs to ahrs
|
// auto_trim - stores roll and pitch radio inputs to ahrs
|
||||||
save_trim();
|
auto_trim();
|
||||||
|
|
||||||
// record throttle output
|
// record throttle output
|
||||||
// ------------------------------
|
// ------------------------------
|
||||||
|
|
|
@ -86,7 +86,7 @@ static void read_trim_switch()
|
||||||
|
|
||||||
case CH7_SAVE_TRIM:
|
case CH7_SAVE_TRIM:
|
||||||
if(ap_system.CH7_flag && control_mode <= ACRO && g.rc_3.control_in == 0) {
|
if(ap_system.CH7_flag && control_mode <= ACRO && g.rc_3.control_in == 0) {
|
||||||
save_trim_counter = 15;
|
save_trim();
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -155,28 +155,40 @@ static void read_trim_switch()
|
||||||
// save_trim - adds roll and pitch trims from the radio to ahrs
|
// save_trim - adds roll and pitch trims from the radio to ahrs
|
||||||
static void save_trim()
|
static void save_trim()
|
||||||
{
|
{
|
||||||
float roll_trim, pitch_trim;
|
// save roll and pitch trim
|
||||||
|
float roll_trim = ToRad((float)g.rc_1.control_in/100.0);
|
||||||
|
float pitch_trim = ToRad((float)g.rc_2.control_in/100.0);
|
||||||
|
ahrs.add_trim(roll_trim, pitch_trim);
|
||||||
|
}
|
||||||
|
|
||||||
if(save_trim_counter > 0) {
|
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
|
||||||
save_trim_counter--;
|
// meant to be called continuously while the pilot attempts to keep the copter level
|
||||||
|
static void auto_trim()
|
||||||
|
{
|
||||||
|
if(auto_trim_counter > 0) {
|
||||||
|
auto_trim_counter--;
|
||||||
|
|
||||||
// first few iterations we simply flash the leds
|
// flash the leds
|
||||||
led_mode = SAVE_TRIM_LEDS;
|
led_mode = SAVE_TRIM_LEDS;
|
||||||
|
|
||||||
if(save_trim_counter == 1) {
|
// calculate roll trim adjustment
|
||||||
// save roll trim
|
float roll_trim_adjustment = ToRad((float)-g.rc_1.control_in / 4000.0);
|
||||||
roll_trim = ToRad((float)g.rc_1.control_in/100.0);
|
|
||||||
pitch_trim = ToRad((float)g.rc_2.control_in/100.0);
|
|
||||||
ahrs.add_trim(roll_trim, pitch_trim);
|
|
||||||
|
|
||||||
reset_control_switch();
|
// calculate pitch trim adjustment
|
||||||
|
float pitch_trim_adjustment = ToRad((float)g.rc_2.control_in / 4000.0);
|
||||||
|
|
||||||
// switch back leds to normal
|
// make sure accelerometer values impact attitude quickly
|
||||||
|
ahrs.set_fast_gains(true);
|
||||||
|
|
||||||
|
// add trim to ahrs object
|
||||||
|
// save to eeprom on last iteration
|
||||||
|
ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
|
||||||
|
|
||||||
|
// on last iteration restore leds and accel gains to normal
|
||||||
|
if(auto_trim_counter == 0) {
|
||||||
|
ahrs.set_fast_gains(false);
|
||||||
led_mode = NORMAL_LEDS;
|
led_mode = NORMAL_LEDS;
|
||||||
|
|
||||||
save_trim_counter = 0;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -3,6 +3,7 @@
|
||||||
// 10 = 1 second
|
// 10 = 1 second
|
||||||
#define ARM_DELAY 20
|
#define ARM_DELAY 20
|
||||||
#define DISARM_DELAY 20
|
#define DISARM_DELAY 20
|
||||||
|
#define AUTO_TRIM_DELAY 100
|
||||||
|
|
||||||
|
|
||||||
// called at 10hz
|
// called at 10hz
|
||||||
|
@ -36,56 +37,58 @@ static void arm_motors()
|
||||||
|
|
||||||
// full right
|
// full right
|
||||||
if (tmp > 4000) {
|
if (tmp > 4000) {
|
||||||
if (arming_counter == ARM_DELAY) {
|
|
||||||
if(motors.armed() == false) {
|
|
||||||
// arm the motors and configure for flight
|
|
||||||
|
|
||||||
|
// increase the arming counter to a maximum of 1 beyond the auto trim counter
|
||||||
|
if( arming_counter <= AUTO_TRIM_DELAY ) {
|
||||||
|
arming_counter++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// arm the motors and configure for flight
|
||||||
|
if (arming_counter == ARM_DELAY && !motors.armed()) {
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters
|
// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
#if AP_LIMITS == ENABLED
|
#if AP_LIMITS == ENABLED
|
||||||
if (limits.enabled() && limits.required()) {
|
if (limits.enabled() && limits.required()) {
|
||||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Limits - Running pre-arm checks"));
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Limits - Running pre-arm checks"));
|
||||||
|
|
||||||
// check only pre-arm required modules
|
// check only pre-arm required modules
|
||||||
if (limits.check_required()) {
|
if (limits.check_required()) {
|
||||||
gcs_send_text_P(SEVERITY_LOW, PSTR("ARMING PREVENTED - Limit Breached"));
|
gcs_send_text_P(SEVERITY_LOW, PSTR("ARMING PREVENTED - Limit Breached"));
|
||||||
limits.set_state(LIMITS_TRIGGERED);
|
limits.set_state(LIMITS_TRIGGERED);
|
||||||
gcs_send_message(MSG_LIMITS_STATUS);
|
gcs_send_message(MSG_LIMITS_STATUS);
|
||||||
|
|
||||||
arming_counter++; // restart timer by cycling
|
arming_counter++; // restart timer by cycling
|
||||||
}else{
|
|
||||||
init_arm_motors();
|
|
||||||
}
|
|
||||||
}else{
|
}else{
|
||||||
init_arm_motors();
|
init_arm_motors();
|
||||||
}
|
}
|
||||||
|
}else{
|
||||||
#else // without AP_LIMITS, just arm motors
|
|
||||||
init_arm_motors();
|
init_arm_motors();
|
||||||
|
}
|
||||||
|
#else // without AP_LIMITS, just arm motors
|
||||||
|
init_arm_motors();
|
||||||
#endif //AP_LIMITS_ENABLED
|
#endif //AP_LIMITS_ENABLED
|
||||||
|
|
||||||
}
|
|
||||||
// keep going up
|
|
||||||
arming_counter++;
|
|
||||||
} else{
|
|
||||||
arming_counter++;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// full left
|
// arm the motors and configure for flight
|
||||||
|
if (arming_counter == AUTO_TRIM_DELAY && motors.armed()) {
|
||||||
|
auto_trim_counter = 250;
|
||||||
|
}
|
||||||
|
|
||||||
|
// full left
|
||||||
}else if (tmp < -4000) {
|
}else if (tmp < -4000) {
|
||||||
if (arming_counter == DISARM_DELAY) {
|
|
||||||
if(motors.armed()) {
|
// increase the counter to a maximum of 1 beyond the disarm delay
|
||||||
// arm the motors and configure for flight
|
if( arming_counter <= DISARM_DELAY ) {
|
||||||
init_disarm_motors();
|
|
||||||
}
|
|
||||||
// keep going up
|
|
||||||
arming_counter++;
|
|
||||||
}else{
|
|
||||||
arming_counter++;
|
arming_counter++;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Yaw is centered
|
// disarm the motors
|
||||||
|
if (arming_counter == DISARM_DELAY && motors.armed()) {
|
||||||
|
init_disarm_motors();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Yaw is centered so reset arming counter
|
||||||
}else{
|
}else{
|
||||||
arming_counter = 0;
|
arming_counter = 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -97,7 +97,7 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
|
||||||
}
|
}
|
||||||
|
|
||||||
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
|
// 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)
|
void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
|
||||||
{
|
{
|
||||||
Vector3f trim = _trim.get();
|
Vector3f trim = _trim.get();
|
||||||
|
|
||||||
|
@ -105,6 +105,11 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
|
||||||
trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0));
|
trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0));
|
||||||
trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0), ToRad(10.0));
|
trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0), ToRad(10.0));
|
||||||
|
|
||||||
// set and save new trim values
|
// set new trim values
|
||||||
_trim.set_and_save(trim);
|
_trim.set(trim);
|
||||||
|
|
||||||
|
// save to eeprom
|
||||||
|
if( save_to_eeprom ) {
|
||||||
|
_trim.save();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -148,7 +148,7 @@ public:
|
||||||
virtual void set_trim(Vector3f new_trim) { _trim.set_and_save(new_trim); }
|
virtual void set_trim(Vector3f new_trim) { _trim.set_and_save(new_trim); }
|
||||||
|
|
||||||
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
|
// 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);
|
virtual void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
|
||||||
|
|
||||||
// settable parameters
|
// settable parameters
|
||||||
AP_Float _kp_yaw;
|
AP_Float _kp_yaw;
|
||||||
|
|
Loading…
Reference in New Issue