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

Plane: removed the old roll/pitch/yaw PID controllers

Browse Source 
use the new controllers, with auto parameter conversion
This commit is contained in:
Andrew Tridgell 2013-05-30 22:58:31 +10:00
parent 3e8af05d15
commit cd55ed45e2
4 changed files with 27 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.pde
View File

@ -1050,8 +1050,6 @@ static void update_current_flight_mode(void)
// don't use a pitch/roll integrators during takeoff if we are // don't use a pitch/roll integrators during takeoff if we are
// below minimum speed // below minimum speed
if (aspeed < g.flybywire_airspeed_min) { if (aspeed < g.flybywire_airspeed_min) {
g.pidServoPitch.reset_I();
g.pidServoRoll.reset_I();
g.pitchController.reset_I(); g.pitchController.reset_I();
g.rollController.reset_I(); g.rollController.reset_I();
} }

55
ArduPlane/Attitude.pde
View File

@ -73,18 +73,7 @@ static void stabilize_roll(float speed_scaler)
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000; if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
} }
switch (g.att_controller) { g.channel_roll.servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min);
case ATT_CONTROL_APMCONTROL:
// calculate roll and pitch control using new APM_Control library
g.channel_roll.servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min);
break;
default:
// Calculate dersired servo output for the roll
// ---------------------------------------------
g.channel_roll.servo_out = g.pidServoRoll.get_pid_4500((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
break;
}
} }
/* /*
@ -95,26 +84,10 @@ static void stabilize_roll(float speed_scaler)
static void stabilize_pitch(float speed_scaler) static void stabilize_pitch(float speed_scaler)
{ {
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + g.channel_throttle.servo_out * g.kff_throttle_to_pitch; int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + g.channel_throttle.servo_out * g.kff_throttle_to_pitch;
switch (g.att_controller) { g.channel_pitch.servo_out = g.pitchController.get_servo_out(demanded_pitch,
case ATT_CONTROL_APMCONTROL: { speed_scaler,
g.channel_pitch.servo_out = g.pitchController.get_servo_out(demanded_pitch, control_mode == STABILIZE,
speed_scaler, g.flybywire_airspeed_min, g.flybywire_airspeed_max);
control_mode == STABILIZE,
g.flybywire_airspeed_min, g.flybywire_airspeed_max);
break;
}
default: {
int32_t tempcalc = demanded_pitch - ahrs.pitch_sensor;
tempcalc += fabsf(ahrs.roll_sensor * g.kff_pitch_compensation);
if (abs(ahrs.roll_sensor) > 9000) {
// when flying upside down the elevator control is inverted
tempcalc = -tempcalc;
}
g.channel_pitch.servo_out = g.pidServoPitch.get_pid_4500(tempcalc, speed_scaler);
break;
}
}
} }
/* /*
@ -334,23 +307,13 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf)
return; return;
} }
switch (g.att_controller) { g.channel_rudder.servo_out = g.yawController.get_servo_out(speed_scaler,
case ATT_CONTROL_APMCONTROL: control_mode == STABILIZE,
g.channel_rudder.servo_out = g.yawController.get_servo_out(speed_scaler, g.flybywire_airspeed_min, g.flybywire_airspeed_max);
control_mode == STABILIZE,
g.flybywire_airspeed_min, g.flybywire_airspeed_max);
break;
default:
// a PID to coordinate the turn (drive y axis accel to zero)
float temp_y = ins.get_accel().y;
int32_t error = -temp_y * 100.0f;
g.channel_rudder.servo_out = g.pidServoRudder.get_pid_4500(error, speed_scaler);
break;
}
// add in rudder mixing from roll // add in rudder mixing from roll
g.channel_rudder.servo_out += g.channel_roll.servo_out * g.kff_rudder_mix; g.channel_rudder.servo_out += g.channel_roll.servo_out * g.kff_rudder_mix;
g.channel_rudder.servo_out = constrain_int16(g.channel_rudder.servo_out, -4500, 4500);
} }

22
ArduPlane/Parameters.h
View File

@ -190,7 +190,7 @@ public:
// //
// 200: Feed-forward gains // 200: Feed-forward gains
// //
k_param_kff_pitch_compensation = 200, k_param_kff_pitch_compensation = 200, // unused
k_param_kff_rudder_mix, k_param_kff_rudder_mix,
k_param_kff_pitch_to_throttle, k_param_kff_pitch_to_throttle,
k_param_kff_throttle_to_pitch, k_param_kff_throttle_to_pitch,
@ -232,8 +232,8 @@ public:
// //
// 240: PID Controllers // 240: PID Controllers
k_param_pidNavRoll = 240, // unused k_param_pidNavRoll = 240, // unused
k_param_pidServoRoll, k_param_pidServoRoll, // unused
k_param_pidServoPitch, k_param_pidServoPitch, // unused
k_param_pidNavPitchAirspeed, k_param_pidNavPitchAirspeed,
k_param_pidServoRudder, k_param_pidServoRudder,
k_param_pidTeThrottle, k_param_pidTeThrottle,
@ -256,7 +256,6 @@ public:
// Feed-forward gains // Feed-forward gains
// //
AP_Float kff_pitch_compensation;
AP_Float kff_rudder_mix; AP_Float kff_rudder_mix;
AP_Float kff_pitch_to_throttle; AP_Float kff_pitch_to_throttle;
AP_Float kff_throttle_to_pitch; AP_Float kff_throttle_to_pitch;
@ -267,9 +266,6 @@ public:
// navigation controller type. See AP_Navigation::ControllerType // navigation controller type. See AP_Navigation::ControllerType
AP_Int8 nav_controller; AP_Int8 nav_controller;
// attitude controller type.
AP_Int8 att_controller;
// Estimation // Estimation
// //
AP_Float altitude_mix; AP_Float altitude_mix;
@ -397,16 +393,13 @@ public:
RC_Channel_aux rc_12; RC_Channel_aux rc_12;
#endif #endif
// PID controllers // Attitude to servo controllers
// //
PID pidServoRoll;
PID pidServoPitch;
PID pidServoRudder;
AP_RollController rollController; AP_RollController rollController;
AP_PitchController pitchController; AP_PitchController pitchController;
AP_YawController yawController; AP_YawController yawController;
// PID controllers
PID pidNavPitchAirspeed; PID pidNavPitchAirspeed;
PID pidTeThrottle; PID pidTeThrottle;
PID pidNavPitchAltitude; PID pidNavPitchAltitude;
@ -433,11 +426,6 @@ public:
#endif #endif
// PID controller initial P initial I initial D initial imax // PID controller initial P initial I initial D initial imax
//----------------------------------------------------------------------------------- //-----------------------------------------------------------------------------------
pidServoRoll (SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
pidServoPitch (SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), 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), 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), pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM),

41
ArduPlane/Parameters.pde
View File

@ -54,14 +54,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Increment: 1 // @Increment: 1
GSCALAR(telem_delay, "TELEM_DELAY", 0), GSCALAR(telem_delay, "TELEM_DELAY", 0),
// @Param: KFF_PTCHCOMP
// @DisplayName: Pitch Compensation
// @Description: Adds pitch input to compensate for the loss of lift due to roll control. 0 = 0 %, 1 = 100%
// @Range: 0 1
// @Increment: 0.01
// @User: Advanced
GSCALAR(kff_pitch_compensation, "KFF_PTCHCOMP", PITCH_COMP),
// @Param: KFF_RDDRMIX // @Param: KFF_RDDRMIX
// @DisplayName: Rudder Mix // @DisplayName: Rudder Mix
// @Description: The amount of rudder mix to apply during aileron movement 0 = 0 %, 1 = 100% // @Description: The amount of rudder mix to apply during aileron movement 0 = 0 %, 1 = 100%
@ -150,13 +142,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard // @User: Standard
GSCALAR(nav_controller, "NAV_CONTROLLER", AP_Navigation::CONTROLLER_L1), GSCALAR(nav_controller, "NAV_CONTROLLER", AP_Navigation::CONTROLLER_L1),
// @Param: ATT_CONTROLLER
// @DisplayName: Attitude controller selection
// @Description: Which attitude (roll, pitch, yaw) controller to enable
// @Values: 0:PID,1:APMControl
// @User: Standard
GSCALAR(att_controller, "ATT_CONTROLLER", ATT_CONTROL_PID),
// @Param: ALT_MIX // @Param: ALT_MIX
// @DisplayName: GPS to Baro Mix // @DisplayName: GPS to Baro Mix
// @Description: The percent of mixing between GPS altitude and baro altitude. 0 = 100% gps, 1 = 100% baro. It is highly recommend that you not change this from the default of 1, as GPS altitude is notoriously unreliable. The only time I would recommend changing this is if you have a high altitude enabled GPS, and you are dropping a plane from a high altitude baloon many kilometers off the ground. // @Description: The percent of mixing between GPS altitude and baro altitude. 0 = 100% gps, 1 = 100% baro. It is highly recommend that you not change this from the default of 1, as GPS altitude is notoriously unreliable. The only time I would recommend changing this is if you have a high altitude enabled GPS, and you are dropping a plane from a high altitude baloon many kilometers off the ground.
@ -772,21 +757,17 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID), GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
GGROUP(pidWheelSteer, "WHEELSTEER_",PID), GGROUP(pidWheelSteer, "WHEELSTEER_",PID),
GGROUP(pidServoRoll, "RLL2SRV_", PID), // @Group: RLL2SRV_
GGROUP(pidServoPitch, "PTCH2SRV_", PID),
GGROUP(pidServoRudder, "YW2SRV_", PID),
// @Group: CTL_RLL_
// @Path: ../libraries/APM_Control/AP_RollController.cpp // @Path: ../libraries/APM_Control/AP_RollController.cpp
GGROUP(rollController, "CTL_RLL_", AP_RollController), GGROUP(rollController, "RLL2SRV_", AP_RollController),
// @Group: CTL_PTCH_ // @Group: PTCH2SRV_
// @Path: ../libraries/APM_Control/AP_PitchController.cpp // @Path: ../libraries/APM_Control/AP_PitchController.cpp
GGROUP(pitchController, "CTL_PTCH_", AP_PitchController), GGROUP(pitchController, "PTCH2SRV_", AP_PitchController),
// @Group: CTL_YAW_ // @Group: YAW2SRV_
// @Path: ../libraries/APM_Control/AP_YawController.cpp // @Path: ../libraries/APM_Control/AP_YawController.cpp
GGROUP(yawController, "CTL_YAW_", AP_YawController), GGROUP(yawController, "YAW2SRV_", AP_YawController),
// variables not in the g class which contain EEPROM saved variables // variables not in the g class which contain EEPROM saved variables
@ -851,9 +832,13 @@ const AP_Param::Info var_info[] PROGMEM = {
old object. This should be zero for top level parameters. old object. This should be zero for top level parameters.
*/ */
const AP_Param::ConversionInfo conversion_table[] PROGMEM = { const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
{ Parameters::k_param_pidServoRoll, 0, AP_PARAM_FLOAT, "CTL_RLL_K_P" }, { Parameters::k_param_pidServoRoll, 0, AP_PARAM_FLOAT, "RLL2SRV_P" },
{ Parameters::k_param_pidServoRoll, 1, AP_PARAM_FLOAT, "CTL_RLL_K_I" }, { Parameters::k_param_pidServoRoll, 1, AP_PARAM_FLOAT, "RLL2SRV_I" },
{ Parameters::k_param_pidServoRoll, 2, AP_PARAM_FLOAT, "CTL_RLL_K_D" }, { Parameters::k_param_pidServoRoll, 2, AP_PARAM_FLOAT, "RLL2SRV_D" },
{ Parameters::k_param_pidServoPitch, 0, AP_PARAM_FLOAT, "PTCH2SRV_P" },
{ Parameters::k_param_pidServoPitch, 1, AP_PARAM_FLOAT, "PTCH2SRV_I" },
{ Parameters::k_param_pidServoPitch, 2, AP_PARAM_FLOAT, "PTCH2SRV_D" },
}; };
static void load_parameters(void) static void load_parameters(void)