aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: added ATT_CONTROLLER option to select attitude controller

Browse Source 
set to 1 for the new APM_Control library
This commit is contained in:
Andrew Tridgell 2013-05-05 15:31:41 +10:00
parent 55f7d18979
commit 4e2b88f6e6
7 changed files with 72 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduPlane/ArduPlane.pde
View File

@ -54,11 +54,6 @@
#include <DataFlash.h> #include <DataFlash.h>
#include <SITL.h> #include <SITL.h>
// optional new controller library
#if APM_CONTROL == ENABLED
#include <APM_Control.h>
#endif
#include <AP_Navigation.h> #include <AP_Navigation.h>
#include <AP_L1_Control.h> #include <AP_L1_Control.h>
@ -1053,17 +1048,17 @@ static void update_current_flight_mode(void)
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd); nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd);
} }
#if APM_CONTROL == DISABLED if (g.att_controller == ATT_CONTROL_PID) {
float aspeed; float aspeed;
if (ahrs.airspeed_estimate(&aspeed)) { if (ahrs.airspeed_estimate(&aspeed)) {
// 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.pidServoPitch.reset_I();
g.pidServoRoll.reset_I(); g.pidServoRoll.reset_I();
}
} }
} }
#endif
// max throttle for takeoff // max throttle for takeoff
g.channel_throttle.servo_out = g.throttle_max; g.channel_throttle.servo_out = g.throttle_max;

76
ArduPlane/Attitude.pde
View File

@ -77,14 +77,18 @@ 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;
} }
#if APM_CONTROL == DISABLED switch (g.att_controller) {
// Calculate dersired servo output for the roll case ATT_CONTROL_APMCONTROL:
// --------------------------------------------- // calculate roll and pitch control using new APM_Control library
g.channel_roll.servo_out = g.pidServoRoll.get_pid_4500((nav_roll_cd - ahrs.roll_sensor), speed_scaler); g.channel_roll.servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min);
#else // APM_CONTROL == ENABLED break;
// 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); default:
#endif // 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;
}
} }
/* /*
@ -94,19 +98,25 @@ static void stabilize_roll(float speed_scaler)
*/ */
static void stabilize_pitch(float speed_scaler) static void stabilize_pitch(float speed_scaler)
{ {
#if APM_CONTROL == DISABLED switch (g.att_controller) {
int32_t tempcalc = nav_pitch_cd + case ATT_CONTROL_APMCONTROL:
fabsf(ahrs.roll_sensor * g.kff_pitch_compensation) + g.channel_pitch.servo_out = g.pitchController.get_servo_out(nav_pitch_cd, speed_scaler,
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - control_mode == STABILIZE,
(ahrs.pitch_sensor - g.pitch_trim_cd); g.flybywire_airspeed_min, g.flybywire_airspeed_max);
if (inverted_flight) { break;
// when flying upside down the elevator control is inverted
tempcalc = -tempcalc; default:
int32_t tempcalc = nav_pitch_cd +
fabsf(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_4500(tempcalc, speed_scaler);
break;
} }
g.channel_pitch.servo_out = g.pidServoPitch.get_pid_4500(tempcalc, speed_scaler);
#else // APM_CONTROL == ENABLED
g.channel_pitch.servo_out = g.pitchController.get_servo_out(nav_pitch_cd, speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min, g.flybywire_airspeed_max);
#endif
} }
/* /*
@ -334,19 +344,23 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf)
return; return;
} }
#if APM_CONTROL == DISABLED switch (g.att_controller) {
// always do rudder mixing from roll case ATT_CONTROL_APMCONTROL:
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out; g.channel_rudder.servo_out = g.yawController.get_servo_out(speed_scaler,
control_mode == STABILIZE,
g.flybywire_airspeed_min, g.flybywire_airspeed_max);
break;
// a PID to coordinate the turn (drive y axis accel to zero) default:
Vector3f temp = ins.get_accel(); // a PID to coordinate the turn (drive y axis accel to zero)
int32_t error = -temp.y*100.0; 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;
}
g.channel_rudder.servo_out += g.pidServoRudder.get_pid_4500(error, speed_scaler); // add in rudder mixing from roll
#else // APM_CONTROL == ENABLED g.channel_rudder.servo_out += g.channel_roll.servo_out * g.kff_rudder_mix;
// use the new APM_Control library
g.channel_rudder.servo_out = g.yawController.get_servo_out(speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min, g.flybywire_airspeed_max) + g.channel_roll.servo_out * g.kff_rudder_mix;
#endif
} }

11
ArduPlane/Parameters.h
View File

@ -86,6 +86,7 @@ public:
k_param_vtail_output, k_param_vtail_output,
k_param_nav_controller, k_param_nav_controller,
k_param_elevon_output, k_param_elevon_output,
k_param_att_controller,
// 110: Telemetry control // 110: Telemetry control
// //
@ -263,6 +264,9 @@ 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;
@ -389,15 +393,13 @@ public:
// PID controllers // PID controllers
// //
#if APM_CONTROL == DISABLED
PID pidServoRoll; PID pidServoRoll;
PID pidServoPitch; PID pidServoPitch;
PID pidServoRudder; PID pidServoRudder;
#else
AP_RollController rollController; AP_RollController rollController;
AP_PitchController pitchController; AP_PitchController pitchController;
AP_YawController yawController; AP_YawController yawController;
#endif
PID pidNavPitchAirspeed; PID pidNavPitchAirspeed;
PID pidTeThrottle; PID pidTeThrottle;
@ -423,15 +425,12 @@ public:
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
rc_12 (CH_12), rc_12 (CH_12),
#endif #endif
// PID controller initial P initial I initial D initial imax // PID controller initial P initial I initial D initial imax
//----------------------------------------------------------------------------------- //-----------------------------------------------------------------------------------
#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),
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
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),

11
ArduPlane/Parameters.pde
View File

@ -138,6 +138,13 @@ 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 // @Description: The percent of mixing between gps altitude and baro altitude. 0 = 100% gps, 1 = 100% baro
@ -680,15 +687,13 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID), GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
GGROUP(pidWheelSteer, "WHEELSTEER_",PID), GGROUP(pidWheelSteer, "WHEELSTEER_",PID),
#if APM_CONTROL == DISABLED
GGROUP(pidServoRoll, "RLL2SRV_", PID), GGROUP(pidServoRoll, "RLL2SRV_", PID),
GGROUP(pidServoPitch, "PTCH2SRV_", PID), GGROUP(pidServoPitch, "PTCH2SRV_", PID),
GGROUP(pidServoRudder, "YW2SRV_", PID), GGROUP(pidServoRudder, "YW2SRV_", PID),
#else
GGROUP(rollController, "CTL_RLL_", AP_RollController), GGROUP(rollController, "CTL_RLL_", AP_RollController),
GGROUP(pitchController, "CTL_PTCH_", AP_PitchController), GGROUP(pitchController, "CTL_PTCH_", AP_PitchController),
GGROUP(yawController, "CTL_YAW_", AP_YawController), GGROUP(yawController, "CTL_YAW_", AP_YawController),
#endif
// variables not in the g class which contain EEPROM saved variables // variables not in the g class which contain EEPROM saved variables

5
ArduPlane/config.h
View File

@ -769,11 +769,6 @@
# define OBC_FAILSAFE DISABLED # define OBC_FAILSAFE DISABLED
#endif #endif
// new APM_Control controller library by Jon Challinger
#ifndef APM_CONTROL
# define APM_CONTROL DISABLED
#endif
#ifndef SERIAL_BUFSIZE #ifndef SERIAL_BUFSIZE
# define SERIAL_BUFSIZE 256 # define SERIAL_BUFSIZE 256
#endif #endif

5
ArduPlane/defines.h
View File

@ -267,5 +267,10 @@ enum {
ALT_CONTROL_NON_AIRSPEED=1 ALT_CONTROL_NON_AIRSPEED=1
}; };
// attitude controller choice
enum {
ATT_CONTROL_PID = 0,
ATT_CONTROL_APMCONTROL = 1
};
#endif // _DEFINES_H #endif // _DEFINES_H

2
ArduPlane/system.pde
View File

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