Merge pull request PX4#5 from tumbili/JMAVsim_interface

JMAVSim interface

Tools/generate_listener.py

@@ -9,19 +9,33 @@
 
 raw_messages = glob.glob(sys.argv[1]+"/msg/*.msg")
 messages = []
-message_floats = []
+message_elements = []
 
 
 for index,m in enumerate(raw_messages):
-	temp_list = []
+	temp_list_floats = []
+	temp_list_uint64 = []
+	temp_list_bool = []
 	if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m):
+		temp_list = []
 		f = open(m,'r')
 		for line in f.readlines():
 			if(line.split(' ')[0] == "float32"):
-				temp_list.append(line.split(' ')[1].split('\t')[0].split('\n')[0])
+				temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0]))
+			elif(line.split(' ')[0] == "uint64"):
+				temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0]))
+			elif (line.split(' ')[0] == "bool"):
+				temp_list.append(("bool",line.split(' ')[1].split('\t')[0].split('\n')[0]))
+			elif (line.split(' ')[0] == "uint8") and len(line.split('=')) == 1:
+				temp_list.append(("uint8",line.split(' ')[1].split('\t')[0].split('\n')[0]))
+			elif ('float32[' in line.split(' ')[0]):
+				num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0])
+				temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
+
 		f.close()
 		messages.append(m.split('/')[-1].split('.')[0])
-		message_floats.append(temp_list)
+		message_elements.append(temp_list)
+
 num_messages = len(messages);
 
 print
@@ -76,6 +90,7 @@
 #include <cstring>
 #include <uORB/uORB.h>
 #include <string.h>
+#include <stdint.h>
 """
 for m in messages:
 	print "#include <uORB/topics/%s.h>" % m
@@ -104,10 +119,28 @@
 	print "\t\tID = ORB_ID(%s);" % m
 	print "\t\tstruct %s_s container;" % m
 	print "\t\tmemset(&container, 0, sizeof(container));"
+	print "\t\tbool updated;"
 	print "\t\tfor(uint32_t i = 0;i<num_msgs;i++) {"
+	print "\t\t\torb_check(sub,&updated);"
+	print "\t\t\tupdated = true;"
+	print "\t\t\tif(updated) {"
 	print "\t\t\torb_copy(ID,sub,&container);"
-	for item in message_floats[index+1]:
-		print "\t\t\tprintf(\"%s: %%f\\n \",container.%s);" % (item, item)
+	for item in message_elements[index+1]:
+		if item[0] == "float":
+			print "\t\t\tprintf(\"%s: %%f\\n \",container.%s);" % (item[1], item[1])
+		elif item[0] == "float_array":
+			print "\t\t\tprintf(\"%s:\");" % item[1]
+			print "\t\t\tfor (int j=0;j<%d;j++) {" % item[2]
+			print "\t\t\t\tprintf(\"%%f \",container.%s[j]);" % item[1]
+			print "\t\t\t}"
+			print "\t\t\tprintf(\"\\n\");"
+		elif item[0] == "uint64":
+			print "\t\t\tprintf(\"%s: %%f\\n \",(float)container.%s);" % (item[1], item[1])
+		elif item[0] == "uint8":
+			print "\t\t\tprintf(\"%s: %%f\\n \",(float)container.%s);" % (item[1], item[1])
+		elif item[0] == "bool":
+			print "\t\t\tprintf(\"%s: %%s\\n \",container.%s ? \"True\" : \"False\");" % (item[1], item[1])
+	print "\t\t\t}"
 	print "\t\t}"
 print "\t} else {"
 print "\t\t printf(\" Topic did not match any known topics\\n\");"

src/modules/simulator/module.mk

@@ -37,4 +37,10 @@
 
 MODULE_COMMAND		= simulator
 
-SRCS			= simulator.cpp 
+SRCS			= simulator.cpp
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed

src/modules/simulator/simulator.cpp

@@ -99,6 +99,90 @@ int Simulator::start(int argc, char *argv[])
 	return ret;
 }
 
+void Simulator::fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_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::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;
+}
+
+void Simulator::handle_message(mavlink_message_t *msg) {
+	switch(msg->msgid) {
+		case MAVLINK_MSG_ID_HIGHRES_IMU:
+			mavlink_highres_imu_t imu;
+			mavlink_msg_highres_imu_decode(msg, &imu);
+			fill_sensors_from_imu_msg(&_sensor, &imu);
+
+			// publish message
+			if(_sensor_combined_pub < 0) {
+				_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor);
+			} else {
+				orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor);
+			}
+			break;
+
+		case MAVLINK_MSG_ID_MANUAL_CONTROL:
+
+			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);
+
+			// publish message
+			if(_manual_control_sp_pub < 0) {
+				_manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp);
+			} else {
+				orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp);
+			}
+			break;
+	}
+}
+
 void Simulator::publishSensorsCombined() {
 
 	struct baro_report baro;
@@ -168,13 +252,36 @@ void Simulator::publishSensorsCombined() {
 #ifndef __PX4_QURT
 void Simulator::updateSamples()
 {
+	struct baro_report baro;
+	memset(&baro,0,sizeof(baro));
+	baro.pressure = 120000.0f;
+
+	// acceleration report
+	struct accel_report accel;
+	memset(&accel,0,sizeof(accel));
+	accel.z = 9.81f;
+	accel.range_m_s2 = 80.0f;
+
+	// gyro report
+	struct gyro_report gyro;
+	memset(&gyro, 0 ,sizeof(gyro));
+
+	// mag report
+	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);
+
 	// get samples from external provider
 	struct sockaddr_in myaddr;
 	struct sockaddr_in srcaddr;
 	socklen_t addrlen = sizeof(srcaddr);
 	int len, fd;
 	const int buflen = 200;
-	const int port = 9876;
+	const int port = 14550;
 	unsigned char buf[buflen];
 
 	if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
@@ -192,6 +299,38 @@ void Simulator::updateSamples()
 		return;
 	}
 
+	// wait for new mavlink messages to arrive
+	for (;;) {
+		len = 0;
+		len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen);
+		if (len > 0) {
+			mavlink_message_t msg;
+			mavlink_status_t status;
+			for (int i = 0; i < len; ++i)
+			{
+				if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
+				{
+					// have a message, handle it
+					handle_message(&msg);
+				}
+			}
+		}
+
+		// 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
+		//printf("Publishing SensorsCombined\n");
+		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);
+	}
+
+	/*
 	for (;;) {
 		len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen);
 		if (len > 0) {
@@ -212,6 +351,7 @@ void Simulator::updateSamples()
 			}
 		}
 	}
+	*/
 }
 #endif
 

src/modules/simulator/simulator.h

@@ -40,11 +40,14 @@
 
 #include <semaphore.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_baro.h>
 #include <drivers/drv_mag.h>
 #include <uORB/uORB.h>
+#include <v1.0/mavlink_types.h>
+#include <v1.0/common/mavlink.h>
 
 namespace simulator {
 
@@ -151,13 +154,24 @@ class Simulator {
 	bool getMPUReport(uint8_t *buf, int len);
 	bool getBaroSample(uint8_t *buf, int len);
 private:
-	Simulator() : _accel(1), _mpu(1), _baro(1) {}
+	Simulator() :
+	_accel(1),
+	_mpu(1),
+	_baro(1),
+	_sensor_combined_pub(-1),
+	_manual_control_sp_pub(-1),
+	_sensor{},
+	_manual_control_sp{}
+	{}
 	~Simulator() { _instance=NULL; }
 
 #ifndef __PX4_QURT
 	void updateSamples();
 #endif
 	void publishSensorsCombined();
+	void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu);
+	void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg);
+	void handle_message(mavlink_message_t *msg);
 
 	static Simulator *_instance;
 
@@ -170,5 +184,9 @@ class Simulator {
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
 	orb_advert_t _sensor_combined_pub;
+	orb_advert_t _manual_control_sp_pub;
+
+	struct sensor_combined_s _sensor;
+	struct manual_control_setpoint_s _manual_control_sp;
 };