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

ArduCopter: changed alt hold to use the sonar if enabled and healthy

Browse Source
This commit is contained in:
rmackay9 2013-01-10 21:43:13 +09:00
parent 4f00c1d8b2
commit 4d21571d92
2 changed files with 7 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduCopter/ArduCopter.pde
View File

@ -1795,7 +1795,6 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
case THROTTLE_HOLD:
case THROTTLE_AUTO:
case THROTTLE_SURFACE_TRACKING:
controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude
set_new_altitude(current_loc.alt); // by default hold the current altitude
if ( throttle_mode < THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual
@ -1958,7 +1957,13 @@ void update_throttle_mode(void)
case THROTTLE_HOLD:
// alt hold plus pilot input of climb rate
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate_stabilized(pilot_climb_rate);
if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {
// if sonar is ok, use surface tracking
get_throttle_surface_tracking(pilot_climb_rate);
}else{
// if no sonar fall back stabilize rate controller
get_throttle_rate_stabilized(pilot_climb_rate);
}
break;
case THROTTLE_AUTO:
@ -1972,18 +1977,6 @@ void update_throttle_mode(void)
// landing throttle controller
get_throttle_land();
break;
case THROTTLE_SURFACE_TRACKING:
// surface tracking with sonar or other rangefinder plus pilot input of climb rate
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {
// if sonar is ok, use surface tracking
get_throttle_surface_tracking(pilot_climb_rate);
}else{
// if no sonar fall back stabilize rate controller
get_throttle_rate_stabilized(pilot_climb_rate);
}
break;
}
}

1
ArduCopter/defines.h
View File

@ -39,7 +39,6 @@
#define THROTTLE_HOLD 6 // alt hold plus pilot input of climb rate
#define THROTTLE_AUTO 7 // auto pilot altitude controller with target altitude held in next_WP.alt
#define THROTTLE_LAND 8 // landing throttle controller
#define THROTTLE_SURFACE_TRACKING 9 // ground tracking with sonar or other rangefinder
// active altitude sensor