aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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

64
ArduPlane/Attitude.pde
View File

@ -43,23 +43,24 @@ 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);
// Calculate dersired servo output for the roll
// ---------------------------------------------
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
int32_t tempcalc = nav_pitch_cd +
fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
(ahrs.pitch_sensor - g.pitch_trim_cd);
#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);
int32_t tempcalc = nav_pitch_cd +
fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
(ahrs.pitch_sensor - g.pitch_trim_cd);
if (inverted_flight) {
// when flying upside down the elevator control is inverted
tempcalc = -tempcalc;
}
g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, speed_scaler);
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
// -----------------------------------------------------------
@ -101,23 +102,23 @@ static void stabilize()
(g.stick_mixing &&
geofence_stickmixing() &&
failsafe == FAILSAFE_NONE)) {
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf);
ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0);
}
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf);
ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0);
}
// Apply output to Rudder
// ----------------------
calc_nav_yaw(speed_scaler);
g.channel_rudder.servo_out *= ch4_inf;
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle();
// Apply output to Rudder
// ----------------------
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();
// Call slew rate limiter if used
// ------------------------------
//#if(ROLL_SLEW_LIMIT != 0)
// g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out);
//#endif
// Call slew rate limiter if used
// ------------------------------
//#if(ROLL_SLEW_LIMIT != 0)
// g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out);
//#endif
}
static void crash_checker()
@ -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"

38
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,14 +346,21 @@ public:
// PID controllers
//
PID pidNavRoll;
PID pidServoRoll;
PID pidServoPitch;
PID pidNavPitchAirspeed;
PID pidServoRudder;
PID pidTeThrottle;
PID pidNavPitchAltitude;
PID pidWheelSteer;
#if APM_CONTROL == DISABLED
PID pidServoRoll;
PID pidServoPitch;
PID pidServoRudder;
#else
AP_RollController rollController;
AP_PitchController pitchController;
AP_YawController yawController;
#endif
PID pidNavRoll;
PID pidNavPitchAirspeed;
PID pidTeThrottle;
PID pidNavPitchAltitude;
PID pidWheelSteer;
Parameters() :
// variable default
@ -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[];

25
ArduPlane/Parameters.pde
View File

@ -531,16 +531,23 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(rc_11, "RC11_", RC_Channel_aux),
#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),
GGROUP(pidNavRoll, "HDNG2RLL_", PID),
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
GGROUP(pidWheelSteer, "WHEELSTEER_",PID),
// variables not in the g class which contain EEPROM saved variables
#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_
// @Path: ../libraries/AP_Compass/Compass.cpp

10
ArduPlane/setup.pde
View File

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

11
ArduPlane/system.pde
View File

@ -182,8 +182,15 @@ static void init_ardupilot()
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
// Do GPS init
g_gps = &g_gps_driver;
#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
g_gps->init(GPS::GPS_ENGINE_AIRBORNE_4G);
g_gps->callback = mavlink_delay;