Linux: Added barosim support to simulator

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

src/modules/simulator/simulator.cpp

@@ -50,11 +50,55 @@ static px4_task_t g_sim_task = -1;
 
 Simulator *Simulator::_instance = NULL;
 
-Simulator::Simulator() : _max_readers(3)
+SimulatorReport::SimulatorReport(int readers, int reportLen) : 
+	_max_readers(readers), 
+	_report_len(reportLen)
 {
 	sem_init(&_lock, 0, _max_readers);
 }
 
+bool SimulatorReport::copyData(void *inbuf, void *outbuf, int len)
+{
+	if (len != _report_len) {
+		return false;
+	}
+	read_lock();
+	memcpy(inbuf, outbuf, _report_len);
+	read_unlock();
+	return true;
+}
+
+bool SimulatorReport::writeData(void *inbuf, void *outbuf, int len)
+{
+	if (len != _report_len) {
+		return false;
+	}
+	memcpy(inbuf, outbuf, _report_len);
+	swapBuffers();
+	return true;
+}
+
+void SimulatorReport::read_lock()
+{
+	sem_wait(&_lock);
+}
+void SimulatorReport::read_unlock()
+{
+	sem_post(&_lock);
+}
+void SimulatorReport::write_lock()
+{
+	for (int i=0; i<_max_readers; i++) {
+		sem_wait(&_lock);
+	}
+}
+void SimulatorReport::write_unlock()
+{
+	for (int i=0; i<_max_readers; i++) {
+		sem_post(&_lock);
+	}
+}
+
 Simulator *Simulator::getInstance()
 {
 	return _instance;
@@ -62,13 +106,17 @@ Simulator *Simulator::getInstance()
 
 bool Simulator::getMPUReport(uint8_t *buf, int len)
 {
-	if (len != sizeof(MPUReport)) {
-		return false;
-	}
-	read_lock();
-	memcpy(buf, &_mpureport[_readidx], sizeof(MPUReport));
-	read_unlock();
-	return true;
+	return _mpu.copyData(buf, &_mpu._data[_mpu.getReadIdx()], len);
+}
+
+bool Simulator::getRawAccelReport(uint8_t *buf, int len)
+{
+	return _accel.copyData(buf, &_accel._data[_accel.getReadIdx()], len);
+}
+
+bool Simulator::getBaroSample(uint8_t *buf, int len)
+{
+	return _baro.copyData(buf, &_baro._data[_baro.getReadIdx()], len);
 }
 
 int Simulator::start(int argc, char *argv[])
@@ -95,7 +143,6 @@ void Simulator::updateSamples()
 	const int buflen = 200;
 	const int port = 9876;
         unsigned char buf[buflen];
-	int writeidx;
 
         if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
                 printf("create socket failed\n");
@@ -115,15 +162,17 @@ void Simulator::updateSamples()
         for (;;) {
                 len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen);
                 if (len > 0) {
-			writeidx = !_readidx;
-			if (len == sizeof(MPUReport)) {
+			if (len == sizeof(MPUReport::RawMPUData)) {
                         	printf("received: MPU data\n");
-				memcpy((void *)&_mpureport[writeidx], (void *)buf, len);
-			
-				// Swap read and write buffers
-				write_lock();
-				_readidx = !_readidx;
-				write_unlock();
+				_mpu.writeData(&_mpu._data[_mpu.getWriteIdx()], buf, len);
+			}
+			else if (len == sizeof(RawAccelReport::RawAccelData)) {
+                        	printf("received: accel data\n");
+				_accel.writeData(&_accel._data[_accel.getWriteIdx()], buf, len);
+			}
+			else if (len == sizeof(BaroReport::RawBaroData)) {
+                        	printf("received: accel data\n");
+				_baro.writeData(&_baro._data[_baro.getWriteIdx()], buf, len);
 			}
 			else {
                         	printf("bad packet: len = %d\n", len);
@@ -132,27 +181,6 @@ void Simulator::updateSamples()
         }
 }
 
-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);
-	}
-}
-
 static void usage()
 {
 	warnx("Usage: simulator {start|stop}");

src/modules/simulator/simulator.h

@@ -40,6 +40,85 @@
 
 #include <semaphore.h>
 
+class SimulatorReport {
+public:
+	SimulatorReport(int readers, int reportLen);
+	~SimulatorReport() {};
+
+	int getReadIdx() { return _readidx; }
+	int getWriteIdx() { return !_readidx; }
+
+	bool copyData(void *inbuf, void *outbuf, int len);
+	bool writeData(void *inbuf, void *outbuf, int len);
+
+protected:
+	void read_lock();
+	void read_unlock();
+	void write_lock();
+	void write_unlock();
+
+	void swapBuffers() 
+	{
+		write_lock(); 
+        	_readidx = !_readidx;
+		write_unlock(); 
+	}
+
+	int _readidx;
+	sem_t _lock;
+	const int _max_readers;
+	const int _report_len;
+};
+
+class RawAccelReport : public SimulatorReport {
+public:
+	RawAccelReport() : SimulatorReport(1, sizeof(RawAccelData)) {}
+	~RawAccelReport() {}
+
+// 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;
+        };
+#pragma pack(pop)
+
+	RawAccelData _data[2];
+};
+
+class MPUReport : public SimulatorReport {
+public:
+	MPUReport() : SimulatorReport(1, sizeof(RawMPUData)) {}
+	~MPUReport() {}
+
+#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];
+	};
+#pragma pack(pop)
+	
+	RawMPUData _data[2];
+};
+
+class BaroReport : public SimulatorReport {
+public:
+	BaroReport() : SimulatorReport(1, sizeof(RawBaroData)) {}
+	~BaroReport() {}
+
+	struct RawBaroData {
+		uint8_t		d[3];
+	};
+	
+	RawBaroData _data[2];
+};
+
 class Simulator {
 public:
 	static Simulator *getInstance();
@@ -60,44 +139,18 @@ public:
 
 	static int start(int argc, char *argv[]);
 
+	bool getRawAccelReport(uint8_t *buf, int len);
 	bool getMPUReport(uint8_t *buf, int len);
+	bool getBaroSample(uint8_t *buf, int len);
 private:
-	Simulator();
+	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;
-
-#pragma pack(push, 1)
-	/**
-	 * Report conversation within the GYROSIM, including command byte and
-	 * interrupt status.
-	 */
-	struct MPUReport {
-		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];
-	};
-#pragma pack(pop)
-	MPUReport _mpureport[2];
+	RawAccelReport 	_accel;
+	MPUReport	_mpu;
+	BaroReport	_baro;
 };
 

src/platforms/linux/drivers/barosim/baro_sim.cpp

@@ -50,6 +50,7 @@
 #include <unistd.h>
 
 #include <drivers/device/sim.h>
+#include <simulator/simulator.h>
 #include "barosim.h"
 #include "board_config.h"
 
@@ -201,5 +202,16 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len,
 {
 	// TODO add Simulation data connection so calls retrieve
 	// data from the simulator
+
+	if (send_len != 1 || send[0] != 0 || recv_len != 3) {
+		return 1;
+	}
+	else {
+		Simulator *sim = Simulator::getInstance();
+		if (sim == NULL) {
+			return -ENODEV;
+		}
+		sim->getBaroSample(recv, recv_len);
+	}
 	return 0;
 }