From d496a2d1fe05059b76a82e4aaed541f8cf7b91f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Nov 2011 14:47:06 +1100 Subject: [PATCH] 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 | 4 +++- libraries/Desktop/support/main.cpp | 17 +++++++++++++- libraries/Desktop/support/sitl.cpp | 31 +++++++++++++++++--------- libraries/Desktop/support/sitl_gps.cpp | 6 ++--- 4 files changed, 43 insertions(+), 15 deletions(-) diff --git a/libraries/Desktop/support/desktop.h b/libraries/Desktop/support/desktop.h index a1cf197cb9..4c0c595e68 100644 --- a/libraries/Desktop/support/desktop.h +++ b/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); diff --git a/libraries/Desktop/support/main.cpp b/libraries/Desktop/support/main.cpp index efec0cb5fc..e7a9f925a8 100644 --- a/libraries/Desktop/support/main.cpp +++ b/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(); diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 65622fe4bb..b81fb8e2d8 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/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) { - (*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2; - (*reg[2]) = (*reg[4]) = (*reg[6]) = 1000*2; - (*reg[5]) = (*reg[7]) = 1800*2; + 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); } diff --git a/libraries/Desktop/support/sitl_gps.cpp b/libraries/Desktop/support/sitl_gps.cpp index 0148b007ab..b21dc1b1a9 100644 --- a/libraries/Desktop/support/sitl_gps.cpp +++ b/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;