dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Lorenz Meier 2015-11-30 00:41:46 +01:00
parent 989b65912b
commit 9ee4760fe0
5 changed files with 69 additions and 214 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/platforms/posix/drivers/barosim/CMakeLists.txt
View File

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

88
src/platforms/posix/drivers/barosim/baro.cpp
View File

@ -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 */

157
src/platforms/posix/drivers/barosim/baro_sim.cpp
View File

@ -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()
{
}

35
src/platforms/posix/drivers/barosim/barosim.h
View File

@ -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);
};

2
src/platforms/posix/drivers/gpssim/gpssim.cpp
View File

@ -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),