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

ArduCopter: update altitude_error reported to GCS

This commit is contained in:
rmackay9 2013-01-05 10:55:18 +09:00
parent cbbba3a359
commit 0a101162c2
2 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/ArduCopter.pde
View File

@ -1767,16 +1767,21 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
case THROTTLE_MANUAL:
case THROTTLE_MANUAL_TILT_COMPENSATED:
throttle_accel_deactivate(); // this controller does not use accel based throttle controller
altitude_error = 0; // clear altitude error reported to GCS
throttle_initialised = true;
break;
case THROTTLE_ACCELERATION: // pilot inputs the desired acceleration
if( g.throttle_accel_enabled ) { // this throttle mode requires use of the accel based throttle controller
altitude_error = 0; // clear altitude error reported to GCS
throttle_initialised = true;
}
break;
case THROTTLE_RATE:
altitude_error = 0; // clear altitude error reported to GCS
throttle_initialised = true;
break;
case THROTTLE_STABILIZED_RATE:
case THROTTLE_DIRECT_ALT:
throttle_initialised = true;
@ -1930,6 +1935,7 @@ void update_throttle_mode(void)
if(g.rc_3.control_in <= 0){
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS
}else{
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate_stabilized(pilot_climb_rate);
@ -1941,8 +1947,8 @@ void update_throttle_mode(void)
if(g.rc_3.control_in <= 0){
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS
}else{
// To-Do: this should update the global desired altitude variable next_WP.alt
int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in);
get_throttle_althold(desired_alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
}

17
ArduCopter/Attitude.pde
View File

@ -988,22 +988,22 @@ get_throttle_rate(int16_t z_target_speed)
static void
get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
{
int32_t altitude_error;
int32_t alt_error;
int16_t desired_rate;
int32_t linear_distance; // the distace we swap between linear and sqrt.
// calculate altitude error
altitude_error = target_alt - current_loc.alt;
alt_error = target_alt - current_loc.alt;
// check kP to avoid division by zero
if( g.pi_alt_hold.kP() != 0 ) {
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if( altitude_error > 2*linear_distance ) {
desired_rate = sqrt(2*250*(altitude_error-linear_distance));
}else if( altitude_error < -2*linear_distance ) {
desired_rate = -sqrt(2*250*(-altitude_error-linear_distance));
if( alt_error > 2*linear_distance ) {
desired_rate = sqrt(2*250*(alt_error-linear_distance));
}else if( alt_error < -2*linear_distance ) {
desired_rate = -sqrt(2*250*(-alt_error-linear_distance));
}else{
desired_rate = g.pi_alt_hold.get_p(altitude_error);
desired_rate = g.pi_alt_hold.get_p(alt_error);
}
}else{
desired_rate = 0;
@ -1014,6 +1014,9 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
// call rate based throttle controller which will update accel based throttle controller targets
get_throttle_rate(desired_rate);
// update altitude error reported to GCS
altitude_error = alt_error;
// TO-DO: enabled PID logging for this controller
}