APM: added RDRSTEER_ PID for steering on ground

this allows for rolling takeoff with steering, and use of rudder in
landing

ArduPlane/Attitude.pde

@@ -172,16 +172,19 @@ static void calc_throttle()
 // ----------------------------------------------------------------------------------------
 static void calc_nav_yaw(float speed_scaler)
 {
-#if HIL_MODE != HIL_MODE_ATTITUDE
+    // always do rudder mixing from roll
-	Vector3f temp = imu.get_accel();
+    g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
-	long error = (long)(-temp.y*100.0);
 
-	// Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero)
+    if (hold_course != -1) {
-	g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, speed_scaler);
+        // steering on or close to ground
-#else
+        g.channel_rudder.servo_out += g.pidRdrSteer.get_pid(bearing_error_cd);  
-	g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
+    } else {
-	// XXX probably need something here based on heading
+        // a PID to coordinate the turn (drive y axis accel to zero)
-#endif
+        Vector3f temp = imu.get_accel();
+        int32_t error = -temp.y*100.0;
+
+        g.channel_rudder.servo_out += g.pidServoRudder.get_pid(error, speed_scaler);
+    }
 }
 
 

ArduPlane/Parameters.h

@@ -198,6 +198,7 @@ public:
         k_param_pidServoRudder,
         k_param_pidTeThrottle,
         k_param_pidNavPitchAltitude,
+        k_param_pidRdrSteer,
 
         // 254,255: reserved
     };
@@ -343,6 +344,7 @@ public:
     PID         pidServoRudder;
     PID         pidTeThrottle;
     PID         pidNavPitchAltitude;
+    PID         pidRdrSteer;
 
     Parameters() :
 	// variable				default
@@ -369,7 +371,8 @@ public:
         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),
         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)
+        pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM),
+        pidRdrSteer         (0, 0, 0, 0)
         {}
 };
 

ArduPlane/Parameters.pde

@@ -515,6 +515,7 @@ const AP_Param::Info var_info[] PROGMEM = {
 	GGROUP(pidServoRudder,          "YW2SRV_",    PID),
 	GGROUP(pidTeThrottle,           "ENRGY2THR_", PID),
 	GGROUP(pidNavPitchAltitude,     "ALT2PTCH_",  PID),
+	GGROUP(pidRdrSteer,             "RDRSTEER_",  PID),
 
 	// variables not in the g class which contain EEPROM saved variables
 

ArduPlane/commands_logic.pde

@@ -283,14 +283,11 @@ static void do_loiter_time()
 /********************************************************************************/
 static bool verify_takeoff()
 {
-	if (g_gps->ground_speed > 300){
+	if (ahrs.yaw_initialised()) {
 		if (hold_course == -1) {
 			// save our current course to take off
-			if(g.compass_enabled) {
+            hold_course = ahrs.yaw_sensor;
-				hold_course = ahrs.yaw_sensor;
+            gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course);
-			} else {
-				hold_course = g_gps->ground_course;
-			}
 		}
 	}
 
@@ -335,10 +332,11 @@ static bool verify_land()
 			// sudden large roll correction which is very nasty at
 			// this point in the landing.
 			hold_course = ahrs.yaw_sensor;
+            gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course);
 		}
 	}
 
-	if (hold_course != -1){
+	if (hold_course != -1) {
 		// recalc bearing error with hold_course;
 		nav_bearing_cd = hold_course;
 		// recalc bearing error

ArduPlane/navigation.pde

@@ -105,24 +105,7 @@ static void calc_gndspeed_undershoot()
 
 static void calc_bearing_error()
 {
-	if(takeoff_complete == true  || g.compass_enabled == true) {
+    bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor;
-        /*
-          most of the time we use the yaw sensor for heading, even if
-          we don't have a compass. The yaw sensor is drift corrected
-          in the DCM library. We only use the gps ground course
-          directly if we haven't completed takeoff, as the yaw drift
-          correction won't have had a chance to kick in. Drift
-          correction using the GPS typically takes 10 seconds or so
-          for a 180 degree correction.
-         */
-		bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor;
-	} else {
-
-		// TODO: we need to use the Yaw gyro for in between GPS reads,
-		// maybe as an offset from a saved gryo value.
-		bearing_error_cd = nav_bearing_cd - g_gps->ground_course;
-	}
-
 	bearing_error_cd = wrap_180_cd(bearing_error_cd);
 }