SITL: cleanup RC control for rover support

added vehicle type, and setup right initial throttle

libraries/Desktop/support/desktop.h

@@ -4,10 +4,16 @@
 #include <unistd.h>
 #include <sys/time.h>
 
+enum vehicle_type {
+	ArduCopter,
+	APMrover2,
+	ArduPlane
+};
+
 struct desktop_info {
 	bool slider; // slider switch state, True means CLI mode
 	struct timeval sketch_start_time;
-	bool quadcopter; // use quadcopter outputs
+	enum vehicle_type vehicle;
 	unsigned framerate;
 	float initial_height;
 	bool console_mode;

libraries/Desktop/support/main.cpp

@@ -67,12 +67,22 @@ int main(int argc, char * const argv[])
 		}
 	}
 
+	printf("Starting sketch '%s'\n", SKETCH);
+
 	if (strcmp(SKETCH, "ArduCopter") == 0) {
-		desktop_state.quadcopter = true;
+		desktop_state.vehicle = ArduCopter;
 		if (desktop_state.framerate == 0) {
 			desktop_state.framerate = 200;
 		}
+	} else if (strcmp(SKETCH, "APMrover2") == 0) {
+		desktop_state.vehicle = APMrover2;
+		if (desktop_state.framerate == 0) {
+			desktop_state.framerate = 50;
+		}
+		// set right default throttle for rover (allowing for reverse)
+		ICR4.set(2, 1500);
 	} else {
+		desktop_state.vehicle = ArduPlane;
 		if (desktop_state.framerate == 0) {
 			desktop_state.framerate = 50;
 		}

libraries/Desktop/support/sitl.cpp

@@ -144,8 +144,7 @@ static void sitl_fdm_input(void)
 }
 
 // used for noise generation in the ADC code
-// motor speed in revolutions per second
-float sitl_motor_speed[4] = {0,0,0,0};
+bool sitl_motors_on;
 
 /*
   send RC outputs to simulator
@@ -169,14 +168,18 @@ static void sitl_simulator_output(void)
 		for (i=0; i<11; i++) {
 			(*reg[i]) = 1000*2;
 		}
-		if (!desktop_state.quadcopter) {
+		if (desktop_state.vehicle == ArduPlane) {
 			(*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2;
 			(*reg[7]) = 1800*2;
 		}
+		if (desktop_state.vehicle == APMrover2) {
+			(*reg[0]) = (*reg[1]) = (*reg[2]) = (*reg[3]) = 1500*2;
+			(*reg[7]) = 1800*2;
+		}
 	}
 
 	// output at chosen framerate
-	if (millis() - last_update < 1000/desktop_state.framerate) {
+	if (last_update != 0 && millis() - last_update < 1000/desktop_state.framerate) {
 		return;
 	}
 	last_update = millis();
@@ -189,19 +192,27 @@ static void sitl_simulator_output(void)
 		}
 	}
 
-	if (!desktop_state.quadcopter) {
+	if (desktop_state.vehicle == ArduPlane) {
 		// add in engine multiplier
 		if (control.pwm[2] > 1000) {
 			control.pwm[2] = ((control.pwm[2]-1000) * sitl.engine_mul) + 1000;
 			if (control.pwm[2] > 2000) control.pwm[2] = 2000;
 		}
-
-		// 400kV motor, 16V
-		sitl_motor_speed[0] = ((control.pwm[2]-1000)/1000.0) * 400 * 16 / 60.0;
+		sitl_motors_on = ((control.pwm[2]-1000)/1000.0) > 0;
+	} else if (desktop_state.vehicle == APMrover2) {
+		// add in engine multiplier
+		if (control.pwm[2] != 1500) {
+			control.pwm[2] = ((control.pwm[2]-1500) * sitl.engine_mul) + 1500;
+			if (control.pwm[2] > 2000) control.pwm[2] = 2000;
+			if (control.pwm[2] < 1000) control.pwm[2] = 1000;
+		}
+		sitl_motors_on = ((control.pwm[2]-1500)/500.0) != 0;
 	} else {
-		// 850kV motor, 16V
+		sitl_motors_on = false;
 		for (i=0; i<4; i++) {
-			sitl_motor_speed[i] = ((control.pwm[i]-1000)/1000.0) * 850 * 12 / 60.0;
+			if ((control.pwm[i]-1000)/1000.0 > 0) {
+				sitl_motors_on = true;
+			}
 		}
 	}
 

libraries/Desktop/support/sitl_adc.cpp

@@ -91,23 +91,16 @@ void sitl_update_adc(float roll, 	float pitch, 	float yaw,		// Relative to earth
 	const float _accel_scale = 9.80665 / 423.8;
 	double adc[7];
 	double p, q, r;
-	extern float sitl_motor_speed[4];
-	bool motors_on = false;
+	extern bool sitl_motors_on;
 
 	SITL::convert_body_frame(roll, pitch,
 				 rollRate, pitchRate, yawRate,
 				 &p, &q, &r);
 
-	for (uint8_t i=0; i<4; i++) {
-		if (sitl_motor_speed[i] > 0.0) {
-			motors_on = true;
-		}
-	}
-
 	// minimum noise levels are 2 bits
 	float accel_noise = _accel_scale*2;
 	float gyro_noise = _gyro_gain_y*2;
-	if (motors_on) {
+	if (sitl_motors_on) {
 		// add extra noise when the motors are on
 		accel_noise += sitl.accel_noise;
 		gyro_noise += ToRad(sitl.gyro_noise);