Merge pull request #6 from tumbili/simulator_udp

implemented bidirectional udp communication with simulator

Author: mcharleb

src/modules/simulator/simulator.cpp

@@ -45,19 +45,18 @@
 #include <string.h>
 #include <sys/types.h> 
 #include <drivers/drv_led.h>
-#ifndef __PX4_QURT
-#include <sys/socket.h>
-#include <netinet/in.h>
-#endif
+
 #include "simulator.h"
-#include <drivers/drv_hrt.h>
 
 using namespace simulator;
 
 static px4_task_t g_sim_task = -1;
 
 Simulator *Simulator::_instance = NULL;
 
+static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
+static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+
 Simulator *Simulator::getInstance()
 {
 	return _instance;
@@ -100,6 +99,83 @@ int Simulator::start(int argc, char *argv[])
 	return ret;
 }
 
+void Simulator::poll_topics() {
+	// copy new data if available
+	bool updated;
+	orb_check(_actuator_outputs_sub, &updated);
+	if(updated) {
+		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
+	}
+
+	orb_check(_vehicle_attitude_sub, &updated);
+	if(updated) {
+		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude);
+	}
+}
+
+void Simulator::send_data() {
+	// check if it's time to send new data
+	hrt_abstime time_now = hrt_absolute_time();
+	if (time_now - _time_last >= (hrt_abstime)(_interval * 1000)) {
+		_time_last = time_now;
+		mavlink_message_t msg;
+		pack_actuator_message(&msg);
+		send_mavlink_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg, 200);
+		// can add more messages here, can also setup different timings
+	}
+}
+
+void Simulator::pack_actuator_message(mavlink_message_t *msg) {
+	// pack message and send
+	mavlink_servo_output_raw_t actuator_msg;
+
+	actuator_msg.time_usec = hrt_absolute_time();
+	actuator_msg.port = 8;	// hardcoded for now
+	actuator_msg.servo1_raw = _actuators.output[0];
+	actuator_msg.servo2_raw = _actuators.output[1];
+	actuator_msg.servo3_raw = _actuators.output[2];
+	actuator_msg.servo4_raw = _actuators.output[3];
+	actuator_msg.servo5_raw = _actuators.output[4];
+	actuator_msg.servo6_raw = _actuators.output[5];
+	actuator_msg.servo7_raw = _actuators.output[6];
+	actuator_msg.servo8_raw = _actuators.output[7];
+
+	// encode the message
+	mavlink_msg_servo_output_raw_encode(1, 100, msg, &actuator_msg);
+}
+
+void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
+	uint8_t payload_len = mavlink_message_lengths[msgid];
+
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+	/* header */
+	buf[0] = MAVLINK_STX;
+	buf[1] = payload_len;
+	/* no idea which numbers should be here*/
+	buf[2] = 100;
+	buf[3] = 1;
+	buf[4] = component_ID;
+	buf[5] = msgid;
+
+	/* payload */
+	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len);
+
+	/* checksum */
+	uint16_t checksum;
+	crc_init(&checksum);
+	crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
+	crc_accumulate(mavlink_message_crcs[msgid], &checksum);
+
+	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
+	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
+
+	ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen);
+	if (len <= 0) {
+		PX4_WARN("Failed sending mavlink message");
+	}
+}
+
 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;
@@ -221,7 +297,6 @@ void Simulator::publishSensorsCombined() {
 	// advertise
 	_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &sensors);
 
-
 	hrt_abstime time_last = hrt_absolute_time();
 	uint64_t delta;
 	for(;;) {
@@ -270,46 +345,52 @@ void Simulator::updateSamples()
 	// 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 = 14550;
-	unsigned char buf[buflen];
+	// subscribe to topics
+	_actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
+	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+
+	// try to setup udp socket for communcation with simulator
+	memset((char *)&_myaddr, 0, sizeof(_myaddr));
+	_myaddr.sin_family = AF_INET;
+	_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+	_myaddr.sin_port = htons(_port);
 
-	if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+	if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
 		PX4_WARN("create socket failed\n");
 		return;
 	}
 
-	memset((char *)&myaddr, 0, sizeof(myaddr));
-	myaddr.sin_family = AF_INET;
-	myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
-	myaddr.sin_port = htons(port);
-
-	if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) {
+	if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
 		PX4_WARN("bind failed\n");
 		return;
 	}
 
+	// this is used to time message sending
+	_time_last = hrt_absolute_time();
+
+	// make socket non-blocking
+	int flags = fcntl(_fd,F_GETFL);
+	flags |= O_NONBLOCK;
+	fcntl(_fd, F_SETFL, flags);
+
+	int len = 0;
 	// wait for new mavlink messages to arrive
 	for (;;) {
-		len = 0;
-		len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen);
+		// see if we can receive data from simulator
+		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))
+				if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status))
 				{
 					// have a message, handle it
 					handle_message(&msg);
@@ -324,35 +405,16 @@ void Simulator::updateSamples()
 		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) {
-			if (len == sizeof(RawMPUData)) {
-				PX4_DBG("received: MPU data\n");
-				_mpu.writeData(buf);
-			}
-			else if (len == sizeof(RawAccelData)) {
-				PX4_DBG("received: accel data\n");
-				_accel.writeData(buf);
-			}
-			else if (len == sizeof(RawBaroData)) {
-				PX4_DBG("received: accel data\n");
-				_baro.writeData(buf);
-			}
-			else {
-				PX4_DBG("bad packet: len = %d\n", len);
-			}
-		}
+		// see if there is new data to send to simulator
+		poll_topics();
+		send_data();
+		usleep(10000);
 	}
-	*/
 }
 #endif
 

src/modules/simulator/simulator.h

@@ -41,13 +41,20 @@
 #include <semaphore.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_baro.h>
 #include <drivers/drv_mag.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
+#include <sys/socket.h>
+#ifndef __PX4_QURT
+#include <netinet/in.h>
+#endif
 
 namespace simulator {
 
@@ -160,33 +167,68 @@ class Simulator {
 	_baro(1),
 	_sensor_combined_pub(-1),
 	_manual_control_sp_pub(-1),
+	_actuator_outputs_sub(-1),
+	_vehicle_attitude_sub(-1),
 	_sensor{},
-	_manual_control_sp{}
-	{}
+	_manual_control_sp{},
+	_actuators{},
+	_attitude{},
+	_interval(1000)
+	{
+		_buf = new unsigned char [_buflen];
+	}
 	~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;
 
+	// simulated sensor instances
 	simulator::Report<simulator::RawAccelData> 	_accel;
 	simulator::Report<simulator::RawMPUData>	_mpu;
 	simulator::Report<simulator::RawBaroData>	_baro;
 
+	// uORB publisher handlers
 	orb_advert_t _accel_pub;
 	orb_advert_t _baro_pub;
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
 	orb_advert_t _sensor_combined_pub;
 	orb_advert_t _manual_control_sp_pub;
 
+	// uORB subscription handlers
+	int _actuator_outputs_sub;
+	int _vehicle_attitude_sub;
+
+	// uORB data containers
 	struct sensor_combined_s _sensor;
 	struct manual_control_setpoint_s _manual_control_sp;
+	struct actuator_outputs_s _actuators;
+	struct vehicle_attitude_s _attitude;
+
+	// udp socket data
+	struct sockaddr_in _myaddr;
+	struct sockaddr_in _srcaddr;
+	socklen_t _addrlen = sizeof(_srcaddr);
+	int _fd;
+	const int _buflen = 200;
+	const int _port = 14550;
+	unsigned char *_buf;
+
+	hrt_abstime _time_last;
+	int _interval;
+
+	// class methods
+	int setup_udp_socket();
+	void do_subscriptions();
+	void poll_topics();
+	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);
+	void send_data();
+	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
+	void pack_actuator_message(mavlink_message_t *msg);
 };
-