Plane: update for new APM_Control API

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

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;

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)

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
 

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()