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 ba41612e7d
commit 703ca469fe
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; if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
} }
// For Testing Only #if APM_CONTROL == DISABLED
// roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; // Calculate dersired servo output for the roll
// Serial.printf_P(PSTR(" roll_sensor ")); // ---------------------------------------------
// Serial.print(roll_sensor,DEC); g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
int32_t tempcalc = nav_pitch_cd +
// Calculate dersired servo output for the roll fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
// --------------------------------------------- (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll_cd - ahrs.roll_sensor), speed_scaler); (ahrs.pitch_sensor - g.pitch_trim_cd);
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) { if (inverted_flight) {
// when flying upside down the elevator control is inverted // when flying upside down the elevator control is inverted
tempcalc = -tempcalc; 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 // Mix Stick input to allow users to override control surfaces
// ----------------------------------------------------------- // -----------------------------------------------------------
@ -101,23 +102,23 @@ static void stabilize()
(g.stick_mixing && (g.stick_mixing &&
geofence_stickmixing() && geofence_stickmixing() &&
failsafe == FAILSAFE_NONE)) { failsafe == FAILSAFE_NONE)) {
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf); ch4_inf = fabs(ch4_inf);
ch4_inf = min(ch4_inf, 400.0); ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0); ch4_inf = ((400.0 - ch4_inf) /400.0);
} }
// Apply output to Rudder // 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 *= ch4_inf;
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle(); g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle();
// Call slew rate limiter if used // Call slew rate limiter if used
// ------------------------------ // ------------------------------
//#if(ROLL_SLEW_LIMIT != 0) //#if(ROLL_SLEW_LIMIT != 0)
// g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out); // g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out);
//#endif //#endif
} }
static void crash_checker() 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 // 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 // always do rudder mixing from roll
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out; 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); 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: obc-sitl:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DOBC_FAILSAFE=ENABLED" 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 // other objects
k_param_sitl = 230, k_param_sitl = 230,
k_param_obc, k_param_obc,
k_param_rollController,
k_param_pitchController,
k_param_yawController,
// //
// 240: PID Controllers // 240: PID Controllers
@ -343,14 +346,21 @@ public:
// PID controllers // PID controllers
// //
PID pidNavRoll; #if APM_CONTROL == DISABLED
PID pidServoRoll; PID pidServoRoll;
PID pidServoPitch; PID pidServoPitch;
PID pidNavPitchAirspeed; PID pidServoRudder;
PID pidServoRudder; #else
PID pidTeThrottle; AP_RollController rollController;
PID pidNavPitchAltitude; AP_PitchController pitchController;
PID pidWheelSteer; AP_YawController yawController;
#endif
PID pidNavRoll;
PID pidNavPitchAirspeed;
PID pidTeThrottle;
PID pidNavPitchAltitude;
PID pidWheelSteer;
Parameters() : Parameters() :
// variable default // variable default
@ -371,16 +381,20 @@ public:
// PID controller initial P initial I initial D initial imax // 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), 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), 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), 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), 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),
pidWheelSteer (0, 0, 0, 0) pidWheelSteer (0, 0, 0, 0)
{
} {}
}; };
extern const AP_Param::Info var_info[]; 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), GGROUP(rc_11, "RC11_", RC_Channel_aux),
#endif #endif
GGROUP(pidNavRoll, "HDNG2RLL_", PID), GGROUP(pidNavRoll, "HDNG2RLL_", PID),
GGROUP(pidServoRoll, "RLL2SRV_", PID), GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
GGROUP(pidServoPitch, "PTCH2SRV_", PID), GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID), GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
GGROUP(pidServoRudder, "YW2SRV_", PID), GGROUP(pidWheelSteer, "WHEELSTEER_",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_ // @Group: COMPASS_
// @Path: ../libraries/AP_Compass/Compass.cpp // @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")); Serial.printf_P(PSTR("Gains\n"));
print_divider(); print_divider();
Serial.printf_P(PSTR("servo roll:\n")); #if APM_CONTROL == DISABLED
print_PID(&g.pidServoRoll); Serial.printf_P(PSTR("servo roll:\n"));
print_PID(&g.pidServoRoll);
Serial.printf_P(PSTR("servo pitch:\n")); Serial.printf_P(PSTR("servo pitch:\n"));
print_PID(&g.pidServoPitch); print_PID(&g.pidServoPitch);
Serial.printf_P(PSTR("servo rudder:\n")); Serial.printf_P(PSTR("servo rudder:\n"));
print_PID(&g.pidServoRudder); print_PID(&g.pidServoRudder);
#endif
Serial.printf_P(PSTR("nav roll:\n")); Serial.printf_P(PSTR("nav roll:\n"));
print_PID(&g.pidNavRoll); print_PID(&g.pidNavRoll);

11
ArduPlane/system.pde
View File

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