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

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
Jason Short 2011-11-06 22:46:57 -08:00
commit 8ac8ea9c1d
6 changed files with 38 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopter/ArduCopter.pde
View File

@ -1066,21 +1066,25 @@ void update_throttle_mode(void)
// clear the new data flag
invalid_throttle = false;
}
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle);
#else
angle_boost = get_angle_boost(g.throttle_cruise);
angle_boost = get_angle_boost(g.throttle_cruise);
if(manual_boost != 0){
//remove alt_hold_velocity when implemented
if(manual_boost != 0){
//remove alt_hold_velocity when implemented
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.throttle_cruise + nav_throttle + manual_boost + get_z_damping()));
#else
g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + get_z_damping();
// reset next_WP.alt
next_WP.alt = max(current_loc.alt, 100);
}else{
#endif
// reset next_WP.alt
next_WP.alt = max(current_loc.alt, 100);
}else{
#if FRAME_CONFIG == HELI_FRAME
//g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping());
g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.throttle_cruise + nav_throttle + get_z_damping()));
#else
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping();
}
#endif
#endif
}
break;
}
}
@ -1280,14 +1284,14 @@ adjust_altitude()
// we remove 0 to 100 PWM from hover
manual_boost = g.rc_3.control_in - 180;
manual_boost = max(-120, manual_boost);
g.throttle_cruise += g.pi_alt_hold.get_integrator();
g.throttle_cruise += (g.pi_alt_hold.get_integrator() * g.pi_throttle.kP() + g.pi_throttle.get_integrator());
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();
}else if (g.rc_3.control_in >= 650){
// we add 0 to 100 PWM to hover
manual_boost = g.rc_3.control_in - 650;
g.throttle_cruise += g.pi_alt_hold.get_integrator();
g.throttle_cruise += (g.pi_alt_hold.get_integrator() * g.pi_throttle.kP() + g.pi_throttle.get_integrator());
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();

9
ArduCopter/radio.pde
View File

@ -8,8 +8,13 @@ static void default_dead_zones()
{
g.rc_1.set_dead_zone(60);
g.rc_2.set_dead_zone(60);
g.rc_3.set_dead_zone(60);
g.rc_4.set_dead_zone(200);
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.set_dead_zone(0);
g.rc_4.set_dead_zone(60);
#else
g.rc_3.set_dead_zone(60);
g.rc_4.set_dead_zone(200);
#endif
}
static void init_rc_in()

6
ArduCopter/system.pde
View File

@ -527,11 +527,7 @@ init_throttle_cruise()
if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
g.pi_throttle.reset_I();
g.pi_alt_hold.reset_I();
#if FRAME_CONFIG == HELI_FRAME
g.throttle_cruise.set_and_save(heli_get_scaled_throttle(g.rc_3.control_in));
#else
g.throttle_cruise.set_and_save(g.rc_3.control_in);
#endif
g.throttle_cruise.set_and_save(g.rc_3.control_in);
}
}

3
libraries/AP_Common/Arduino.mk
View File

@ -36,6 +36,9 @@
#
SYSTYPE := $(shell uname)
# force LANG to C so awk works sanely on MacOS
export LANG=C
#
# Locate the sketch sources based on the initial Makefile's path
#

25
libraries/AP_DCM/AP_DCM.cpp
View File

@ -125,14 +125,6 @@ AP_DCM::matrix_update(float _G_Dt)
Matrix3f update_matrix;
Matrix3f temp_matrix;
//Record when you saturate any of the gyros.
/*
if( (fabs(_gyro_vector.x) >= radians(300)) ||
(fabs(_gyro_vector.y) >= radians(300)) ||
(fabs(_gyro_vector.z) >= radians(300))){
gyro_sat_count++;
}*/
_omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
@ -232,6 +224,9 @@ AP_DCM::normalize(void)
_dcm_matrix.c.x = 0.0f;
_dcm_matrix.c.y = 0.0f;
_dcm_matrix.c.z = 1.0f;
_omega_I.x = 0.0f;
_omega_I.y = 0.0f;
_omega_I.z = 0.0f;
}
}
@ -348,10 +343,10 @@ AP_DCM::drift_correction(void)
_omega_P += _error_yaw * _kp_yaw; // Adding yaw correction to proportional correction vector.
_omega_I += _error_yaw * _ki_yaw; // adding yaw correction to integrator correction vector.
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
// Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second
integrator_magnitude = _omega_I.length();
if (integrator_magnitude > radians(300)) {
_omega_I *= (0.5f * radians(300) / integrator_magnitude); // Why do we have this discontinuous? EG, why the 0.5?
if (integrator_magnitude > radians(30)) {
_omega_I *= (radians(30) / integrator_magnitude);
}
//Serial.print("*");
}
@ -398,12 +393,4 @@ AP_DCM::euler_yaw(void)
yaw_sensor += 36000;
}
/**************************************************/
float
AP_DCM::get_health(void)
{
return _health;
}

6
libraries/AP_DCM/AP_DCM.h
View File

@ -43,7 +43,9 @@ public:
Vector3f get_accel(void) { return _accel_vector; }
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
Vector3f get_integrator(void) {return _omega_I; } // We return the current drift correction integrator values
float get_health(void) {return _health;}
void set_centripetal(bool b) {_centripetal = b;}
bool get_centripetal(void) {return _centripetal;}
void set_compass(Compass *compass);
@ -52,8 +54,6 @@ public:
void update_DCM(void);
void update_DCM_fast(void);
float get_health(void);
long roll_sensor; // Degrees * 100
long pitch_sensor; // Degrees * 100
long yaw_sensor; // Degrees * 100