Added simulator

Simulator listens for UDP input data at port 9876.

Data is for now comma separated. Not yet connected to the various sim
classes: accelsim, gyrosim, magsim.

Barometer measurements not yet supported.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/modules/simulator/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2015 MArk Charlebois. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build simulator 
+#
+
+MODULE_COMMAND		= simulator
+
+SRCS			= simulator.cpp 

src/modules/simulator/simulator.cpp

@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file simulator.cpp
+ * A device simulator
+ */
+
+#include <px4_tasks.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/types.h> 
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include "simulator.h"
+
+Simulator *Simulator::_instance = NULL;
+
+Simulator::Simulator() : _max_readers(3)
+{
+	sem_init(&_lock, 0, _max_readers);
+}
+
+Simulator *Simulator::getInstance()
+{
+	return _instance;
+}
+
+int Simulator::getSample(sim_dev_t dev, sample &val)
+{
+	int ret;
+
+	switch (dev) {
+	case SIM_GYRO:
+		read_lock();
+		val = _gyro[_readidx];
+		read_unlock();
+		ret = 0;
+		break;
+	case SIM_ACCEL:
+		read_lock();
+		val = _accel[_readidx];
+		read_unlock();
+		ret = 0;
+		break;
+	case SIM_MAG:
+		read_lock();
+		val = _mag[_readidx];
+		read_unlock();
+		ret = 0;
+		break;
+	default:
+		ret = 1;
+	}	
+	return ret;
+}
+
+int Simulator::start(int argc, char *argv[])
+{
+	int ret = 0;
+	_instance = new Simulator();
+	if (_instance) 
+		_instance->updateSamples();
+	else {
+		printf("Simulator creation failed\n");
+		ret = 1;
+	}
+	return ret;
+}
+
+
+void Simulator::updateSamples()
+{
+	// 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;
+        unsigned char buf[buflen];
+	int writeidx, num;
+
+        if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+                printf("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) {
+                printf("bind failed\n");
+		return;
+        }
+
+        for (;;) {
+                len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen);
+                if (len > 0) {
+			writeidx = !_readidx;
+                        buf[len] = 0;
+                        printf("received: %s\n", buf);
+			// FIXME - temp hack to read data, not safe - bad bad bad
+			num = sscanf((const char *)buf, "%f,%f,%f,%f,%f,%f,%f,%f,%f",
+				&_gyro[writeidx].x, &_gyro[writeidx].y, &_gyro[writeidx].z,
+				&_accel[writeidx].x, &_accel[writeidx].y, &_accel[writeidx].z,
+				&_mag[writeidx].x, &_mag[writeidx].y, &_mag[writeidx].z);
+			if (num != 9) {
+				printf("Only read %d items\n", num);
+			}
+			else {
+				// Swap read and write buffers
+				write_lock();
+				_readidx = !_readidx;
+				write_unlock();
+			}
+                }
+        }
+}
+
+void Simulator::read_lock()
+{
+	sem_wait(&_lock);
+}
+void Simulator::read_unlock()
+{
+	sem_post(&_lock);
+}
+void Simulator::write_lock()
+{
+	for (int i=0; i<_max_readers; i++) {
+		sem_wait(&_lock);
+	}
+}
+void Simulator::write_unlock()
+{
+	for (int i=0; i<_max_readers; i++) {
+		sem_post(&_lock);
+	}
+}
+
+extern "C" {
+
+int simulator_main(int argc, char *argv[])
+{
+	return (int)(px4_task_spawn_cmd("Simulator",
+			SCHED_DEFAULT,
+			SCHED_PRIORITY_MAX - 5,
+			1500,
+			Simulator::start,
+			nullptr) < 0);
+
+	return 0;
+}
+
+}
+

src/modules/simulator/simulator.h

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file simulator.h
+ * A device simulator
+ */
+
+#pragma once
+
+#include <semaphore.h>
+
+class Simulator {
+public:
+	static Simulator *getInstance();
+
+	enum sim_dev_t {
+		SIM_GYRO,
+		SIM_ACCEL,
+		SIM_MAG
+	};
+
+	struct sample {
+		float x;
+		float y;
+		float z;
+		sample() : x(0), y(0), z(0) {}
+		sample(float a, float b, float c) : x(a), y(b), z(c) {}
+	};
+
+	static int start(int argc, char *argv[]);
+
+	int getSample(sim_dev_t dev, sample &val);
+private:
+	Simulator();
+	~Simulator() { _instance=NULL; }
+
+	void updateSamples();
+
+	void read_lock();
+	void read_unlock();
+	void write_lock();
+	void write_unlock();
+
+	int _readidx;
+
+	sample _accel[2];
+	sample _gyro[2];
+	sample _mag[2];
+
+	static Simulator *_instance;
+	sem_t _lock;
+
+	const int _max_readers;
+};
+