diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 5bce920a5e..1eb285d43a 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -54,11 +54,6 @@ #include #include -// optional new controller library -#if APM_CONTROL == ENABLED -#include -#endif - #include #include @@ -1053,17 +1048,17 @@ static void update_current_flight_mode(void) nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd); } -#if APM_CONTROL == DISABLED - float aspeed; - if (ahrs.airspeed_estimate(&aspeed)) { - // don't use a pitch/roll integrators during takeoff if we are - // below minimum speed - if (aspeed < g.flybywire_airspeed_min) { - g.pidServoPitch.reset_I(); - g.pidServoRoll.reset_I(); + if (g.att_controller == ATT_CONTROL_PID) { + float aspeed; + if (ahrs.airspeed_estimate(&aspeed)) { + // don't use a pitch/roll integrators during takeoff if we are + // below minimum speed + if (aspeed < g.flybywire_airspeed_min) { + g.pidServoPitch.reset_I(); + g.pidServoRoll.reset_I(); + } } } -#endif // max throttle for takeoff g.channel_throttle.servo_out = g.throttle_max; diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index f87df84e24..8ba9bdb431 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -77,14 +77,18 @@ static void stabilize_roll(float speed_scaler) if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000; } -#if APM_CONTROL == DISABLED - // 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); -#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.flybywire_airspeed_min); -#endif + switch (g.att_controller) { + 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; + } } /* @@ -94,19 +98,25 @@ static void stabilize_roll(float speed_scaler) */ static void stabilize_pitch(float speed_scaler) { -#if APM_CONTROL == DISABLED - 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; + switch (g.att_controller) { + case ATT_CONTROL_APMCONTROL: + 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); + break; + + 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; } -#if APM_CONTROL == DISABLED - // always do rudder mixing from roll - g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out; + switch (g.att_controller) { + case ATT_CONTROL_APMCONTROL: + 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) - Vector3f temp = ins.get_accel(); - int32_t error = -temp.y*100.0; + 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; + } - g.channel_rudder.servo_out += g.pidServoRudder.get_pid_4500(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, control_mode == STABILIZE, g.flybywire_airspeed_min, g.flybywire_airspeed_max) + g.channel_roll.servo_out * g.kff_rudder_mix; -#endif + // add in rudder mixing from roll + g.channel_rudder.servo_out += g.channel_roll.servo_out * g.kff_rudder_mix; } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index a20e7103e1..978c4f26e1 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -86,6 +86,7 @@ public: k_param_vtail_output, k_param_nav_controller, k_param_elevon_output, + k_param_att_controller, // 110: Telemetry control // @@ -263,6 +264,9 @@ public: // navigation controller type. See AP_Navigation::ControllerType AP_Int8 nav_controller; + // attitude controller type. + AP_Int8 att_controller; + // Estimation // AP_Float altitude_mix; @@ -389,15 +393,13 @@ public: // PID controllers // -#if APM_CONTROL == DISABLED PID pidServoRoll; PID pidServoPitch; PID pidServoRudder; -#else + AP_RollController rollController; AP_PitchController pitchController; AP_YawController yawController; -#endif PID pidNavPitchAirspeed; PID pidTeThrottle; @@ -423,15 +425,12 @@ public: #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 rc_12 (CH_12), #endif - // 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), 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), -#endif 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), diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index d06a4b3cad..22c38cc77a 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -138,6 +138,13 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard 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 // @DisplayName: Gps to Baro Mix // @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(pidWheelSteer, "WHEELSTEER_",PID), -#if APM_CONTROL == DISABLED GGROUP(pidServoRoll, "RLL2SRV_", PID), GGROUP(pidServoPitch, "PTCH2SRV_", PID), GGROUP(pidServoRudder, "YW2SRV_", PID), -#else + GGROUP(rollController, "CTL_RLL_", AP_RollController), GGROUP(pitchController, "CTL_PTCH_", AP_PitchController), GGROUP(yawController, "CTL_YAW_", AP_YawController), -#endif // variables not in the g class which contain EEPROM saved variables diff --git a/ArduPlane/config.h b/ArduPlane/config.h index e1429e5e13..700dd7d0d9 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -769,11 +769,6 @@ # define OBC_FAILSAFE DISABLED #endif -// new APM_Control controller library by Jon Challinger -#ifndef APM_CONTROL -# define APM_CONTROL DISABLED -#endif - #ifndef SERIAL_BUFSIZE # define SERIAL_BUFSIZE 256 #endif diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index dccc8c5d53..975b2c1194 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -267,5 +267,10 @@ enum { ALT_CONTROL_NON_AIRSPEED=1 }; +// attitude controller choice +enum { + ATT_CONTROL_PID = 0, + ATT_CONTROL_APMCONTROL = 1 +}; #endif // _DEFINES_H diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index a16a71730e..f4c604887d 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -171,12 +171,10 @@ 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;