major simulator rework:

- wait for first message from jMAVSim
  before sending data
- publish raw rc data coming from PIXHAWK (temporary)
- send some interesting messages to jMAVSim
- prepare sensor data for sim drivers to read

src/modules/simulator/simulator.cpp

@@ -71,11 +71,32 @@ bool Simulator::getRawAccelReport(uint8_t *buf, int len)
 	return _accel.copyData(buf, len);
 }
 
+bool Simulator::getMagReport(uint8_t *buf, int len)
+{
+	return _mag.copyData(buf, len);
+}
+
 bool Simulator::getBaroSample(uint8_t *buf, int len)
 {
 	return _baro.copyData(buf, len);
 }
 
+void Simulator::write_MPU_data(void *buf) {
+	_mpu.writeData(buf);
+}
+
+void Simulator::write_accel_data(void *buf) {
+	_accel.writeData(buf);
+}
+
+void Simulator::write_mag_data(void *buf) {
+	_mag.writeData(buf);
+}
+
+void Simulator::write_baro_data(void *buf) {
+	_baro.writeData(buf);
+}
+
 int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;

src/modules/simulator/simulator.h

@@ -48,6 +48,7 @@
 #include <drivers/drv_baro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
 #include <uORB/uORB.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
@@ -61,31 +62,46 @@ namespace simulator {
 // FIXME - what is the endianness of these on actual device?
 #pragma pack(push, 1)
 struct RawAccelData {
-        int16_t x;
-        int16_t y;
-        int16_t z;
+		float temperature;
+        float x;
+        float y;
+        float z;
+};
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+struct RawMagData {
+		float temperature;
+        float x;
+        float y;
+        float z;
 };
 #pragma pack(pop)
 
 #pragma pack(push, 1)
 struct RawMPUData {
-	uint8_t	accel_x[2];
-	uint8_t	accel_y[2];
-	uint8_t	accel_z[2];
-	uint8_t	temp[2];
-	uint8_t	gyro_x[2];
-	uint8_t	gyro_y[2];
-	uint8_t	gyro_z[2];
+	float	accel_x;
+	float	accel_y;
+	float	accel_z;
+	float	temp;
+	float	gyro_x;
+	float	gyro_y;
+	float	gyro_z;
 };
 #pragma pack(pop)
 
+#pragma pack(push, 1)
 struct RawBaroData {
-	uint8_t		d[3];
+	float pressure;
+	float altitude;
+	float temperature;
 };
+#pragma pack(pop)
 
 template <typename RType> class Report {
 public:
 	Report(int readers) :
+			_readidx(0),
         	_max_readers(readers),
         	_report_len(sizeof(RType))
 	{
@@ -158,23 +174,33 @@ public:
 	static int start(int argc, char *argv[]);
 
 	bool getRawAccelReport(uint8_t *buf, int len);
+	bool getMagReport(uint8_t *buf, int len);
 	bool getMPUReport(uint8_t *buf, int len);
 	bool getBaroSample(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);
+
 private:
 	Simulator() :
 	_accel(1),
 	_mpu(1),
 	_baro(1),
+	_mag(1),
 	_sensor_combined_pub(nullptr)
 #ifndef __PX4_QURT
 	,
-	_manual_control_sp_pub(nullptr),
+	_rc_channels_pub(nullptr),
 	_actuator_outputs_sub(-1),
 	_vehicle_attitude_sub(-1),
+	_manual_sub(-1),
 	_sensor{},
-	_manual_control_sp{},
+	_rc_input{},
 	_actuators{},
-	_attitude{}
+	_attitude{},
+	_manual{}
 #endif
 	{}
 	~Simulator() { _instance=NULL; }
@@ -189,6 +215,7 @@ private:
 	simulator::Report<simulator::RawAccelData> 	_accel;
 	simulator::Report<simulator::RawMPUData>	_mpu;
 	simulator::Report<simulator::RawBaroData>	_baro;
+	simulator::Report<simulator::RawMagData> 	_mag;
 
 	// uORB publisher handlers
 	orb_advert_t _accel_pub;
@@ -202,21 +229,26 @@ private:
 
 #ifndef __PX4_QURT
 	// uORB publisher handlers
-	orb_advert_t _manual_control_sp_pub;
+	orb_advert_t _rc_channels_pub;
 
 	// uORB subscription handlers
 	int _actuator_outputs_sub;
 	int _vehicle_attitude_sub;
+	int _manual_sub;
 
 	// uORB data containers
 	struct sensor_combined_s _sensor;
-	struct manual_control_setpoint_s _manual_control_sp;
+	struct rc_input_values _rc_input;
 	struct actuator_outputs_s _actuators;
 	struct vehicle_attitude_s _attitude;
+	struct manual_control_setpoint_s _manual;
 
 	int _fd;
 	unsigned char _buf[200];
 	hrt_abstime _time_last;
+	hrt_abstime _heartbeat_last;
+	hrt_abstime _attitude_last;
+	hrt_abstime _manual_last;
 	struct sockaddr_in _srcaddr;
 	socklen_t _addrlen = sizeof(_srcaddr);
 
@@ -225,6 +257,7 @@ private:
 	void send_data();
 	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);
 	static void *sending_trampoline(void *);
 	void send();
 #endif

src/modules/simulator/simulator_mavlink.cpp

@@ -85,68 +85,107 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 }
 
 void Simulator::send_data() {
-	// check if it's time to send new data
-	hrt_abstime time_now = hrt_absolute_time();
-	if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) {
-		_time_last = time_now;
-		mavlink_hil_controls_t msg;
-		pack_actuator_message(msg);
-		send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
-		// can add more messages here, can also setup different timings
+	mavlink_hil_controls_t msg;
+	pack_actuator_message(msg);
+	send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
+
+	// send heartbeat
+	if (hrt_elapsed_time(&_heartbeat_last) >= 1e6) {
+		_heartbeat_last = hrt_absolute_time();
+		mavlink_heartbeat_t msg;
+		msg.base_mode = 0;
+		msg.custom_mode = 0;
+		msg.autopilot = MAV_AUTOPILOT_PX4;
+		msg.mavlink_version = 3;
+		send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 200);
+	}
+
+	// send attitude message
+	if (hrt_elapsed_time(&_attitude_last) > 20000) {
+		_attitude_last = hrt_absolute_time();
+		mavlink_attitude_t msg;
+		msg.time_boot_ms = _attitude.timestamp / 1000;
+		msg.roll = _attitude.roll;
+		msg.pitch = _attitude.pitch;
+		msg.yaw = _attitude.yaw;
+		msg.rollspeed = _attitude.rollspeed;
+		msg.pitchspeed = _attitude.pitchspeed;
+		msg.yawspeed = _attitude.yawspeed;
+		send_mavlink_message(MAVLINK_MSG_ID_ATTITUDE, &msg, 200);
+	}
+
+	// send manual control setpoint
+	if (hrt_elapsed_time(&_manual_last) > 200000) {
+		_manual_last = hrt_absolute_time();
+		mavlink_manual_control_t msg;
+		msg.x = _manual.x * 1000;
+		msg.y = _manual.y * 1000;
+		msg.z = _manual.z * 1000;
+		msg.r = _manual.r * 1000;
+		msg.buttons = 0;
+		send_mavlink_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg, 200);
 	}
 }
 
-static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) {
-	manual->timestamp = hrt_absolute_time();
-	manual->x = man_msg->x / 1000.0f;
-	manual->y = man_msg->y / 1000.0f;
-	manual->r = man_msg->r / 1000.0f;
-	manual->z = man_msg->z / 1000.0f;
+static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) {
+	rc->timestamp_publication = hrt_absolute_time();
+	rc->timestamp_last_signal = hrt_absolute_time();
+	rc->channel_count = rc_channels->chancount;
+	rc->rssi = rc_channels->rssi;
+
+	rc->values[0] = rc_channels->chan1_raw;
+	rc->values[1] = rc_channels->chan2_raw;
+	rc->values[2] = rc_channels->chan3_raw;
+	rc->values[3] = rc_channels->chan4_raw;
+	rc->values[4] = rc_channels->chan5_raw;
+	rc->values[5] = rc_channels->chan6_raw;
+	rc->values[6] = rc_channels->chan7_raw;
+	rc->values[7] = rc_channels->chan8_raw;
+	rc->values[8] = rc_channels->chan9_raw;
+	rc->values[9] = rc_channels->chan10_raw;
+	rc->values[10] = rc_channels->chan11_raw;
+	rc->values[11] = rc_channels->chan12_raw;
+	rc->values[12] = rc_channels->chan13_raw;
+	rc->values[13] = rc_channels->chan14_raw;
+	rc->values[14] = rc_channels->chan15_raw;
+	rc->values[15] = rc_channels->chan16_raw;
+	rc->values[16] = rc_channels->chan17_raw;
+	rc->values[17] = rc_channels->chan18_raw;
 }
 
-static void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) {
-	hrt_abstime timestamp = hrt_absolute_time();
-	sensor->timestamp = timestamp;
-	sensor->gyro_raw[0] = imu->xgyro * 1000.0f;
-	sensor->gyro_raw[1] = imu->ygyro * 1000.0f;
-	sensor->gyro_raw[2] = imu->zgyro * 1000.0f;
-	sensor->gyro_rad_s[0] = imu->xgyro;
-	sensor->gyro_rad_s[1] = imu->ygyro;
-	sensor->gyro_rad_s[2] = imu->zgyro;
-
-	sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2;
-	sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2;
-	sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2;
-	sensor->accelerometer_m_s2[0] = imu->xacc;
-	sensor->accelerometer_m_s2[1] = imu->yacc;
-	sensor->accelerometer_m_s2[2] = imu->zacc;
-	sensor->accelerometer_mode = 0; // TODO what is this?
-	sensor->accelerometer_range_m_s2 = 32.7f; // int16
-	sensor->accelerometer_timestamp = timestamp;
-	sensor->timestamp = timestamp;
-
-	sensor->adc_voltage_v[0] = 0.0f;
-	sensor->adc_voltage_v[1] = 0.0f;
-	sensor->adc_voltage_v[2] = 0.0f;
-
-	sensor->magnetometer_raw[0] = imu->xmag * 1000.0f;
-	sensor->magnetometer_raw[1] = imu->ymag * 1000.0f;
-	sensor->magnetometer_raw[2] = imu->zmag * 1000.0f;
-	sensor->magnetometer_ga[0] = imu->xmag;
-	sensor->magnetometer_ga[1] = imu->ymag;
-	sensor->magnetometer_ga[2] = imu->zmag;
-	sensor->magnetometer_range_ga = 32.7f; // int16
-	sensor->magnetometer_mode = 0; // TODO what is this
-	sensor->magnetometer_cuttoff_freq_hz = 50.0f;
-	sensor->magnetometer_timestamp = timestamp;
-
-	sensor->baro_pres_mbar = imu->abs_pressure;
-	sensor->baro_alt_meter = imu->pressure_alt;
-	sensor->baro_temp_celcius = imu->temperature;
-	sensor->baro_timestamp = timestamp;
-
-	sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa
-	sensor->differential_pressure_timestamp = timestamp;
+void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) {
+	// write sensor data to memory so that drivers can copy data from there
+	RawMPUData mpu;
+	mpu.accel_x = imu->xacc;
+	mpu.accel_y = imu->yacc;
+	mpu.accel_z = imu->zacc;
+	mpu.temp = imu->temperature;
+	mpu.gyro_x = imu->xgyro;
+	mpu.gyro_y = imu->ygyro;
+	mpu.gyro_z = imu->zgyro;
+
+	write_MPU_data((void *)&mpu);
+
+	RawAccelData accel;
+	accel.x = imu->xacc;
+	accel.y = imu->yacc;
+	accel.z = imu->zacc;
+
+	write_accel_data((void *)&accel);
+
+	RawMagData mag;
+	mag.x = imu->xmag;
+	mag.y = imu->ymag;
+	mag.z = imu->zmag;
+
+	write_mag_data((void *)&mag);
+
+	RawBaroData baro;
+	baro.pressure = imu->abs_pressure;
+	baro.altitude = imu->pressure_alt;
+	baro.temperature = imu->temperature;
+
+	write_baro_data((void *)&baro);
 }
 
 void Simulator::handle_message(mavlink_message_t *msg) {
@@ -154,27 +193,20 @@ void Simulator::handle_message(mavlink_message_t *msg) {
 		case MAVLINK_MSG_ID_HIL_SENSOR:
 			mavlink_hil_sensor_t imu;
 			mavlink_msg_hil_sensor_decode(msg, &imu);
-			fill_sensors_from_imu_msg(&_sensor, &imu);
-
-			// publish message
-			if(_sensor_combined_pub == nullptr) {
-				_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor);
-			} else {
-				orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor);
-			}
+			update_sensors(&_sensor, &imu);
 			break;
 
-		case MAVLINK_MSG_ID_MANUAL_CONTROL:
+		case MAVLINK_MSG_ID_RC_CHANNELS:
 
-			mavlink_manual_control_t man_ctrl_sp;
-			mavlink_msg_manual_control_decode(msg, &man_ctrl_sp);
-			fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp);
+			mavlink_rc_channels_t rc_channels;
+			mavlink_msg_rc_channels_decode(msg, &rc_channels);
+			fill_rc_input_msg(&_rc_input, &rc_channels);
 
 			// publish message
-			if(_manual_control_sp_pub == nullptr) {
-				_manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp);
+			if(_rc_channels_pub == nullptr) {
+				_rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input);
 			} else {
-				orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp);
+				orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input);
 			}
 			break;
 	}
@@ -226,6 +258,12 @@ void Simulator::poll_topics() {
 	if(updated) {
 		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude);
 	}
+
+	orb_check(_manual_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
+	}
 }
 
 void *Simulator::sending_trampoline(void *) {
@@ -238,11 +276,18 @@ void Simulator::send() {
 	fds[0].fd = _actuator_outputs_sub;
 	fds[0].events = POLLIN;
 
-	_time_last = hrt_absolute_time();
+	_time_last = 0;
+	_heartbeat_last = 0;
+	_attitude_last = 0;
+	_manual_last = 0;
+	int pret = -1;
+	while(pret <= 0) {
+		pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+	}
 
 	while(true) {
 		// wait for up to 100ms for data
-		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		//timed out
 		if (pret == 0)
@@ -288,15 +333,10 @@ void Simulator::updateSamples()
 	struct mag_report mag;
 	memset(&mag, 0 ,sizeof(mag));
 
-	// init publishers
-	_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
-	_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
-	_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
-	_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
-
 	// subscribe to topics
 	_actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
 	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	// try to setup udp socket for communcation with simulator
 	memset((char *)&_myaddr, 0, sizeof(_myaddr));
@@ -355,10 +395,23 @@ void Simulator::updateSamples()
 	fds[1].events = POLLIN;
 
 	int len = 0;
+
+	// wait for first data from simulator and respond with first controls
+	// this is important for the UDP communication to work
+	int pret = -1;
+	while (pret <= 0) {
+		pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100);
+	}
+
+	if (fds[0].revents & POLLIN) {
+		len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
+		send_data();
+	}
+
 	// wait for new mavlink messages to arrive
 	while (true) {
 
-		int pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
+		pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
 
 		//timed out
 		if (pret == 0)
@@ -405,17 +458,5 @@ void Simulator::updateSamples()
 				}
 			}
 		}
-
-		// publish these messages so that attitude estimator does not complain
-		hrt_abstime time_last = hrt_absolute_time();
-		baro.timestamp = time_last;
-		accel.timestamp = time_last;
-		gyro.timestamp = time_last;
-		mag.timestamp = time_last;
-		// publish the sensor values
-		orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
-		orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro);
-		orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro);
-		orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
 	}
 }