Sub: Replace sport mode with transect mode

ArduSub/GCS_Mavlink.cpp

@@ -323,7 +323,7 @@ void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
         wp_distance / 1.0e2f,
         pos_control.get_alt_error() / 1.0e2f,
         0,
-        0);
+        gps.crosstrack_error());
 }
 
 // report simulator state

ArduSub/Parameters.cpp

@@ -775,6 +775,10 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GGROUP(p_pos_xy,                "POS_XY_", AC_P),
 
+	GGROUP(pid_crosstrack_control, "XTRACK_", AC_PID),
+
+	GGROUP(pid_heading_control, "HEAD_", AC_PID),
+
     // variables not in the g class which contain EEPROM saved variables
 
 #if CAMERA == ENABLED

ArduSub/Parameters.h

@@ -387,6 +387,9 @@ public:
 		k_param_jbtn_14,
 		k_param_jbtn_15, // 276
 
+		k_param_pid_crosstrack_control,
+		k_param_pid_heading_control,
+		
 		k_param_water_detector, // water detector object
 		k_param_failsafe_leak, // leak failsafe behavior
     };
@@ -535,6 +538,13 @@ public:
     AC_P                    p_pos_xy;
     AC_P                    p_alt_hold;
 
+
+
+
+	AC_PID pid_crosstrack_control;
+	AC_PID pid_heading_control;
+
+
     // Autotune
     AP_Int8                 autotune_axis_bitmask;
     AP_Float                autotune_aggressiveness;
@@ -572,7 +582,12 @@ public:
         //----------------------------------------------------------------------
         p_pos_xy                (POS_XY_P),
 
-        p_alt_hold              (ALT_HOLD_P)
+        p_alt_hold              (ALT_HOLD_P),
+
+		pid_crosstrack_control  (XTRACK_P,        XTRACK_I,         XTRACK_D,       XTRACK_IMAX,        XTRACK_FILT_HZ,      XTRACK_DT),
+
+		pid_heading_control     (HEAD_P,		  HEAD_I,           HEAD_D,         HEAD_IMAX,          HEAD_FILT_HZ,       HEAD_DT)
+
     {
     }
 };

ArduSub/control_stabilize.cpp

@@ -57,7 +57,7 @@ void Sub::stabilize_run()
     // call attitude controller
 	// update attitude controller targets
 
-	if (target_yaw_rate != 0) { // call attitude controller with rate yaw determined by pilot input
+	if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
 		attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
 		last_pilot_heading = ahrs.yaw_sensor;
 		last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading