SITL: allow starting location to come from parameters

libraries/SITL/SIM_ADSB.cpp

@@ -22,6 +22,7 @@
 #include <stdio.h>
 
 #include "SIM_Aircraft.h"
+#include <AP_HAL_SITL/SITL_State.h>
 
 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);
 }
 
 

libraries/SITL/SIM_Aircraft.cpp

@@ -74,78 +74,24 @@ Aircraft::Aircraft(const char *frame_str) :
     terrain = reinterpret_cast<AP_Terrain *>(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)) {
+    home = start_loc;
-        ::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_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<int32_t>(strtod(lat_s, nullptr) * 1.0e7);
-    loc.lng = static_cast<int32_t>(strtod(lon_s, nullptr) * 1.0e7);
-    loc.alt = static_cast<int32_t>(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
  */

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;

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
 
 };

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);