save gps data so driver can read

src/modules/simulator/simulator.cpp

@@ -81,6 +81,11 @@ bool Simulator::getBaroSample(uint8_t *buf, int len)
 	return _baro.copyData(buf, len);
 }
 
+bool Simulator::getGPSSample(uint8_t *buf, int len)
+{
+	return _gps.copyData(buf, len);
+}
+
 void Simulator::write_MPU_data(void *buf) {
 	_mpu.writeData(buf);
 }
@@ -97,6 +102,10 @@ void Simulator::write_baro_data(void *buf) {
 	_baro.writeData(buf);
 }
 
+void Simulator::write_gps_data(void *buf) {
+	_gps.writeData(buf);
+}
+
 int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;

src/modules/simulator/simulator.h

@@ -98,6 +98,23 @@ struct RawBaroData {
 };
 #pragma pack(pop)
 
+#pragma pack(push, 1)
+struct RawGPSData {
+ int32_t lat;
+ int32_t lon;
+ int32_t alt;
+ uint16_t eph;
+ uint16_t epv;
+ uint16_t vel;
+ int16_t vn;
+ int16_t ve;
+ int16_t vd;
+ uint16_t cog;
+ uint8_t fix_type;
+ uint8_t satellites_visible;
+};
+#pragma pack(pop)
+
 template <typename RType> class Report {
 public:
 	Report(int readers) :
@@ -177,11 +194,13 @@ public:
 	bool getMagReport(uint8_t *buf, int len);
 	bool getMPUReport(uint8_t *buf, int len);
 	bool getBaroSample(uint8_t *buf, int len);
+	bool getGPSSample(uint8_t *buf, int len);
 
 	void write_MPU_data(void *buf);
 	void write_accel_data(void *buf);
 	void write_mag_data(void *buf);
 	void write_baro_data(void *buf);
+	void write_gps_data(void *buf);
 
 private:
 	Simulator() :
@@ -189,6 +208,7 @@ private:
 	_mpu(1),
 	_baro(1),
 	_mag(1),
+	_gps(1),
 	_sensor_combined_pub(nullptr)
 #ifndef __PX4_QURT
 	,
@@ -216,6 +236,7 @@ private:
 	simulator::Report<simulator::RawMPUData>	_mpu;
 	simulator::Report<simulator::RawBaroData>	_baro;
 	simulator::Report<simulator::RawMagData> 	_mag;
+	simulator::Report<simulator::RawGPSData>	_gps;
 
 	// uORB publisher handlers
 	orb_advert_t _accel_pub;
@@ -258,6 +279,7 @@ private:
 	void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
 	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
 	void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu);
+	void update_gps(mavlink_hil_gps_t *gps_sim);
 	static void *sending_trampoline(void *);
 	void send();
 #endif

src/modules/simulator/simulator_mavlink.cpp

@@ -188,6 +188,25 @@ void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sen
 	write_baro_data((void *)&baro);
 }
 
+void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
+	RawGPSData gps;
+	gps.lat = gps_sim->lat; 
+ 	gps.lon = gps_sim->lon;
+ 	gps.alt = gps_sim->alt;
+	gps.eph = gps_sim->eph;
+	gps.epv = gps_sim->epv;
+	gps.vel = gps_sim->vel;
+	gps.vn = gps_sim->vn;
+	gps.ve = gps_sim->ve;
+	gps.vd = gps_sim->vd;
+	gps.cog = gps_sim->cog;
+	gps.fix_type = gps_sim->fix_type;
+	gps.satellites_visible = gps_sim->satellites_visible;
+
+	write_gps_data((void *)&gps);
+
+}
+
 void Simulator::handle_message(mavlink_message_t *msg) {
 	switch(msg->msgid) {
 		case MAVLINK_MSG_ID_HIL_SENSOR:
@@ -196,6 +215,12 @@ void Simulator::handle_message(mavlink_message_t *msg) {
 			update_sensors(&_sensor, &imu);
 			break;
 
+		case MAVLINK_MSG_ID_HIL_GPS:
+			mavlink_hil_gps_t gps_sim;
+			mavlink_msg_hil_gps_decode(msg, &gps_sim);
+			update_gps(&gps_sim);
+			break;
+
 		case MAVLINK_MSG_ID_RC_CHANNELS:
 
 			mavlink_rc_channels_t rc_channels;