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

APM: added optional use of new APM_Control library

Browse Source 
thanks to Jon Challinger for the new controllers!
This commit is contained in:
Andrew Tridgell 2012-08-22 12:34:40 +10:00
parent 8b53f1272a
commit d445482ace
6 changed files with 95 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/Attitude.pde
View File

@ -43,11 +43,7 @@ static void stabilize()
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
}
// For Testing Only
// roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10;
// Serial.printf_P(PSTR(" roll_sensor "));
// Serial.print(roll_sensor,DEC);
#if APM_CONTROL == DISABLED
// Calculate dersired servo output for the roll
// ---------------------------------------------
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
@ -60,6 +56,11 @@ static void stabilize()
tempcalc = -tempcalc;
}
g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, speed_scaler);
#else // APM_CONTROL == ENABLED
// 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.channel_pitch.servo_out = g.pitchController.get_servo_out(nav_pitch_cd, speed_scaler, control_mode == STABILIZE);
#endif
// Mix Stick input to allow users to override control surfaces
// -----------------------------------------------------------
@ -109,7 +110,7 @@ static void stabilize()
// Apply output to Rudder
// ----------------------
calc_nav_yaw(speed_scaler);
calc_nav_yaw(speed_scaler, ch4_inf);
g.channel_rudder.servo_out *= ch4_inf;
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle();
@ -170,8 +171,9 @@ static void calc_throttle()
// Yaw is separated into a function for future implementation of heading hold on rolling take-off
// ----------------------------------------------------------------------------------------
static void calc_nav_yaw(float speed_scaler)
static void calc_nav_yaw(float speed_scaler, float ch4_inf)
{
#if APM_CONTROL == DISABLED
// always do rudder mixing from roll
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
@ -185,6 +187,10 @@ static void calc_nav_yaw(float speed_scaler)
g.channel_rudder.servo_out += g.pidServoRudder.get_pid(error, speed_scaler);
}
#else // APM_CONTROL == ENABLED
// use the new APM_Control library
g.channel_rudder.servo_out = g.yawController.get_servo_out(speed_scaler, ch4_inf < 0.25) + g.channel_roll.servo_out * g.kff_rudder_mix;
#endif
}

3
ArduPlane/Makefile
View File

@ -48,3 +48,6 @@ obc:
obc-sitl:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DOBC_FAILSAFE=ENABLED"
sitl-newcontrollers:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DAPM_CONTROL=ENABLED"

26
ArduPlane/Parameters.h
View File

@ -191,6 +191,9 @@ public:
// other objects
k_param_sitl = 230,
k_param_obc,
k_param_rollController,
k_param_pitchController,
k_param_yawController,
//
// 240: PID Controllers
@ -343,11 +346,18 @@ public:
// PID controllers
//
PID pidNavRoll;
#if APM_CONTROL == DISABLED
PID pidServoRoll;
PID pidServoPitch;
PID pidNavPitchAirspeed;
PID pidServoRudder;
#else
AP_RollController rollController;
AP_PitchController pitchController;
AP_YawController yawController;
#endif
PID pidNavRoll;
PID pidNavPitchAirspeed;
PID pidTeThrottle;
PID pidNavPitchAltitude;
PID pidWheelSteer;
@ -371,16 +381,20 @@ public:
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------
pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
#if APM_CONTROL == DISABLED
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),
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
#endif
pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
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)
{
}
{}
};
extern const AP_Param::Info var_info[];

13
ArduPlane/Parameters.pde
View File

@ -532,14 +532,21 @@ const AP_Param::Info var_info[] PROGMEM = {
#endif
GGROUP(pidNavRoll, "HDNG2RLL_", PID),
GGROUP(pidServoRoll, "RLL2SRV_", PID),
GGROUP(pidServoPitch, "PTCH2SRV_", PID),
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
GGROUP(pidServoRudder, "YW2SRV_", PID),
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
GGROUP(pidWheelSteer, "WHEELSTEER_",PID),
#if APM_CONTROL == DISABLED
GGROUP(pidServoRoll, "RLL2SRV_", PID),
GGROUP(pidServoPitch, "PTCH2SRV_", PID),
GGROUP(pidServoRudder, "YW2SRV_", PID),
#else
GGROUP(rollController, "RLL_", AP_RollController),
GGROUP(pitchController, "PTCH_", AP_PitchController),
GGROUP(yawController, "YWCTL_", AP_YawController),
#endif
// variables not in the g class which contain EEPROM saved variables
// @Group: COMPASS_

2
ArduPlane/setup.pde
View File

@ -369,6 +369,7 @@ static void report_gains()
Serial.printf_P(PSTR("Gains\n"));
print_divider();
#if APM_CONTROL == DISABLED
Serial.printf_P(PSTR("servo roll:\n"));
print_PID(&g.pidServoRoll);
@ -377,6 +378,7 @@ static void report_gains()
Serial.printf_P(PSTR("servo rudder:\n"));
print_PID(&g.pidServoRudder);
#endif
Serial.printf_P(PSTR("nav roll:\n"));
print_PID(&g.pidNavRoll);

7
ArduPlane/system.pde
View File

@ -182,6 +182,13 @@ static void init_ardupilot()
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
#if APM_CONTROL == ENABLED
// the axis controllers need access to the AHRS system
g.rollController.set_ahrs(&ahrs);
g.pitchController.set_ahrs(&ahrs);
g.yawController.set_ahrs(&ahrs);
#endif
// Do GPS init
g_gps = &g_gps_driver;
// GPS Initialization