AP_HAL_AVR_SITL: Adding Swift Binary Protocol stand-alone simulator

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -75,6 +75,8 @@ private:
     #define MAX_GPS_DELAY 100
     static gps_data _gps_data[MAX_GPS_DELAY];
 
+    static bool _gps_has_basestation_position;
+    static gps_data _gps_basestation_data;
     static void _gps_write(const uint8_t *p, uint16_t size);
     static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
     static void _update_gps_ubx(const struct gps_data *d);
@@ -84,6 +86,8 @@ private:
     static uint16_t _gps_nmea_checksum(const char *s);
     static void _gps_nmea_printf(const char *fmt, ...);
     static void _update_gps_nmea(const struct gps_data *d);
+    static void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
+    static void _update_gps_sbp(const struct gps_data *d, bool sim_rtk);
 
     static void _update_gps(double latitude, double longitude, float altitude,
 			    double speedN, double speedE, double speedD, bool have_lock);

libraries/AP_HAL_AVR_SITL/sitl_gps.cpp

@@ -33,6 +33,8 @@ extern const AP_HAL::HAL& hal;
 static uint8_t next_gps_index;
 static uint8_t gps_delay;
 SITL_State::gps_data SITL_State::_gps_data[MAX_GPS_DELAY];
+bool SITL_State::_gps_has_basestation_position = false;
+SITL_State::gps_data SITL_State::_gps_basestation_data;
 
 // state of GPS emulation
 static struct gps_state {
@@ -550,6 +552,159 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d)
                      dstring);
 }
 
+void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload)
+{
+	if (len != 0 && payload == 0) {
+		return; //SBP_NULL_ERROR;
+	}
+
+	uint8_t preamble = 0x55;
+	_gps_write(&preamble, 1);
+	_gps_write((uint8_t*)&msg_type, 2);
+	_gps_write((uint8_t*)&sender_id, 2);	
+	_gps_write(&len, 1);
+	if (len > 0) {
+		_gps_write((uint8_t*)payload, len);
+	}
+
+	uint16_t crc;
+	crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0);
+	crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc);
+	crc = crc16_ccitt(&(len), 1, crc);
+	crc = crc16_ccitt(payload, len, crc);
+	_gps_write((uint8_t*)&crc, 2);
+}
+
+
+void SITL_State::_update_gps_sbp(const struct gps_data *d, bool sim_rtk)
+{
+    struct PACKED sbp_gps_time_t {
+        uint16_t wn;     //< GPS week number
+        uint32_t tow;    //< GPS Time of Week rounded to the nearest ms
+        int32_t ns;      //< Nanosecond remainder of rounded tow
+        uint8_t flags;   //< Status flags (reserved)
+    } t;
+    struct PACKED sbp_pos_llh_t {
+        uint32_t tow;         //< GPS Time of Week
+        double lat;           //< Latitude
+        double lon;           //< Longitude
+        double height;        //< Height
+        uint16_t h_accuracy;  //< Horizontal position accuracy estimate
+        uint16_t v_accuracy;  //< Vertical position accuracy estimate
+        uint8_t n_sats;       //< Number of satellites used in solution
+        uint8_t flags;        //< Status flags
+    } pos;
+    struct PACKED sbp_vel_ned_t {
+        uint32_t tow;          //< GPS Time of Week
+        int32_t n;             //< Velocity North coordinate
+        int32_t e;             //< Velocity East coordinate
+        int32_t d;             //< Velocity Down coordinate
+        uint16_t h_accuracy;   //< Horizontal velocity accuracy estimate
+        uint16_t v_accuracy;   //< Vertical velocity accuracy estimate
+        uint8_t n_sats;        //< Number of satellites used in solution
+        uint8_t flags;         //< Status flags (reserved)
+    } velned;
+    struct PACKED sbp_dops_t {
+        uint32_t tow;    //< GPS Time of Week
+        uint16_t gdop;   //< Geometric Dilution of Precision
+        uint16_t pdop;   //< Position Dilution of Precision
+        uint16_t tdop;   //< Time Dilution of Precision
+        uint16_t hdop;   //< Horizontal Dilution of Precision
+        uint16_t vdop;   //< Vertical Dilution of Precision
+    } dops;
+    struct PACKED sbp_baseline_ecef_t {
+        uint32_t tow;        //< GPS Time of Week
+        int32_t x;           //< Baseline ECEF X coordinate
+        int32_t y;           //< Baseline ECEF Y coordinate
+        int32_t z;           //< Baseline ECEF Z coordinate
+        uint16_t accuracy;   //< Position accuracy estimate
+        uint8_t n_sats;      //< Number of satellites used in solution
+        uint8_t flags;       //< Status flags (reserved)
+    } baseline;
+    static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100;
+    static const uint16_t SBP_DOPS_MSGTYPE = 0x0206;
+    static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201;
+    static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x0202;
+    static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205;
+
+    uint16_t time_week;
+    uint32_t time_week_ms;
+
+    gps_time(&time_week, &time_week_ms);
+
+    t.wn = time_week;
+    t.tow = time_week_ms;
+    t.ns = 0;
+    t.flags = 0;
+    _sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
+
+    if (!d->have_lock) {
+    	return;
+    }
+
+    pos.tow = time_week_ms;
+    pos.lon = d->longitude;
+    pos.lat= d->latitude;
+    pos.height = d->altitude;
+    pos.h_accuracy = 5e3;
+    pos.v_accuracy = 10e3;
+    pos.n_sats = _sitl->gps_numsats;
+    pos.flags = 0;
+    _sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
+
+    velned.tow = time_week_ms;
+    velned.n = 1e3 * d->speedN;
+    velned.e  = 1e3 * d->speedE;
+    velned.d  = 1e3 * d->speedD;
+    velned.h_accuracy = 5e3;
+    velned.v_accuracy = 5e3;
+    velned.n_sats = _sitl->gps_numsats;
+    velned.flags = 0;
+    _sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
+
+    dops.tow = time_week_ms;
+    dops.gdop = 1;
+    dops.pdop = 1;
+    dops.tdop = 1;
+    dops.hdop = 100;
+    dops.vdop = 1;    
+    _sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), (uint8_t*)&dops);
+
+    //Also send baseline messages
+    if (sim_rtk && _gps_has_basestation_position) {
+
+        Vector3d homeLLH;
+        Vector3d currentLLH;
+        Vector3d homeECEF;
+        Vector3d currentECEF;
+        Vector3d baselineVector;
+
+        homeLLH[0] = _gps_basestation_data.latitude * DEG_TO_RAD_DOUBLE;
+        homeLLH[1] = _gps_basestation_data.longitude * DEG_TO_RAD_DOUBLE;
+        homeLLH[2] = _gps_basestation_data.altitude;
+
+        currentLLH[0] = d->latitude * DEG_TO_RAD_DOUBLE;
+        currentLLH[1] = d->longitude * DEG_TO_RAD_DOUBLE;
+        currentLLH[2] = d->altitude;
+
+        wgsllh2ecef(homeLLH, homeECEF);
+        wgsllh2ecef(currentLLH, currentECEF);
+
+        baselineVector = currentECEF - homeECEF;
+
+        baseline.tow = time_week_ms;
+        baseline.x = (int32_t) (baselineVector[0]*1e3); //Convert to MM
+        baseline.y = (int32_t) (baselineVector[1]*1e3); //Convert to MM
+        baseline.z = (int32_t) (baselineVector[2]*1e3); //Convert to MM
+        baseline.accuracy = 5e3;
+        baseline.n_sats = _sitl->gps_numsats;
+        baseline.flags = 0;
+        //printf("Sending baseline with length %f\n",baselineVector.length());
+        _sbp_send_message(SBP_BASELINE_ECEF_MSGTYPE, 0x2222, sizeof(baseline), (uint8_t*)&baseline);
+    }
+
+}
+
 /*
   possibly send a new GPS packet
  */
@@ -560,6 +715,20 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
 	char c;
     Vector3f glitch_offsets = _sitl->gps_glitch;
 
+    //Capture current position as basestation location for 
+    if (!_gps_has_basestation_position) {
+    	if (have_lock) {
+    		_gps_basestation_data.latitude = latitude;
+    		_gps_basestation_data.longitude = longitude;
+    		_gps_basestation_data.altitude = altitude;
+    		_gps_basestation_data.speedN = speedN;
+    		_gps_basestation_data.speedE = speedE;
+    		_gps_basestation_data.speedD = speedD;
+    		_gps_basestation_data.have_lock = have_lock;
+    		_gps_has_basestation_position = true;
+    	}
+    }
+
 	// run at configured GPS rate (default 5Hz)
 	if ((hal.scheduler->millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) {
 		return;
@@ -628,6 +797,15 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
 	case SITL::GPS_TYPE_NMEA:
 		_update_gps_nmea(&d);
 		break;
+
+	case SITL::GPS_TYPE_SBP:
+		_update_gps_sbp(&d, false);
+		break;
+
+	case SITL::GPS_TYPE_SBP_RTK:
+		_update_gps_sbp(&d, true);
+		break;
+
 	}
 }