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

removed wind comp code,

Browse Source 
Added Landing boost code
This commit is contained in:
Jason Short 2012-01-12 22:28:51 -08:00
parent 5418d330f6
commit b884462ce3
1 changed files with 17 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
ArduCopter/ArduCopter.pde
View File

@ -598,11 +598,12 @@ static boolean takeoff_complete;
static int32_t takeoff_timer; static int32_t takeoff_timer;
// Used to see if we have landed and if we should shut our engines - not fully implemented // Used to see if we have landed and if we should shut our engines - not fully implemented
static boolean land_complete = true; static boolean land_complete = true;
// used to manually override throttle in interactive Alt hold modes // used to manually override throttle in interactive Alt hold modes
static int16_t manual_boost; static int16_t manual_boost;
// An additional throttle added to keep the copter at the same altitude when banking // An additional throttle added to keep the copter at the same altitude when banking
static int16_t angle_boost; static int16_t angle_boost;
// Push copter down for clean landing
static uint8_t landing_boost;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -1418,16 +1419,9 @@ void update_roll_pitch_mode(void)
update_simple_mode(); update_simple_mode();
} }
#if WIND_COMP_STAB == 1 // in this mode, nav_roll and nav_pitch = the iterm
// in this mode, nav_roll and nav_pitch = the iterm g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll); g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch);
#else
// in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
#endif
break; break;
case ROLL_PITCH_AUTO: case ROLL_PITCH_AUTO:
@ -1616,9 +1610,9 @@ void update_throttle_mode(void)
} }
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping()); throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost);
#else #else
throttle_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping(); throttle_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping() - landing_boost;
#endif #endif
} }
@ -1702,6 +1696,7 @@ static void update_navigation()
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
} }
// Kick us out of loiter and begin landing if the auto_land_timer is set
if(auto_land_timer != 0 && (millis() - auto_land_timer) > 20000){ if(auto_land_timer != 0 && (millis() - auto_land_timer) > 20000){
// just to make sure we clear the timer // just to make sure we clear the timer
auto_land_timer = 0; auto_land_timer = 0;
@ -1713,10 +1708,6 @@ static void update_navigation()
break; break;
case LAND: case LAND:
wp_control = LOITER_MODE;
// verify land will override WP_control if we are below
// a certain altitude
verify_land(); verify_land();
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
@ -1835,6 +1826,7 @@ static void update_altitude()
#else #else
// This is real life // This is real life
// calc the vertical accel rate // calc the vertical accel rate
// positive = going up.
sonar_rate = (sonar_alt - old_sonar_alt) * 10; sonar_rate = (sonar_alt - old_sonar_alt) * 10;
old_sonar_alt = sonar_alt; old_sonar_alt = sonar_alt;
#endif #endif
@ -2006,6 +1998,7 @@ static void tuning(){
} }
} }
// Outputs Nav_Pitch and Nav_Roll
static void update_nav_wp() static void update_nav_wp()
{ {
if(wp_control == LOITER_MODE){ if(wp_control == LOITER_MODE){
@ -2071,24 +2064,17 @@ static void update_nav_wp()
// use error as the desired rate towards the target // use error as the desired rate towards the target
calc_nav_rate(speed); calc_nav_rate(speed);
// rotate pitch and roll to the copter frame of reference // rotate pitch and roll to the copter frame of reference
//calc_nav_pitch_roll();
calc_loiter_pitch_roll(); calc_loiter_pitch_roll();
}else if(wp_control == NO_NAV_MODE){ }else if(wp_control == NO_NAV_MODE){
// calc the Iterms for Loiter based on velocity // clear out our nav so we can do things like land straight down
#if WIND_COMP_STAB == 1
if (g_gps->ground_speed < 50)
calc_wind_compensation();
else
reduce_wind_compensation();
// rotate nav_lat, nav_lon to roll and pitch // We bring in our iterms for wind control, but we don't navigate
calc_loiter_pitch_roll(); nav_lon = g.pi_loiter_lon.get_integrator();
#else nav_lat = g.pi_loiter_lat.get_integrator();
// clear out our nav so we can do things like land straight
nav_pitch = 0; // rotate pitch and roll to the copter frame of reference
nav_roll = 0; calc_loiter_pitch_roll();
#endif
} }
} }