diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index f6054acafa..fa6768e3bb 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -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 } diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile index 7c981d83dd..654fe55964 100644 --- a/ArduPlane/Makefile +++ b/ArduPlane/Makefile @@ -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" diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 92620a7514..6cc4b57d04 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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[]; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 18efa11a7c..6e0907296d 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -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 diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index f3cba9ac40..258ec3e47b 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -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); diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index dfafe5fa04..0d0bb44630 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -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;