desktop: added framerate and initial height code

this allows the sensors to initialise before the simulator has
connected. The GPS will give a value indicating no GPS lock

libraries/Desktop/support/desktop.h

@@ -5,6 +5,8 @@ struct desktop_info {
 	bool slider; // slider switch state, True means CLI mode
 	struct timeval sketch_start_time;
 	bool quadcopter; // use quadcopter outputs
+	unsigned framerate;
+	float initial_height;
 };
 
 extern struct desktop_info desktop_state;
@@ -16,7 +18,7 @@ int sitl_gps_pipe(void);
 ssize_t sitl_gps_read(int fd, void *buf, size_t count);
 void sitl_update_compass(float heading, float roll, float pitch, float yaw);
 void sitl_update_gps(float latitude, float longitude, float altitude,
-		     float speedN, float speedE);
+		     float speedN, float speedE, bool have_lock);
 void sitl_update_adc(float roll, float pitch, float yaw, float airspeed);
 void sitl_setup_adc(void);
 void sitl_update_barometer(float altitude);

libraries/Desktop/support/main.cpp

@@ -20,6 +20,8 @@ static void usage(void)
 	printf("Options:\n");
 	printf("\t-s          enable CLI slider switch\n");
 	printf("\t-w          wipe eeprom and dataflash\n");
+	printf("\t-r RATE     set SITL framerate\n");
+	printf("\t-H HEIGHT   initial barometric height\n");
 }
 
 int main(int argc, char * const argv[])
@@ -29,7 +31,7 @@ int main(int argc, char * const argv[])
 	desktop_state.slider = false;
 	gettimeofday(&desktop_state.sketch_start_time, NULL);
 
-	while ((opt = getopt(argc, argv, "swh")) != -1) {
+	while ((opt = getopt(argc, argv, "swhr:H:")) != -1) {
 		switch (opt) {
 		case 's':
 			desktop_state.slider = true;
@@ -38,6 +40,12 @@ int main(int argc, char * const argv[])
 			unlink("eeprom.bin");
 			unlink("dataflash.bin");
 			break;
+		case 'r':
+			desktop_state.framerate = (unsigned)atoi(optarg);
+			break;
+		case 'H':
+			desktop_state.initial_height = atof(optarg);
+			break;
 		default:
 			usage();
 			exit(1);
@@ -46,6 +54,13 @@ int main(int argc, char * const argv[])
 
 	if (strcmp(SKETCH, "ArduCopter") == 0) {
 		desktop_state.quadcopter = true;
+		if (desktop_state.framerate == 0) {
+			desktop_state.framerate = 200;
+		}
+	} else {
+		if (desktop_state.framerate == 0) {
+			desktop_state.framerate = 50;
+		}
 	}
 
 	sitl_setup();

libraries/Desktop/support/sitl.cpp

@@ -125,7 +125,7 @@ static void sitl_fgear_input(void)
 
 		sitl_update_gps(d.fg_pkt.latitude, d.fg_pkt.longitude,
 				ft2m(d.fg_pkt.altitude),
-				ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE));
+				ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE), true);
 		sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading, kt2mps(d.fg_pkt.airspeed));
 		sitl_update_barometer(ft2m(d.fg_pkt.altitude));
 		sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading);
@@ -204,13 +204,19 @@ static void sitl_simulator_output(void)
 	uint8_t i;
 
 	if (last_update == 0) {
+		if (desktop_state.quadcopter) {
+			for (i=0; i<8; i++) {
+				(*reg[i]) = 1000*2;
+			}
+		} else {
 			(*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2;
 			(*reg[2]) = (*reg[4]) = (*reg[6]) = 1000*2;
 			(*reg[5]) = (*reg[7]) = 1800*2;
 		}
+	}
 
-	if (millis() - last_update < 50) {
-		// output to simulator at 20Hz
+	// output at chosen framerate
+	if (millis() - last_update < 1000/desktop_state.framerate) {
 		return;
 	}
 	last_update = millis();
@@ -251,6 +257,10 @@ static void timer_handler(int signum)
 
 	// send RC output to flight sim
 	sitl_simulator_output();
+
+	if (sim_input_count == 0) {
+		sitl_update_gps(0, 0, 0, 0, 0, false);
+	}
 }
 
 
@@ -292,8 +302,9 @@ void sitl_setup(void)
 	sitl_setup_adc();
 	printf("Starting SITL input\n");
 
-	// wait until the first valid sim packet has
-	// been processed, to ensure the sensor hardware
-	// has sane values
-	while (sim_input_count == 0) /* noop */ ;
+	// setup some initial values
+	sitl_update_barometer(desktop_state.initial_height);
+	sitl_update_adc(0, 0, 0, 0);
+	sitl_update_compass(0, 0, 0, 0);
+	sitl_update_gps(0, 0, 0, 0, 0, false);
 }

libraries/Desktop/support/sitl_gps.cpp

@@ -79,7 +79,7 @@ static void gps_send(uint8_t msgid, uint8_t *buf, uint16_t size)
   possibly send a new GPS UBLOX packet
  */
 void sitl_update_gps(float latitude, float longitude, float altitude,
-		     float speedN, float speedE)
+		     float speedN, float speedE, bool have_lock)
 {
 	struct ubx_nav_posllh {
 		uint32_t	time; // GPS msToW
@@ -130,8 +130,8 @@ void sitl_update_gps(float latitude, float longitude, float altitude,
 	pos.vertical_accuracy = 10;
 
 	status.time = pos.time;
-	status.fix_type = 3;
-	status.fix_status = 1;
+	status.fix_type = have_lock?3:0;
+	status.fix_status = have_lock?1:0;
 	status.differential_status = 0;
 	status.res = 0;
 	status.time_to_first_fix = 0;