Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: add set_throttle_mode to better control initialisation of variables

Browse Source
This commit is contained in:
rmackay9 2012-11-24 16:45:28 +09:00
parent 1285198b71
commit e629fe2eb0
5 changed files with 77 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
ArduCopter/ArduCopter.pde
View File

@ -431,8 +431,6 @@ static int16_t lon_speed; // expressed in cm/s. positive numbers mean mov
static int16_t lat_speed; // expressed in cm/s. positive numbers when moving north
static int16_t ground_bearing; // expressed in centidegrees
static int16_t desired_climb_rate;
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t x_rate_error;
@ -616,6 +614,7 @@ static int16_t throttle_accel_target_ef = 0; // earth frame throttle accelera
static bool throttle_accel_controller_active = false; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directly
static float z_accel_meas = 0; // filtered throttle acceleration
static float throttle_avg; // g.throttle_cruise as a float
static int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only
////////////////////////////////////////////////////////////////////////////////
@ -698,8 +697,6 @@ static int16_t sonar_rate;
static int32_t baro_alt;
// The climb_rate as reported by Baro in cm/s
static int16_t baro_rate;
// used to switch out of Manual Boost
static uint8_t reset_throttle_counter;
static int16_t saved_toy_throttle;
@ -1669,6 +1666,64 @@ void update_simple_mode(void)
g.rc_2.control_in = _pitch;
}
// set_throttle_mode - sets the throttle mode and initialises any variables as required
bool set_throttle_mode( uint8_t new_throttle_mode )
{
// boolean to ensure proper initialisation of throttle modes
bool throttle_initialised = false;
// initialise any variables required for the new throttle mode
switch(new_throttle_mode) {
case THROTTLE_MANUAL:
case THROTTLE_MANUAL_TILT_COMPENSATED:
throttle_accel_controller_active = false; // this controller does not use accel based throttle controller
throttle_initialised = true;
break;
case THROTTLE_ACCELERATION:
case THROTTLE_RATE:
case THROTTLE_STABILIZED_RATE:
case THROTTLE_DIRECT_ALT:
throttle_accel_controller_active = true; // this controller uses accel based throttle controller
throttle_initialised = true;
break;
case THROTTLE_HOLD:
case THROTTLE_AUTO:
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
reset_throttle_I();
}
throttle_accel_controller_active = true; // this controller uses accel based throttle controller
throttle_initialised = true;
break;
case THROTTLE_LAND:
set_land_complete(false); // mark landing as incomplete
ground_detector = 0; // A counter that goes up if our climb rate stalls out.
set_new_altitude(0); // Set a new target altitude
throttle_accel_controller_active = true; // this controller uses accel based throttle controller
throttle_initialised = true;
break;
default:
cliSerial->printf_P(PSTR("Unsupported throttle mode: %d!!"),new_throttle_mode);
break;
}
// update the throttle mode
if( throttle_initialised ) {
throttle_mode = new_throttle_mode;
// reset some variables used for logging
desired_climb_rate = 0;
nav_throttle = 0;
}
// return success or failure
return throttle_initialised;
}
// 50 hz update rate
// controls all throttle behavior
void update_throttle_mode(void)

3
ArduCopter/Attitude.pde
View File

@ -864,6 +864,9 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
desired_rate = 0;
}
// desired climb rate for logging
desired_climb_rate = desired_rate;
return desired_rate;
}

11
ArduCopter/commands_logic.pde
View File

@ -272,19 +272,10 @@ static void do_nav_wp()
static void do_land()
{
wp_control = LOITER_MODE;
throttle_mode = THROTTLE_LAND;
// just to make sure
set_land_complete(false);
// A counter that goes up if our climb rate stalls out.
ground_detector = 0;
set_throttle_mode(THROTTLE_LAND);
// hold at our current location
set_next_WP(&current_loc);
// Set a new target altitude
set_new_altitude(0);
}
static void do_loiter_unlimited()

2
ArduCopter/limits.pde
View File

@ -269,7 +269,7 @@ static void limits_loop() {
//set_recovery_home_alt();
set_mode(POSITION);
throttle_mode = THROTTLE_AUTO;
set_throttle_mode(THROTTLE_AUTO);
limits.last_action = millis();
gcs_send_message(MSG_LIMITS_STATUS);
break;

38
ArduCopter/system.pde
View File

@ -410,9 +410,6 @@ static void set_mode(byte mode)
motors.auto_armed(g.rc_3.control_in > 0);
set_auto_armed(g.rc_3.control_in > 0);
// clearing value used in interactive alt hold
reset_throttle_counter = 0;
// do not auto_land if we are leaving RTL
loiter_timer = 0;
@ -436,7 +433,7 @@ static void set_mode(byte mode)
ap.manual_attitude = true;
yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO;
throttle_mode = THROTTLE_MANUAL;
set_throttle_mode(THROTTLE_MANUAL);
// reset acro axis targets to current attitude
if(g.axis_enabled){
roll_axis = ahrs.roll_sensor;
@ -450,7 +447,7 @@ static void set_mode(byte mode)
ap.manual_attitude = true;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE;
throttle_mode = THROTTLE_MANUAL_TILT_COMPENSATED;
set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
break;
case ALT_HOLD:
@ -458,7 +455,7 @@ static void set_mode(byte mode)
ap.manual_attitude = true;
yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP;
throttle_mode = ALT_HOLD_THR;
set_throttle_mode(ALT_HOLD_THR);
force_new_altitude(max(current_loc.alt, 100));
break;
@ -467,7 +464,7 @@ static void set_mode(byte mode)
ap.manual_attitude = false;
yaw_mode = AUTO_YAW;
roll_pitch_mode = AUTO_RP;
throttle_mode = AUTO_THR;
set_throttle_mode(AUTO_THR);
// loads the commands from where we left off
init_commands();
@ -478,7 +475,7 @@ static void set_mode(byte mode)
ap.manual_attitude = false;
yaw_mode = CIRCLE_YAW;
roll_pitch_mode = CIRCLE_RP;
throttle_mode = CIRCLE_THR;
set_throttle_mode(CIRCLE_THR);
set_next_WP(&current_loc);
circle_WP = next_WP;
circle_angle = 0;
@ -489,7 +486,7 @@ static void set_mode(byte mode)
ap.manual_attitude = false;
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
throttle_mode = LOITER_THR;
set_throttle_mode(LOITER_THR);
set_next_WP(&current_loc);
break;
@ -498,7 +495,7 @@ static void set_mode(byte mode)
ap.manual_attitude = false;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_MANUAL;
set_throttle_mode(THROTTLE_MANUAL);
set_next_WP(&current_loc);
break;
@ -507,7 +504,7 @@ static void set_mode(byte mode)
ap.manual_attitude = false;
yaw_mode = YAW_AUTO;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO;
set_throttle_mode(THROTTLE_AUTO);
next_WP = current_loc;
set_next_WP(&guided_WP);
break;
@ -525,7 +522,6 @@ static void set_mode(byte mode)
roll_pitch_mode = ROLL_PITCH_STABLE;
}
ap.manual_throttle = false;
throttle_mode = THROTTLE_LAND;
do_land();
break;
@ -534,7 +530,7 @@ static void set_mode(byte mode)
ap.manual_attitude = false;
yaw_mode = RTL_YAW;
roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR;
set_throttle_mode(RTL_THR);
set_rtl_reached_alt(false);
set_next_WP(&current_loc);
set_new_altitude(get_RTL_alt());
@ -545,7 +541,7 @@ static void set_mode(byte mode)
ap.manual_attitude = false;
yaw_mode = OF_LOITER_YAW;
roll_pitch_mode = OF_LOITER_RP;
throttle_mode = OF_LOITER_THR;
set_throttle_mode(OF_LOITER_THR);
set_next_WP(&current_loc);
break;
@ -557,14 +553,12 @@ static void set_mode(byte mode)
ap.manual_attitude = true;
yaw_mode = YAW_TOY;
roll_pitch_mode = ROLL_PITCH_TOY;
throttle_mode = THROTTLE_AUTO;
set_throttle_mode(THROTTLE_AUTO);
wp_control = NO_NAV_MODE;
// save throttle for fast exit of Alt hold
saved_toy_throttle = g.rc_3.control_in;
// hold the current altitude
set_new_altitude(current_loc.alt);
break;
case TOY_M:
@ -573,7 +567,7 @@ static void set_mode(byte mode)
yaw_mode = YAW_TOY;
roll_pitch_mode = ROLL_PITCH_TOY;
wp_control = NO_NAV_MODE;
throttle_mode = THROTTLE_HOLD;
set_throttle_mode(THROTTLE_HOLD);
break;
default:
@ -582,7 +576,7 @@ static void set_mode(byte mode)
if(ap.failsafe) {
// this is to allow us to fly home without interactive throttle control
throttle_mode = THROTTLE_AUTO;
set_throttle_mode(THROTTLE_AUTO);
ap.manual_throttle = false;
// does not wait for us to be in high throttle, since the
@ -591,18 +585,12 @@ static void set_mode(byte mode)
set_auto_armed(true);
}
if(ap.manual_throttle) {
desired_climb_rate = 0;
}
if(ap.manual_attitude) {
// We are under manual attitude control
// remove the navigation from roll and pitch command
reset_nav_params();
// remove the wind compenstaion
reset_wind_I();
// Clears the alt hold compensation
reset_throttle_I();
}
Log_Write_Mode(control_mode);