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

Arducopter: Altitude change nav 

Split altitude hold into two controls to parallel navigation.
This commit is contained in:
Jason Short 2012-07-18 22:40:31 -07:00
parent cb1cdcd4a7
commit aa645afe2b
1 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopter/Attitude.pde
View File

@ -137,7 +137,6 @@ get_acro_yaw(int32_t target_rate)
static int16_t static int16_t
get_acro_yaw2(int32_t target_rate) get_acro_yaw2(int32_t target_rate)
{ {
static int32_t last_target_rate = 0; // last iteration's target rate
int32_t p,i,d; // used to capture pid values for logging int32_t p,i,d; // used to capture pid values for logging
int32_t rate_error; // current yaw rate error int32_t rate_error; // current yaw rate error
int32_t current_rate; // current real yaw rate int32_t current_rate; // current real yaw rate
@ -155,6 +154,7 @@ get_acro_yaw2(int32_t target_rate)
//strengthen the yaw output to slow down the yaw! //strengthen the yaw output to slow down the yaw!
#if (FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME) #if (FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME)
static int32_t last_target_rate = 0; // last iteration's target rate
if ( target_rate > 0 && last_target_rate > target_rate && rate_error < 0 ){ if ( target_rate > 0 && last_target_rate > target_rate && rate_error < 0 ){
decel_boost = 1; decel_boost = 1;
} else if (target_rate < 0 && last_target_rate < target_rate && rate_error > 0 ){ } else if (target_rate < 0 && last_target_rate < target_rate && rate_error > 0 ){
@ -345,10 +345,7 @@ get_rate_yaw(int32_t target_rate)
static int16_t static int16_t
get_nav_throttle(int32_t z_error) get_nav_throttle(int32_t z_error)
{ {
int16_t z_rate_error, z_target_speed, output; int16_t z_target_speed;
// a small boost for alt control to improve takeoff
//int16_t boost_p = constrain(z_error >> 1, -10, 50);
// convert to desired Rate: // convert to desired Rate:
z_target_speed = g.pi_alt_hold.get_p(z_error); z_target_speed = g.pi_alt_hold.get_p(z_error);
@ -360,6 +357,16 @@ get_nav_throttle(int32_t z_error)
// compensates throttle setpoint error for hovering // compensates throttle setpoint error for hovering
int16_t i_hold = g.pi_alt_hold.get_i(z_error, .02); int16_t i_hold = g.pi_alt_hold.get_i(z_error, .02);
// output control:
return get_throttle_rate(z_target_speed) + i_hold; //+ boost_p;
}
static int16_t
get_throttle_rate(int16_t z_target_speed)
{
int16_t z_rate_error, output;
// calculate rate error // calculate rate error
#if INERTIAL_NAV == ENABLED #if INERTIAL_NAV == ENABLED
z_rate_error = z_target_speed - accels_velocity.z; // calc the speed error z_rate_error = z_target_speed - accels_velocity.z; // calc the speed error
@ -367,11 +374,16 @@ get_nav_throttle(int32_t z_error)
z_rate_error = z_target_speed - climb_rate; // calc the speed error z_rate_error = z_target_speed - climb_rate; // calc the speed error
#endif #endif
// limit the rate int32_t tmp = (z_target_speed * z_target_speed * (int32_t)g.throttle_cruise) / 200000;
output = constrain(g.pid_throttle.get_pid(z_rate_error, .02), -80, 120);
// output control: if(z_target_speed < 0) tmp = -tmp;
return output + i_hold; //+ boost_p;
output = constrain(tmp, -3200, 3200);
// limit the rate
output += constrain(g.pid_throttle.get_pid(z_rate_error, .02), -80, 120);
return output;
} }
// Keeps old data out of our calculation / logs // Keeps old data out of our calculation / logs
@ -777,7 +789,7 @@ void roll_pitch_toy()
//g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); //g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
if(g.rc_1.control_in != 0){ // roll if(g.rc_1.control_in != 0){ // roll
g.rc_4.servo_out = get_acro_yaw(yaw_rate); g.rc_4.servo_out = get_acro_yaw(yaw_rate/2);
yaw_stopped = false; yaw_stopped = false;
yaw_timer = 150; yaw_timer = 150;
}else if (!yaw_stopped){ }else if (!yaw_stopped){