Barosim: Simplify to a plain loop without reschedules or sub interfaces. Behaves now. The DriverFramework needs closer inspection for busy-running threads.

src/platforms/posix/drivers/barosim/CMakeLists.txt

@@ -37,7 +37,6 @@ px4_add_module(
 		-Os
 	SRCS
 		baro.cpp
-		baro_sim.cpp
 	DEPENDS
 		platforms__common
 	)

src/platforms/posix/drivers/barosim/baro.cpp

@@ -53,7 +53,6 @@
 #include <math.h>
 #include <unistd.h>
 
-#include <px4_workqueue.h>
 #include <arch/board/board.h>
 #include <board_config.h>
 
@@ -62,6 +61,8 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/device/ringbuffer.h>
 
+#include <simulator/simulator.h>
+
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
@@ -83,6 +84,8 @@
 #define BAROSIM_CONVERSION_INTERVAL	10000	/* microseconds */
 #define BAROSIM_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
 
+using namespace DriverFramework;
+
 class BAROSIM : public VirtDevObj
 {
 public:
@@ -100,9 +103,6 @@ public:
 	void			print_info();
 
 protected:
-	BAROSIM_DEV		*_interface;
-
-	struct work_s		_work;
 
 	ringbuffer::RingBuffer	*_reports;
 
@@ -155,6 +155,8 @@ protected:
 	 */
 	void			cycle();
 
+	int			transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+
 	/**
 	 * Get the internal / external state
 	 *
@@ -195,7 +197,6 @@ extern "C" __EXPORT int barosim_main(int argc, char *argv[]);
 
 BAROSIM::BAROSIM(const char *path) :
 	VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 1e6 / 100),
-	_interface(nullptr),
 	_reports(nullptr),
 	_collect_phase(false),
 	_measure_phase(0),
@@ -212,10 +213,7 @@ BAROSIM::BAROSIM(const char *path) :
 	_comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "barosim_buffer_overflows"))
 {
-	// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
 
-	_interface = new BAROSIM_DEV;
 }
 
 BAROSIM::~BAROSIM()
@@ -234,7 +232,6 @@ BAROSIM::~BAROSIM()
 	perf_free(_comms_errors);
 	perf_free(_buffer_overflows);
 
-	delete _interface;
 }
 
 int
@@ -392,6 +389,8 @@ BAROSIM::devRead(void *buffer, size_t buflen)
 int
 BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
 {
+	PX4_WARN("baro IOCTL %llu", hrt_absolute_time());
+
 	switch (cmd) {
 
 	case SENSORIOCSPOLLRATE:
@@ -510,6 +509,7 @@ BAROSIM::start_cycle()
 void
 BAROSIM::stop_cycle()
 {
+	stop();
 	setSampleInterval(0);
 }
 
@@ -523,7 +523,8 @@ void
 BAROSIM::cycle()
 {
 	int ret;
-	unsigned long dummy = 0;
+
+	//PX4_WARN("baro cycle %llu", hrt_absolute_time());
 
 	/* collection phase? */
 	if (_collect_phase) {
@@ -532,11 +533,14 @@ BAROSIM::cycle()
 		ret = collect();
 
 		if (ret != OK) {
-			/* issue a reset command to the sensor */
-			_interface->devIOCTL(IOCTL_RESET, dummy);
+			uint8_t		cmd = ADDR_RESET_CMD;
+			int		result;
+
+			/* bump the retry count */
+			result = transfer(&cmd, 1, nullptr, 0);
 
 			/* reset the collection state machine and try again */
-			start_cycle();
+			//start_cycle();
 
 			return;
 		}
@@ -551,7 +555,7 @@ BAROSIM::cycle()
 		 */
 		if (_measure_phase != 0) {
 
-			setSampleInterval(BAROSIM_CONVERSION_INTERVAL);
+			//setSampleInterval(BAROSIM_CONVERSION_INTERVAL);
 
 			return;
 		}
@@ -563,17 +567,17 @@ BAROSIM::cycle()
 	if (ret != OK) {
 		//DEVICE_LOG("measure error %d", ret);
 		/* issue a reset command to the sensor */
-		_interface->devIOCTL(IOCTL_RESET, dummy);
+		//_interface->devIOCTL(IOCTL_RESET, dummy);
 
 		/* reset the collection state machine and try again */
-		start_cycle();
+		//start_cycle();
 		return;
 	}
 
 	/* next phase is collection */
 	_collect_phase = true;
 
-	setSampleInterval(BAROSIM_CONVERSION_INTERVAL);
+	//setSampleInterval(BAROSIM_CONVERSION_INTERVAL);
 }
 
 int
@@ -581,6 +585,8 @@ BAROSIM::measure()
 {
 	int ret;
 
+	//PX4_WARN("baro measure %llu", hrt_absolute_time());
+
 	perf_begin(_measure_perf);
 
 	/*
@@ -591,7 +597,8 @@ BAROSIM::measure()
 	/*
 	 * Send the command to begin measuring.
 	 */
-	ret = _interface->devIOCTL(IOCTL_MEASURE, addr);
+	uint8_t cmd = addr;
+	ret = transfer(&cmd, 1, nullptr, 0);
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
@@ -602,9 +609,48 @@ BAROSIM::measure()
 	return ret;
 }
 
+int
+BAROSIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	if (send_len == 1 && send[0] == ADDR_RESET_CMD) {
+		/* reset command */
+		return 0;
+
+	} else if (send_len == 1 && (send[0] == ADDR_CMD_CONVERT_D2 || send[0] == ADDR_CMD_CONVERT_D1)) {
+		/* measure command */
+		if (send[0] == ADDR_CMD_CONVERT_D2) {
+		} else {
+		}
+
+		return 0;
+
+	} else if (send[0] == 0 && send_len == 1) {
+		/* read requested */
+		Simulator *sim = Simulator::getInstance();
+
+		if (sim == NULL) {
+			PX4_ERR("Error BAROSIM_DEV::transfer no simulator");
+			return -ENODEV;
+		}
+
+		PX4_DEBUG("BAROSIM_DEV::transfer getting sample");
+		sim->getBaroSample(recv, recv_len);
+		return recv_len;
+
+	} else {
+		PX4_WARN("BAROSIM_DEV::transfer invalid param %u %u %u", send_len, send[0], recv_len);
+		return 1;
+	}
+
+
+	return 0;
+}
+
 int
 BAROSIM::collect()
 {
+	//PX4_WARN("baro collect %llu", hrt_absolute_time());
+
 	int ret;
 
 #pragma pack(push, 1)
@@ -623,7 +669,8 @@ BAROSIM::collect()
 	report.error_count = perf_event_count(_comms_errors);
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
-	ret = _interface->devRead((void *)&baro_report, sizeof(baro_report));
+	uint8_t cmd = 0;
+	ret = transfer(&cmd, 1, (uint8_t*)&report, sizeof(baro_report));
 
 	if (ret < 0) {
 		perf_count(_comms_errors);
@@ -660,7 +707,8 @@ BAROSIM::collect()
 		}
 
 		/* notify anyone waiting for data */
-		DevMgr::updateNotify(*this);
+		//DevMgr::updateNotify(*this);
+		updateNotify();
 	}
 
 	/* update the measurement state machine */

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

@@ -1,157 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. 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 baro_sim.cpp
- *
- * Simulation interface for barometer
- */
-
-/* XXX trim includes */
-#include <px4_config.h>
-#include <px4_defines.h>
-
-#include <sys/types.h>
-#include <assert.h>
-#include <unistd.h>
-
-#include <drivers/device/sim.h>
-#include <simulator/simulator.h>
-#include "barosim.h"
-#include "board_config.h"
-
-BAROSIM_DEV::BAROSIM_DEV() :
-	VirtDevObj("BAROSIM_DEV", "/dev/BAROSIM_DEV", BARO_BASE_DEVICE_PATH, 0)
-{
-}
-
-BAROSIM_DEV::~BAROSIM_DEV()
-{
-}
-
-int
-BAROSIM_DEV::init()
-{
-	return VirtDevObj::init();
-}
-
-ssize_t
-BAROSIM_DEV::devRead(void *data, size_t count)
-{
-	/* read the most recent measurement */
-	uint8_t cmd = 0;
-	int ret = transfer(&cmd, 1, static_cast<uint8_t *>(data), count);
-
-	return ret;
-}
-
-int
-BAROSIM_DEV::devIOCTL(unsigned long operation, unsigned long arg)
-{
-	int ret;
-
-	switch (operation) {
-	case IOCTL_RESET:
-		ret = _reset();
-		break;
-
-	case IOCTL_MEASURE:
-		ret = _doMeasurement(arg);
-		break;
-
-	default:
-		ret = EINVAL;
-	}
-
-	return ret;
-}
-
-int
-BAROSIM_DEV::_reset()
-{
-	uint8_t		cmd = ADDR_RESET_CMD;
-	int		result;
-
-	/* bump the retry count */
-	result = transfer(&cmd, 1, nullptr, 0);
-
-	return result;
-}
-
-int
-BAROSIM_DEV::_doMeasurement(unsigned addr)
-{
-	uint8_t cmd = addr;
-	return transfer(&cmd, 1, nullptr, 0);
-}
-
-int
-BAROSIM_DEV::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
-{
-	if (send_len == 1 && send[0] == ADDR_RESET_CMD) {
-		/* reset command */
-		return 0;
-
-	} else if (send_len == 1 && (send[0] == ADDR_CMD_CONVERT_D2 || send[0] == ADDR_CMD_CONVERT_D1)) {
-		/* measure command */
-		if (send[0] == ADDR_CMD_CONVERT_D2) {
-		} else {
-		}
-
-		return 0;
-
-	} else if (send[0] == 0 && send_len == 1) {
-		/* read requested */
-		Simulator *sim = Simulator::getInstance();
-
-		if (sim == NULL) {
-			PX4_ERR("Error BAROSIM_DEV::transfer no simulator");
-			return -ENODEV;
-		}
-
-		PX4_DEBUG("BAROSIM_DEV::transfer getting sample");
-		sim->getBaroSample(recv, recv_len);
-		return recv_len;
-
-	} else {
-		PX4_WARN("BAROSIM_DEV::transfer invalid param %u %u %u", send_len, send[0], recv_len);
-		return 1;
-	}
-
-
-	return 0;
-}
-
-void BAROSIM_DEV::_measure()
-{
-}

src/platforms/posix/drivers/barosim/barosim.h

@@ -79,38 +79,3 @@ union prom_u {
 extern bool crc4(uint16_t *n_prom);
 
 } /* namespace */
-
-using namespace DriverFramework;
-
-class BAROSIM_DEV : public VirtDevObj
-{
-public:
-	BAROSIM_DEV();
-	virtual ~BAROSIM_DEV();
-
-	virtual int	init();
-	virtual ssize_t	devRead(void *data, size_t count);
-	virtual int	devIOCTL(unsigned long operation, unsigned long arg);
-
-	virtual int	transfer(const uint8_t *send, unsigned send_len,
-				 uint8_t *recv, unsigned recv_len);
-
-protected:
-	virtual void _measure();
-
-private:
-	/**
-	 * Send a reset command to the barometer simulator.
-	 *
-	 * This is required after any bus reset.
-	 */
-	int		_reset();
-
-	/**
-	 * Send a measure command to the barometer simulator.
-	 *
-	 * @param addr		Which address to use for the measure operation.
-	 */
-	int		_doMeasurement(unsigned addr);
-
-};

src/platforms/posix/drivers/gpssim/gpssim.cpp

@@ -169,7 +169,7 @@ GPSSIM	*g_dev = nullptr;
 
 
 GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) :
-	VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 0),
+	VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10),
 	_task_should_exit(false),
 	//_healthy(false),
 	//_mode_changed(false),