diff --git a/libraries/SITL/SIM_ADSB.cpp b/libraries/SITL/SIM_ADSB.cpp index 926214ee76..1c94004d70 100644 --- a/libraries/SITL/SIM_ADSB.cpp +++ b/libraries/SITL/SIM_ADSB.cpp @@ -22,6 +22,7 @@ #include #include "SIM_Aircraft.h" +#include namespace SITL { @@ -30,7 +31,7 @@ SITL *_sitl; ADSB::ADSB(const struct sitl_fdm &_fdm, const char *_home_str) { float yaw_degrees; - Aircraft::parse_home(_home_str, home, yaw_degrees); + HALSITL::SITL_State::parse_home(_home_str, home, yaw_degrees); } diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index c6d0013640..b37270b46d 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -74,78 +74,24 @@ Aircraft::Aircraft(const char *frame_str) : terrain = reinterpret_cast(AP_Param::find_object("TERRAIN_")); } -void Aircraft::set_start_location(const char *home_str) +void Aircraft::set_start_location(const Location &start_loc, const float start_yaw) { - if (!parse_home(home_str, home, home_yaw)) { - ::printf("Failed to parse home string (%s). Should be LAT,LON,ALT,HDG e.g. 37.4003371,-122.0800351,0,353\n", home_str); - } + home = start_loc; + home_yaw = start_yaw; + home_is_set = true; + ::printf("Home: %f %f alt=%fm hdg=%f\n", home.lat*1e-7, home.lng*1e-7, home.alt*0.01, home_yaw); + location = home; ground_level = home.alt * 0.01f; dcm.from_euler(0.0f, 0.0f, radians(home_yaw)); } -/* - parse a home string into a location and yaw - */ -bool Aircraft::parse_home(const char *home_str, Location &loc, float &yaw_degrees) -{ - char *saveptr = nullptr; - char *s = strdup(home_str); - if (!s) { - free(s); - ::printf("No home string supplied\n"); - return false; - } - char *lat_s = strtok_r(s, ",", &saveptr); - if (!lat_s) { - free(s); - ::printf("Failed to parse latitude\n"); - return false; - } - char *lon_s = strtok_r(nullptr, ",", &saveptr); - if (!lon_s) { - free(s); - ::printf("Failed to parse longitude\n"); - return false; - } - char *alt_s = strtok_r(nullptr, ",", &saveptr); - if (!alt_s) { - free(s); - ::printf("Failed to parse altitude\n"); - return false; - } - char *yaw_s = strtok_r(nullptr, ",", &saveptr); - if (!yaw_s) { - free(s); - ::printf("Failed to parse yaw\n"); - return false; - } - - loc = {}; - loc.lat = static_cast(strtod(lat_s, nullptr) * 1.0e7); - loc.lng = static_cast(strtod(lon_s, nullptr) * 1.0e7); - loc.alt = static_cast(strtod(alt_s, nullptr) * 1.0e2); - - if (loc.lat == 0 && loc.lng == 0) { - // default to CMAC instead of middle of the ocean. This makes - // SITL in MissionPlanner a bit more useful - loc.lat = -35.363261*1e7; - loc.lng = 149.165230*1e7; - loc.alt = 584*100; - } - - yaw_degrees = strtof(yaw_s, nullptr); - free(s); - - return true; -} - /* return difference in altitude between home position and current loc */ @@ -471,6 +417,21 @@ void Aircraft::set_speedup(float speedup) setup_frame_time(rate_hz, speedup); } +void Aircraft::update_model(const struct sitl_input &input) +{ + if (!home_is_set) { + if (sitl == nullptr) { + return; + } + Location loc; + loc.lat = sitl->opos.lat.get() * 1.0e7; + loc.lng = sitl->opos.lng.get() * 1.0e7; + loc.alt = sitl->opos.alt.get() * 1.0e2; + set_start_location(loc, sitl->opos.hdg.get()); + } + update(input); +} + /* update the simulation attitude and relative position */ diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index fa62c7fad2..7eb4966dd9 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -40,7 +40,7 @@ public: Aircraft(const char *frame_str); // called directly after constructor: - virtual void set_start_location(const char *home_str); + virtual void set_start_location(const Location &start_loc, const float start_yaw); /* set simulation speedup @@ -69,6 +69,8 @@ public: */ virtual void update(const struct sitl_input &input) = 0; + void update_model(const struct sitl_input &input); + /* fill a sitl_fdm structure from the simulator state */ void fill_fdm(struct sitl_fdm &fdm); @@ -78,9 +80,6 @@ public: /* return normal distribution random numbers */ static double rand_normal(double mean, double stddev); - /* parse a home location string */ - static bool parse_home(const char *home_str, Location &loc, float &yaw_degrees); - // get frame rate of model in Hz float get_rate_hz(void) const { return rate_hz; } @@ -131,6 +130,7 @@ public: protected: SITL *sitl; Location home; + bool home_is_set; Location location; float ground_level; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 377e0b5c9e..638a57fe44 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -180,6 +180,12 @@ const AP_Param::GroupInfo SITL::var_info2[] = { AP_GROUPINFO("TIDE_DIR", 49, SITL, tide.direction, 0.0f), AP_GROUPINFO("TIDE_SPEED", 50, SITL, tide.speed, 0.0f), + // the following coordinates are for CMAC, in Canberra + AP_GROUPINFO("OPOS_LAT", 51, SITL, opos.lat, -35.363261f), + AP_GROUPINFO("OPOS_LNG", 52, SITL, opos.lng, 149.165230f), + AP_GROUPINFO("OPOS_ALT", 53, SITL, opos.alt, 584.0f), + AP_GROUPINFO("OPOS_HDG", 54, SITL, opos.hdg, 353.0f), + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index aaf8430fe0..327b26fad6 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -268,6 +268,14 @@ public: AP_Float speed; // m/s } tide; + // original simulated position + struct { + AP_Float lat; + AP_Float lng; + AP_Float alt; // metres + AP_Float hdg; // 0 to 360 + } opos; + uint16_t irlock_port; void simstate_send(mavlink_channel_t chan);