From accfd46633f67bb1f9dd81d6c2960d85e93f796b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 Aug 2013 14:57:41 +1000 Subject: [PATCH] Plane: update for new APM_Control API --- ArduPlane/ArduPlane.pde | 6 ++++++ ArduPlane/Attitude.pde | 36 ++++++++++++++++++------------------ ArduPlane/Parameters.h | 11 ----------- ArduPlane/Parameters.pde | 6 +++--- ArduPlane/system.pde | 11 +++-------- 5 files changed, 30 insertions(+), 40 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 6451a4d12d..5cd4b8a1e2 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -249,6 +249,12 @@ AP_AHRS_DCM ahrs(&ins, g_gps); static AP_L1_Control L1_controller(ahrs); static AP_TECS TECS_controller(&ahrs, aparm); +// Attitude to servo controllers +static AP_RollController rollController(ahrs, aparm); +static AP_PitchController pitchController(ahrs, aparm); +static AP_YawController yawController(ahrs, aparm); + + #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL SITL sitl; #endif diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 4d4c4359c5..ac7b1480c9 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -77,9 +77,9 @@ static void stabilize_roll(float speed_scaler) if (control_mode == STABILIZE && channel_roll->control_in != 0) { disable_integrator = true; } - channel_roll->servo_out = g.rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, - speed_scaler, - disable_integrator); + channel_roll->servo_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, + speed_scaler, + disable_integrator); } /* @@ -94,9 +94,9 @@ static void stabilize_pitch(float speed_scaler) if (control_mode == STABILIZE && channel_pitch->control_in != 0) { disable_integrator = true; } - channel_pitch->servo_out = g.pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, - speed_scaler, - disable_integrator); + channel_pitch->servo_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, + speed_scaler, + disable_integrator); } /* @@ -265,16 +265,16 @@ static void stabilize_acro(float speed_scaler) nav_roll_cd = ahrs.roll_sensor + roll_error_cd; // try to reduce the integrated angular error to zero. We set // 'stabilze' to true, which disables the roll integrator - channel_roll->servo_out = g.rollController.get_servo_out(roll_error_cd, - speed_scaler, - true); + channel_roll->servo_out = rollController.get_servo_out(roll_error_cd, + speed_scaler, + true); } else { /* aileron stick is non-zero, use pure rate control until the user releases the stick */ acro_state.locked_roll = false; - channel_roll->servo_out = g.rollController.get_rate_out(roll_rate, speed_scaler); + channel_roll->servo_out = rollController.get_rate_out(roll_rate, speed_scaler); } if (pitch_rate == 0) { @@ -289,15 +289,15 @@ static void stabilize_acro(float speed_scaler) // try to hold the locked pitch. Note that we have the pitch // integrator enabled, which helps with inverted flight nav_pitch_cd = acro_state.locked_pitch_cd; - channel_pitch->servo_out = g.pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, - speed_scaler, - false); + channel_pitch->servo_out = pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, + speed_scaler, + false); } else { /* user has non-zero pitch input, use a pure rate controller */ acro_state.locked_pitch = false; - channel_pitch->servo_out = g.pitchController.get_rate_out(pitch_rate, speed_scaler); + channel_pitch->servo_out = pitchController.get_rate_out(pitch_rate, speed_scaler); } /* @@ -345,9 +345,9 @@ static void stabilize() // we are low, with no climb rate, and zero throttle, and very // low ground speed. Zero the attitude controller // integrators. This prevents integrator buildup pre-takeoff. - g.rollController.reset_I(); - g.pitchController.reset_I(); - g.yawController.reset_I(); + rollController.reset_I(); + pitchController.reset_I(); + yawController.reset_I(); } } @@ -386,7 +386,7 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf) if (control_mode == STABILIZE && channel_rudder->control_in != 0) { disable_integrator = true; } - channel_rudder->servo_out = g.yawController.get_servo_out(speed_scaler, disable_integrator); + channel_rudder->servo_out = yawController.get_servo_out(speed_scaler, disable_integrator); // add in rudder mixing from roll channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 3aef41ab4e..a0518cedd9 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -408,12 +408,6 @@ public: RC_Channel_aux rc_12; #endif - // Attitude to servo controllers - // - AP_RollController rollController; - AP_PitchController pitchController; - AP_YawController yawController; - // PID controllers PID pidWheelSteer; @@ -439,11 +433,6 @@ public: rc_12 (CH_12), #endif - // pass the aircraft parameters structure into the attitude controllers - rollController(aparm), - pitchController(aparm), - yawController(aparm), - // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------- pidWheelSteer (0, 0, 0, 0) diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index d4fc9df313..c6b8b56753 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -808,15 +808,15 @@ const AP_Param::Info var_info[] PROGMEM = { // @Group: RLL2SRV_ // @Path: ../libraries/APM_Control/AP_RollController.cpp - GGROUP(rollController, "RLL2SRV_", AP_RollController), + GOBJECT(rollController, "RLL2SRV_", AP_RollController), // @Group: PTCH2SRV_ // @Path: ../libraries/APM_Control/AP_PitchController.cpp - GGROUP(pitchController, "PTCH2SRV_", AP_PitchController), + GOBJECT(pitchController, "PTCH2SRV_", AP_PitchController), // @Group: YAW2SRV_ // @Path: ../libraries/APM_Control/AP_YawController.cpp - GGROUP(yawController, "YAW2SRV_", AP_YawController), + GOBJECT(yawController, "YAW2SRV_", AP_YawController), // variables not in the g class which contain EEPROM saved variables diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 10ed6c0d4c..287fc00e07 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -177,11 +177,6 @@ static void init_ardupilot() // give AHRS the airspeed sensor ahrs.set_airspeed(&airspeed); - // the axis controllers need access to the AHRS system - g.rollController.set_ahrs(&ahrs); - g.pitchController.set_ahrs(&ahrs); - g.yawController.set_ahrs(&ahrs); - // Do GPS init g_gps = &g_gps_driver; // GPS Initialization @@ -371,9 +366,9 @@ static void set_mode(enum FlightMode mode) Log_Write_Mode(control_mode); // reset attitude integrators on mode change - g.rollController.reset_I(); - g.pitchController.reset_I(); - g.yawController.reset_I(); + rollController.reset_I(); + pitchController.reset_I(); + yawController.reset_I(); } static void check_long_failsafe()