Rover: added new STEERING mode

this makes it easier to tune for auto mode

APMrover2/APMrover2.pde

@@ -390,8 +390,7 @@ static int16_t throttle_last = 0, throttle = 500;
 ////////////////////////////////////////////////////////////////////////////////
 // Difference between current bearing and desired bearing.  in centi-degrees
 static int32_t bearing_error_cd;
-// Difference between current altitude and desired altitude.  Centimeters
-static int32_t altitude_error;
+
 // Distance perpandicular to the course line that we are off trackline.  Meters 
 static float	crosstrack_error;
 
@@ -422,13 +421,13 @@ static float	current_total1;
 ////////////////////////////////////////////////////////////////////////////////
 // Navigation control variables
 ////////////////////////////////////////////////////////////////////////////////
-// The instantaneous desired bank angle.  Hundredths of a degree
+// The instantaneous desired steering angle.  Hundredths of a degree
 static int32_t nav_steer;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Waypoint distances
 ////////////////////////////////////////////////////////////////////////////////
-// Distance between plane and next waypoint.  Meters
+// Distance between rover and next waypoint.  Meters
 static float wp_distance;
 // Distance between previous and next waypoint.  Meters
 static int32_t wp_totalDistance;
@@ -471,7 +470,7 @@ static struct 	Location home;
 static bool	home_is_set;
 // The location of the previous waypoint.  Used for track following and altitude ramp calculations
 static struct 	Location prev_WP;
-// The plane's current location
+// The rover's current location
 static struct 	Location current_loc;
 // The location of the current/active waypoint.  Used for track following
 static struct 	Location next_WP;
@@ -656,11 +655,6 @@ static void fast_loop()
 	// ---------------------------------------
 	update_current_mode();
 
-	// apply desired steering if in an auto mode
-	if (control_mode >= AUTO) {
-        g.channel_steer.servo_out = nav_steer;
-    }
-
 	// write out the servo PWM values
 	// ------------------------------
 	set_servos();
@@ -847,12 +841,32 @@ static void update_current_mode(void)
     case RTL:
     case GUIDED:
         calc_nav_steer();
-        calc_throttle();
+        calc_throttle(g.speed_cruise);
+        break;
+
+    case STEERING:
+        /*
+          in steering mode we control the bearing error, which gives
+          the same type of steering control as auto mode. The throttle
+          controls the target speed, in proportion to the throttle
+         */
+        bearing_error_cd = g.channel_steer.pwm_to_angle();
+        calc_nav_steer();
+
+        /* we need to reset the I term or it will build up */
+        g.pidNavSteer.reset_I();
+        calc_throttle(g.channel_throttle.pwm_to_angle() * 0.01 * g.speed_cruise);
         break;
 
     case LEARNING:
     case MANUAL:
-        nav_steer        = 0;
+        /*
+          in both MANUAL and LEARNING we pass through the
+          controls. Setting servo_out here actually doesn't matter, as
+          we set the exact value in set_servos(), but it helps for
+          logging
+         */
+        g.channel_throttle.servo_out = g.channel_throttle.radio_in;
         g.channel_steer.servo_out = g.channel_steer.pwm_to_angle();
         break;
 
@@ -866,6 +880,7 @@ static void update_navigation()
     switch (control_mode) {
     case MANUAL:
     case LEARNING:
+    case STEERING:
     case INITIALISING:
         break;
 

APMrover2/GCS_Mavlink.pde

@@ -42,9 +42,8 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
     // ArduPlane documentation
     switch (control_mode) {
     case MANUAL:
-        base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-        break;
     case LEARNING:
+    case STEERING:
         base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
         break;
     case AUTO:
@@ -139,6 +138,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         break;
 
     case LEARNING:
+    case STEERING:
         control_sensors_enabled |= (1<<10); // 3D angular rate control
         control_sensors_enabled |= (1<<11); // attitude stabilisation
         break;
@@ -244,7 +244,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
         bearing,
         target_bearing / 100,
         wp_distance,
-        altitude_error / 1.0e2,
+        0,
         groundspeed_error,
         crosstrack_error);
 }
@@ -1082,6 +1082,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             switch (packet.custom_mode) {
             case MANUAL:
             case LEARNING:
+            case STEERING:
             case AUTO:
             case RTL:
                 set_mode((enum mode)packet.custom_mode);

APMrover2/Log.pde

@@ -420,10 +420,9 @@ static void Log_Read_Control_Tuning()
 struct log_Nav_Tuning {
     LOG_PACKET_HEADER;
     uint16_t yaw;
-    uint32_t wp_distance;
+    float    wp_distance;
     uint16_t target_bearing_cd;
     uint16_t nav_bearing_cd;
-    int16_t altitude_error_cm;
     int16_t nav_gain_scheduler;
 };
 
@@ -433,10 +432,9 @@ static void Log_Write_Nav_Tuning()
     struct log_Nav_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG),
         yaw                 : (uint16_t)ahrs.yaw_sensor,
-        wp_distance         : (uint32_t)wp_distance,
+        wp_distance         : wp_distance,
         target_bearing_cd   : (uint16_t)target_bearing,
         nav_bearing_cd      : (uint16_t)nav_bearing,
-        altitude_error_cm   : (int16_t)altitude_error,
         nav_gain_scheduler  : (int16_t)(nav_gain_scaler*1000)
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
@@ -448,12 +446,11 @@ static void Log_Read_Nav_Tuning()
     struct log_Nav_Tuning pkt;
     DataFlash.ReadPacket(&pkt, sizeof(pkt));
 
-    cliSerial->printf_P(PSTR("NTUN, %4.4f, %lu, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+    cliSerial->printf_P(PSTR("NTUN, %4.4f, %4.2f, %4.4f, %4.4f, %4.4f\n"),
                     (float)pkt.yaw/100.0f,
-                    (unsigned long)pkt.wp_distance,
+                    pkt.wp_distance,
                     (float)(pkt.target_bearing_cd/100.0f),
                     (float)(pkt.nav_bearing_cd/100.0f),
-                    (float)(pkt.altitude_error_cm/100.0f),
                     (float)(pkt.nav_gain_scheduler/100.0f));
 }
 

APMrover2/Parameters.pde

@@ -268,7 +268,7 @@ const AP_Param::Info var_info[] PROGMEM = {
 	// @DisplayName: Sonar trigger angle
 	// @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the sonar. A positive number means to turn right, and a negative angle means to turn left.
 	// @Units: centimeters
-    // @Range: -90 90
+    // @Range: -45 45
     // @Increment: 1
 	// @User: Standard
 	GSCALAR(sonar_turn_angle,   "SONAR_TURN_ANGLE",    45),
@@ -290,7 +290,7 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Param: MODE1
     // @DisplayName: Mode1
-    // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
     // @User: Standard
     // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
 	GSCALAR(mode1,           "MODE1",         MODE_1),
@@ -298,35 +298,35 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: MODE2
     // @DisplayName: Mode2
     // @Description: Driving mode for switch position 2 (1231 to 1360)
-    // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
     // @User: Standard
 	GSCALAR(mode2,           "MODE2",         MODE_2),
 
     // @Param: MODE3
     // @DisplayName: Mode3
     // @Description: Driving mode for switch position 3 (1361 to 1490)
-    // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
     // @User: Standard
 	GSCALAR(mode3,           "MODE3",         MODE_3),
 
     // @Param: MODE4
     // @DisplayName: Mode4
     // @Description: Driving mode for switch position 4 (1491 to 1620)
-    // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
     // @User: Standard
 	GSCALAR(mode4,           "MODE4",         MODE_4),
 
     // @Param: MODE5
     // @DisplayName: Mode5
     // @Description: Driving mode for switch position 5 (1621 to 1749)
-    // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
     // @User: Standard
 	GSCALAR(mode5,           "MODE5",         MODE_5),
 
     // @Param: MODE6
     // @DisplayName: Mode6
     // @Description: Driving mode for switch position 6 (1750 to 2049)
-    // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided
     // @User: Standard
 	GSCALAR(mode6,           "MODE6",         MODE_6),
 

APMrover2/Steering.pde

@@ -17,11 +17,35 @@ static void throttle_slew_limit(int16_t last_throttle)
     }
 }
 
-static void calc_throttle()
+static void calc_throttle(float target_speed)
 {  
+   if (target_speed <= 0) {
+       // cope with zero requested speed
+       g.channel_throttle.servo_out = g.throttle_min.get();
+       return;
+   }
+
    int throttle_target = g.throttle_cruise + throttle_nudge;  
+
+   /*
+     reduce target speed in proportion to turning rate, up to the
+     SPEED_TURN_GAIN percentage.
+    */
+   float steer_rate = fabsf(nav_steer / (float)SERVO_MAX);
+   steer_rate = constrain(steer_rate, 0.0, 1.0);
+   float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
+
+   if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
+       // in auto-modes we reduce speed when approaching waypoints
+       float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
+       if (reduction2 < reduction) {
+           reduction = reduction2;
+       }
+   }
+
+   target_speed *= reduction;
    
-   groundspeed_error = g.speed_cruise - ground_speed; 
+   groundspeed_error = target_speed - ground_speed; 
         
    throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
    g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
@@ -45,8 +69,10 @@ static void calc_nav_steer()
 	nav_steer = g.pidNavSteer.get_pid(bearing_error_cd, nav_gain_scaler);
 
     if (obstacle) {  // obstacle avoidance 
-	    nav_steer += g.sonar_turn_angle;
+	    nav_steer += g.sonar_turn_angle*100;
     }
+
+    g.channel_steer.servo_out = nav_steer;
 }
 
 /*****************************************
@@ -56,23 +82,15 @@ static void set_servos(void)
 {
     int16_t last_throttle = g.channel_throttle.radio_out;
 
-	if ((control_mode == MANUAL) || (control_mode == LEARNING)) {
+	if (control_mode == MANUAL || control_mode == LEARNING) {
 		// do a direct pass through of radio values
 		g.channel_steer.radio_out 		= g.channel_steer.radio_in;
-
-        if (obstacle)    // obstacle in front, turn right in Stabilize mode
-            g.channel_steer.radio_out -= 500;
-
 		g.channel_throttle.radio_out 	= g.channel_throttle.radio_in;
 	} else {       
         g.channel_steer.calc_pwm();
-		g.channel_throttle.radio_out = g.channel_throttle.radio_in;
 		g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, 
                                                        g.throttle_min.get(), 
                                                        g.throttle_max.get());
-    }
-                
-    if (control_mode >= AUTO) {
         // convert 0 to 100% into PWM
         g.channel_throttle.calc_pwm();
 

APMrover2/control_modes.pde

@@ -79,7 +79,7 @@ static void read_trim_switch()
                     }
                     CH7_wp_index = 1;     
 					return;
-				} else if (control_mode == LEARNING) {    
+				} else if (control_mode == LEARNING || control_mode == STEERING) {    
                     // if SW7 is ON in LEARNING = record the Wp
                     // set the next_WP (home is stored at 0)
 

APMrover2/defines.h

@@ -70,6 +70,7 @@ enum ch7_option {
 enum mode {
     MANUAL=0,
 	LEARNING=2,
+    STEERING=3,
     AUTO=10,
     RTL=11,
     GUIDED=15,

APMrover2/events.pde

@@ -11,6 +11,7 @@ static void failsafe_long_on_event(int fstype)
 	{
 		case MANUAL: 
 		case LEARNING:
+		case STEERING:
 			set_mode(RTL);
 			break;
 

APMrover2/navigation.pde

@@ -15,7 +15,7 @@ static void navigate()
 		return;
 	}
 
-	// waypoint distance from plane
+	// waypoint distance from rover
 	// ----------------------------
 	wp_distance = get_distance(&current_loc, &next_WP);
 

APMrover2/radio.pde

@@ -2,7 +2,7 @@
 
 //Function that will read the radio data, limit servos and trigger a failsafe
 // ----------------------------------------------------------------------------
-static uint8_t failsafeCounter = 0;		// we wait a second to take over the throttle and send the plane circling
+static uint8_t failsafeCounter = 0;		// we wait a second to take over the throttle and send the rover circling
 
 
 static void init_rc_in()

APMrover2/setup.pde

@@ -231,6 +231,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 			while (
 				mode != MANUAL &&
 				mode != LEARNING &&
+				mode != STEERING &&
 				mode != AUTO &&
 				mode != RTL) 
 			{

APMrover2/system.pde

@@ -311,6 +311,7 @@ static void set_mode(enum mode mode)
 	{
 		case MANUAL:
 		case LEARNING:
+		case STEERING:
 			break;
 
 		case AUTO:
@@ -362,7 +363,7 @@ static void startup_INS_ground(bool force_accel_level)
 	// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
 	// -----------------------
 	demo_servos(2);
-    gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
+    gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move vehicle"));
 	mavlink_delay(1000);
 
     ahrs.init();
@@ -493,6 +494,9 @@ print_mode(uint8_t mode)
     case LEARNING:
         cliSerial->println_P(PSTR("Learning"));
         break;
+    case STEERING:
+        cliSerial->println_P(PSTR("Stearing"));
+        break;
     case AUTO:
         cliSerial->println_P(PSTR("AUTO"));
         break;