Linux: connected gyrosim to Simulator

Simulator gets incoming MPU data and gives raw MPU data to the
gyrosim sensor when read.

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

src/modules/simulator/simulator.cpp

@@ -37,6 +37,8 @@
  */
 
 #include <px4_tasks.h>
+#include <err.h>
+#include <errno.h>
 #include <stdio.h>
 #include <string.h>
 #include <sys/types.h> 
@@ -44,6 +46,8 @@
 #include <netinet/in.h>
 #include "simulator.h"
 
+static px4_task_t g_sim_task = -1;
+
 Simulator *Simulator::_instance = NULL;
 
 Simulator::Simulator() : _max_readers(3)
@@ -56,33 +60,15 @@ Simulator *Simulator::getInstance()
 	return _instance;
 }
 
-int Simulator::getSample(sim_dev_t dev, sample &val)
+bool Simulator::getMPUReport(uint8_t *buf, int len)
 {
-	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;
+	if (len != sizeof(MPUReport)) {
+		return false;
+	}
+	read_lock();
+	memcpy(buf, &_mpureport[_readidx], sizeof(MPUReport));
+	read_unlock();
+	return true;
 }
 
 int Simulator::start(int argc, char *argv[])
@@ -109,7 +95,7 @@ void Simulator::updateSamples()
 	const int buflen = 200;
 	const int port = 9876;
         unsigned char buf[buflen];
-	int writeidx, num;
+	int writeidx;
 
         if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
                 printf("create socket failed\n");
@@ -130,22 +116,18 @@ void Simulator::updateSamples()
                 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 {
+			if (len == sizeof(MPUReport)) {
+                        	printf("received: MPU data\n");
+				memcpy((void *)&_mpureport[writeidx], (void *)buf, len);
+			
 				// Swap read and write buffers
 				write_lock();
 				_readidx = !_readidx;
 				write_unlock();
 			}
+			else {
+                        	printf("bad packet: len = %d\n", len);
+			}
                 }
         }
 }
@@ -171,18 +153,47 @@ void Simulator::write_unlock()
 	}
 }
 
+static void usage()
+{
+	warnx("Usage: simulator {start|stop}");
+}
+
 extern "C" {
 
 int simulator_main(int argc, char *argv[])
 {
-	return (int)(px4_task_spawn_cmd("Simulator",
+	int ret = 0;
+	if (argc != 2) {
+		usage();
+		return 1;
+	}
+	if (strcmp(argv[1], "start") == 0) {
+		if (g_sim_task >= 0) {
+			warnx("Simulator already started");
+			return 0;
+		}
+		g_sim_task = px4_task_spawn_cmd("Simulator",
 			SCHED_DEFAULT,
 			SCHED_PRIORITY_MAX - 5,
 			1500,
 			Simulator::start,
-			nullptr) < 0);
+			nullptr);
+	}
+	else if (strcmp(argv[1], "stop") == 0) {
+		if (g_sim_task < 0) {
+			warnx("Simulator not running");
+		}
+		else {
+			px4_task_delete(g_sim_task);
+			g_sim_task = -1;
+		}
+	}
+	else {
+		usage();
+		ret = -EINVAL;
+	}
 
-	return 0;
+	return ret;
 }
 
 }

src/modules/simulator/simulator.h

@@ -60,7 +60,7 @@ public:
 
 	static int start(int argc, char *argv[]);
 
-	int getSample(sim_dev_t dev, sample &val);
+	bool getMPUReport(uint8_t *buf, int len);
 private:
 	Simulator();
 	~Simulator() { _instance=NULL; }
@@ -82,5 +82,22 @@ private:
 	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];
 };
 

src/platforms/linux/drivers/gyrosim/gyrosim.cpp

@@ -59,6 +59,8 @@
 #include <unistd.h>
 #include <getopt.h>
 
+#include <simulator/simulator.h>
+
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 #include <systemlib/conversions.h>
@@ -207,13 +209,6 @@ public:
 
 	void			print_registers();
 
-	/**
-	 * Test behaviour against factory offsets
-	 *
-	 * @return 0 on success, 1 on failure
-	 */
-	int 			factory_self_test();
-
 protected:
 	friend class GYROSIM_gyro;
 
@@ -273,10 +268,6 @@ private:
 	uint8_t			_checked_values[GYROSIM_NUM_CHECKED_REGISTERS];
 	uint8_t			_checked_next;
 
-	// use this to avoid processing measurements when in factory
-	// self test
-	volatile bool		_in_factory_test;
-
 	// last temperature reading for print_info()
 	float			_last_temperature;
 
@@ -419,6 +410,8 @@ private:
 		uint8_t		gyro_z[2];
 	};
 #pragma pack(pop)
+
+	uint8_t _regdata[108];
 };
 
 /*
@@ -507,7 +500,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro
 	_gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_rotation(rotation),
 	_checked_next(0),
-	_in_factory_test(false),
 	_last_temperature(0)
 {
 	// disable debug() calls
@@ -661,6 +653,30 @@ int GYROSIM::reset()
 int
 GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
+	uint8_t cmd = send[0];
+	uint8_t reg = cmd & 0x7F;
+	const uint8_t MPUREAD = MPUREG_INT_STATUS | DIR_READ;
+
+	if (cmd == MPUREAD) {
+		// Get data from the simulator
+		Simulator *sim = Simulator::getInstance();
+		if (sim == NULL)
+			return ENODEV;
+
+		// FIXME - not sure what interrupt status should be
+		recv[1] = 0;
+		// skip cmd and status bytes
+		sim->getMPUReport(&recv[2], len-2);
+	}
+	else if (cmd & DIR_READ)
+	{
+		printf("Reading %u bytes from register %u\n", len-1, reg);
+		memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1);
+	}
+	else {
+		printf("Writing %u bytes to register %u\n", len-1, reg);
+		memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1);
+	}
 	return PX4_OK;
 }
 
@@ -835,157 +851,6 @@ GYROSIM::gyro_self_test()
 	return 0;
 }
 
-
-
-/*
-  perform a self-test comparison to factory trim values. This takes
-  about 200ms and will return OK if the current values are within 14%
-  of the expected values (as per datasheet)
- */
-int
-GYROSIM::factory_self_test()
-{
-	_in_factory_test = true;
-	uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG);
-	uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG);
-	const uint16_t repeats = 100;
-	int ret = OK;
-
-	// gyro self test has to be done at 250DPS
-	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
-
-	struct MPUReport mpu_report;
-	float accel_baseline[3];
-	float gyro_baseline[3];
-	float accel[3];
-	float gyro[3];
-	float accel_ftrim[3];
-	float gyro_ftrim[3];
-
-	// get baseline values without self-test enabled
-        //set_frequency(GYROSIM_HIGH_BUS_SPEED);
-
-	memset(accel_baseline, 0, sizeof(accel_baseline));
-	memset(gyro_baseline, 0, sizeof(gyro_baseline));
-	memset(accel, 0, sizeof(accel));
-	memset(gyro, 0, sizeof(gyro));
-
-	for (uint8_t i=0; i<repeats; i++) {
-		usleep(1000);
-		mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
-		transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
-
-		accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x);
-		accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y);
-		accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z);
-		gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x);
-		gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y);
-		gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z);
-	}
-
-#if 1
-	write_reg(MPUREG_GYRO_CONFIG,
-		  BITS_FS_250DPS |
-		  BITS_GYRO_ST_X |
-		  BITS_GYRO_ST_Y |
-		  BITS_GYRO_ST_Z);
-
-	// accel 8g, self-test enabled all axes
-	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0);
-#endif
-
-	usleep(20000);
-
-	// get values with self-test enabled
-        //set_frequency(GYROSIM_HIGH_BUS_SPEED);
-
-
-	for (uint8_t i=0; i<repeats; i++) {
-		usleep(1000);
-		mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
-		transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
-		accel[0] += int16_t_from_bytes(mpu_report.accel_x);
-		accel[1] += int16_t_from_bytes(mpu_report.accel_y);
-		accel[2] += int16_t_from_bytes(mpu_report.accel_z);
-		gyro[0] += int16_t_from_bytes(mpu_report.gyro_x);
-		gyro[1] += int16_t_from_bytes(mpu_report.gyro_y);
-		gyro[2] += int16_t_from_bytes(mpu_report.gyro_z);
-	}
-
-	for (uint8_t i=0; i<3; i++) {
-		accel_baseline[i] /= repeats;
-		gyro_baseline[i] /= repeats;
-		accel[i] /= repeats;
-		gyro[i] /= repeats;
-	}
-
-	// extract factory trim values
-	uint8_t trims[4];
-	trims[0] = read_reg(MPUREG_TRIM1);
-	trims[1] = read_reg(MPUREG_TRIM2);
-	trims[2] = read_reg(MPUREG_TRIM3);
-	trims[3] = read_reg(MPUREG_TRIM4);
-	uint8_t atrim[3];
-	uint8_t gtrim[3];
-
-	atrim[0] = ((trims[0]>>3)&0x1C) | ((trims[3]>>4)&0x03);
-	atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03);
-	atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03);
-	gtrim[0] = trims[0] & 0x1F;
-	gtrim[1] = trims[1] & 0x1F;
-	gtrim[2] = trims[2] & 0x1F;
-
-	// convert factory trims to right units
-	for (uint8_t i=0; i<3; i++) {
-		accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f);
-		gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1);
-	}
-	// Y gyro trim is negative
-	gyro_ftrim[1] *= -1;
-
-	for (uint8_t i=0; i<3; i++) {
-		float diff = accel[i]-accel_baseline[i];
-		float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i];
-		::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n",
-			 (unsigned)i,
-			 (int)(1000*accel_baseline[i]),
-			 (int)(1000*accel[i]),
-			 (int)(1000*diff),
-			 (int)(1000*accel_ftrim[i]),
-			 (int)err);
-		if (fabsf(err) > 14) {
-			::printf("FAIL\n");
-			ret = -EIO;
-		}
-	}
-	for (uint8_t i=0; i<3; i++) {
-		float diff = gyro[i]-gyro_baseline[i];
-		float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
-		::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n",
-			 (unsigned)i,
-			 (int)(1000*gyro_baseline[i]),
-			 (int)(1000*gyro[i]),
-			 (int)(1000*(gyro[i]-gyro_baseline[i])),
-			 (int)(1000*gyro_ftrim[i]),
-			 (int)err);
-		if (fabsf(err) > 14) {
-			::printf("FAIL\n");
-			ret = -EIO;
-		}
-	}
-
-	write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
-	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
-
-	_in_factory_test = false;
-	if (ret == OK) {
-		::printf("PASSED\n");
-	}
-
-	return ret;
-}
-
-
 ssize_t
 GYROSIM::gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen)
 {
@@ -1426,11 +1291,6 @@ GYROSIM::check_registers(void)
 void
 GYROSIM::measure()
 {
-	if (_in_factory_test) {
-		// don't publish any data while in factory test mode
-		return;
-	}
-
 	if (hrt_absolute_time() < _reset_wait) {
 		// we're waiting for a reset to complete
 		return;
@@ -1746,7 +1606,6 @@ int	test();
 int	reset();
 int	info();
 int	regdump();
-int	factorytest();
 void	usage();
 
 /**
@@ -1952,27 +1811,10 @@ regdump()
 	return 0;
 }
 
-/**
- * Dump the register information
- */
-int
-factorytest()
-{
-	GYROSIM **g_dev_ptr = &g_dev_sim;
-	if (*g_dev_ptr == nullptr) {
-		warnx("driver not running");
-		return 1;
-	}
-
-	(*g_dev_ptr)->factory_self_test();
-
-	return 0;
-}
-
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest'");
+	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'");
 	warnx("options:");
 	warnx("    -R rotation");
 }
@@ -2042,10 +1884,6 @@ gyrosim_main(int argc, char *argv[])
 		ret = gyrosim::regdump();
 	}
 
-	else if (!strcmp(verb, "factorytest")) {
-		ret = gyrosim::factorytest();
-	}
-
 	else  {
 		gyrosim::usage();
 		ret = 1;