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

Plane: removed old speed/altitude control algorithms 

Use TECS only. This makes the code a lot simpler and easier to
properly document
This commit is contained in:
Andrew Tridgell 2013-07-22 13:28:11 +10:00
parent ef104b6629
commit 233b033e8c
6 changed files with 12 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduPlane/ArduPlane.pde
View File

@ -428,13 +428,6 @@ static int32_t target_airspeed_cm;
// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
static float airspeed_error_cm;
// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
// Used by the throttle controller
static int32_t energy_error;
// kinetic portion of energy error (m^2/s^2)
static int32_t airspeed_energy_error;
// An amount that the airspeed should be increased in auto modes based on the user positioning the
// throttle stick in the top half of the range. Centimeters per second.
static int16_t airspeed_nudge_cm;
@ -827,10 +820,7 @@ static void fast_loop()
*/
static void update_speed_height(void)
{
if ((g.alt_control_algorithm == ALT_CONTROL_TECS ||
g.alt_control_algorithm == ALT_CONTROL_DEFAULT) &&
auto_throttle_mode && !throttle_suppressed)
{
if (auto_throttle_mode && !throttle_suppressed) {
// Call TECS 50Hz update
SpdHgt_Controller->update_50hz(relative_altitude());
}
@ -1068,7 +1058,7 @@ static void update_flight_mode(void)
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
}
if (alt_control_airspeed()) {
if (airspeed.use()) {
calc_nav_pitch();
if (nav_pitch_cd < takeoff_pitch_cd)
nav_pitch_cd = takeoff_pitch_cd;
@ -1094,7 +1084,7 @@ static void update_flight_mode(void)
nav_pitch_cd = g.land_pitch_cd;
} else {
calc_nav_pitch();
if (!alt_control_airspeed()) {
if (!airspeed.use()) {
// when not under airspeed control, don't allow
// down pitch in landing
nav_pitch_cd = constrain_int32(nav_pitch_cd, 0, nav_pitch_cd);
@ -1305,13 +1295,8 @@ static void update_alt()
geofence_check(true);
// Calculate new climb rate
//if(medium_loopCounter == 0 && slow_loopCounter == 0)
// add_altitude_data(millis() / 100, g_gps->altitude / 10);
// Update the speed & height controller states
if ((g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) &&
auto_throttle_mode && !throttle_suppressed) {
if (auto_throttle_mode && !throttle_suppressed) {
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100),
target_airspeed_cm,
(control_mode==AUTO && takeoff_complete == false),

47
ArduPlane/Attitude.pde
View File

@ -354,39 +354,7 @@ static void calc_throttle()
return;
}
if (g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) {
channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand();
} else if (!alt_control_airspeed()) {
int16_t throttle_target = aparm.throttle_cruise + throttle_nudge;
// TODO: think up an elegant way to bump throttle when
// groundspeed_undershoot > 0 in the no airspeed sensor case; PID
// control?
// no airspeed sensor, we use nav pitch to determine the proper throttle output
// AUTO, RTL, etc
// ---------------------------------------------------------------------------
if (nav_pitch_cd >= 0) {
channel_throttle->servo_out = throttle_target + (aparm.throttle_max - throttle_target) * nav_pitch_cd / aparm.pitch_limit_max_cd;
} else {
channel_throttle->servo_out = throttle_target - (throttle_target - aparm.throttle_min) * nav_pitch_cd / aparm.pitch_limit_min_cd;
}
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, aparm.throttle_min.get(), aparm.throttle_max.get());
} else {
// throttle control with airspeed compensation
// -------------------------------------------
energy_error = airspeed_energy_error + altitude_error_cm * 0.098f;
// positive energy errors make the throttle go higher
channel_throttle->servo_out = aparm.throttle_cruise + g.pidTeThrottle.get_pid(energy_error);
channel_throttle->servo_out += (channel_pitch->servo_out * g.kff_pitch_to_throttle);
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
aparm.throttle_min.get(), aparm.throttle_max.get());
}
channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand();
}
/*****************************************
@ -419,13 +387,7 @@ static void calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
if (g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) {
nav_pitch_cd = SpdHgt_Controller->get_pitch_demand();
} else if (alt_control_airspeed()) {
nav_pitch_cd = -g.pidNavPitchAirspeed.get_pid(airspeed_error_cm);
} else {
nav_pitch_cd = g.pidNavPitchAltitude.get_pid(altitude_error_cm);
}
nav_pitch_cd = SpdHgt_Controller->get_pitch_demand();
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
}
@ -864,8 +826,3 @@ static void demo_servos(uint8_t i)
}
}
// return true if we should use airspeed for altitude/throttle control
static bool alt_control_airspeed(void)
{
return airspeed.use() && g.alt_control_algorithm == ALT_CONTROL_AIRSPEED;
}

16
ArduPlane/Parameters.h
View File

@ -200,7 +200,7 @@ public:
//
k_param_kff_pitch_compensation = 200, // unused
k_param_kff_rudder_mix,
k_param_kff_pitch_to_throttle,
k_param_kff_pitch_to_throttle, // unused
k_param_kff_throttle_to_pitch,
k_param_scaling_speed,
@ -244,10 +244,10 @@ public:
k_param_pidNavRoll = 240, // unused
k_param_pidServoRoll, // unused
k_param_pidServoPitch, // unused
k_param_pidNavPitchAirspeed,
k_param_pidServoRudder,
k_param_pidTeThrottle,
k_param_pidNavPitchAltitude,
k_param_pidNavPitchAirspeed, // unused
k_param_pidServoRudder, // unused
k_param_pidTeThrottle, // unused
k_param_pidNavPitchAltitude, // unused
k_param_pidWheelSteer,
// 254,255: reserved
@ -415,9 +415,6 @@ public:
AP_YawController yawController;
// PID controllers
PID pidNavPitchAirspeed;
PID pidTeThrottle;
PID pidNavPitchAltitude;
PID pidWheelSteer;
Parameters() :
@ -449,9 +446,6 @@ public:
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM),
pidWheelSteer (0, 0, 0, 0)
{}

11
ArduPlane/Parameters.pde
View File

@ -59,14 +59,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(kff_rudder_mix, "KFF_RDDRMIX", RUDDER_MIX),
// @Param: KFF_PTCH2THR
// @DisplayName: Pitch to Throttle Mix
// @Description: Pitch to throttle feed-forward gain.
// @Range: 0 5
// @Increment: 0.01
// @User: Advanced
GSCALAR(kff_pitch_to_throttle, "KFF_PTCH2THR", 0),
// @Param: KFF_THR2PTCH
// @DisplayName: Throttle to Pitch Mix
// @Description: Throttle to pitch feed-forward gain.
@ -812,9 +804,6 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(rc_12, "RC12_", RC_Channel_aux),
#endif
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
GGROUP(pidWheelSteer, "WHEELSTEER_",PID),
// @Group: RLL2SRV_

1
ArduPlane/navigation.pde
View File

@ -111,7 +111,6 @@ static void calc_airspeed_errors()
target_airspeed_cm = (aparm.airspeed_max * 100);
airspeed_error_cm = target_airspeed_cm - aspeed_cm;
airspeed_energy_error = ((target_airspeed_cm * target_airspeed_cm) - (aspeed_cm*aspeed_cm))*0.00005;
}
static void calc_gndspeed_undershoot()

10
ArduPlane/radio.pde
View File

@ -101,7 +101,7 @@ static void read_radio()
if (g.throttle_nudge && channel_throttle->servo_out > 50) {
float nudge = (channel_throttle->servo_out - 50) * 0.02;
if (alt_control_airspeed()) {
if (airspeed.use()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
@ -110,14 +110,6 @@ static void read_radio()
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
/*
* cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
* (int)g.rc_1.control_in,
* (int)g.rc_2.control_in,
* (int)g.rc_3.control_in,
* (int)g.rc_4.control_in);
*/
}
static void control_failsafe(uint16_t pwm)