Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 21:23:57 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

tfmini : fix scheduling and modernize output

Browse Source 
This reduces the scheduling interval to 9ms, such that the driver is always ready to read new data. Running it at exactly 100Hz is not correct since the driver and sensor measurement intervals are not "in sync", causing the driver to miss data. This causes a fill-up of the UART buffer.
This commit is contained in:
Mohammed Kabir
2018-08-24 11:19:31 -04:00
committed by Lorenz Meier
parent e4088204ee
commit 3afa018954
1 changed files with 32 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/tfmini/tfmini.cpp
+32 -81
View File
@@ -118,16 +118,11 @@ private:
orb_advert_t _distance_sensor_topic;
unsigned _consecutive_fail_count;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
@@ -174,7 +169,7 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) :
_rotation(rotation),
_min_distance(0.30f),
_max_distance(12.0f),
_conversion_interval(10000),
_conversion_interval(9000),
_reports(nullptr),
_measure_ticks(0),
_collect_phase(false),
@@ -185,7 +180,6 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) :
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(nullptr),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "tfmini_read")),
_comms_errors(perf_alloc(PC_COUNT, "tfmini_com_err"))
{
@@ -233,7 +227,7 @@ TFMINI::init()
case 1: /* TFMINI (12m, 100 Hz)*/
_min_distance = 0.3f;
_max_distance = 12.0f;
_conversion_interval = 10000;
_conversion_interval = 9000;
break;
default:
@@ -247,10 +241,10 @@ TFMINI::init()
do { /* create a scope to handle exit conditions using break */
/* open fd */
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);
_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_fd < 0) {
warnx("Error opening fd");
PX4_ERR("Error opening fd");
return -1;
}
@@ -268,19 +262,19 @@ TFMINI::init()
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
warnx("ERR CFG: %d ISPD", termios_state);
PX4_ERR("CFG: %d ISPD", termios_state);
ret = -1;
break;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERR CFG: %d OSPD\n", termios_state);
PX4_ERR("CFG: %d OSPD\n", termios_state);
ret = -1;
break;
}
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR baud %d ATTR", termios_state);
PX4_ERR("baud %d ATTR", termios_state);
ret = -1;
break;
}
@@ -302,7 +296,7 @@ TFMINI::init()
uart_config.c_cc[VTIME] = 1;
if (_fd < 0) {
warnx("FAIL: laser fd");
PX4_ERR("FAIL: laser fd");
ret = -1;
break;
}
@@ -316,7 +310,7 @@ TFMINI::init()
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
warnx("mem err");
PX4_ERR("mem err");
ret = -1;
break;
}
@@ -538,7 +532,7 @@ TFMINI::collect()
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
DEVICE_DEBUG("read err: %d", ret);
PX4_ERR("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
@@ -629,7 +623,7 @@ TFMINI::cycle()
/* fds initialized? */
if (_fd < 0) {
/* open fd */
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);
_fd = ::open(_port, O_RDWR | O_NOCTTY);
}
/* collection phase? */
@@ -639,33 +633,15 @@ TFMINI::cycle()
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
/* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */
work_queue(HPWORK,
&_work,
(worker_t)&TFMINI::cycle_trampoline,
this,
USEC2TICK(1042 * 8));
USEC2TICK(87 * 9));
return;
}
if (OK != collect_ret) {
/* we know the sensor needs about four seconds to initialize */
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
DEVICE_LOG("collection error #%u", _consecutive_fail_count);
}
_consecutive_fail_count++;
/* restart the measurement state machine */
start();
return;
} else {
/* apparently success */
_consecutive_fail_count = 0;
}
/* next phase is measurement */
_collect_phase = false;
@@ -717,7 +693,6 @@ TFMINI *g_dev;
void start(const char *port, uint8_t rotation);
void stop();
void test();
void reset();
void info();
void usage();
@@ -730,7 +705,8 @@ start(const char *port, uint8_t rotation)
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
PX4_ERR("already started");
exit(1);
}
/* create the driver */
@@ -748,7 +724,7 @@ start(const char *port, uint8_t rotation)
fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
warnx("Opening device '%s' failed");
PX4_ERR("Opening device '%s' failed");
goto fail;
}
@@ -765,7 +741,8 @@ fail:
g_dev = nullptr;
}
errx(1, "driver start failed");
PX4_ERR("driver start failed");
exit(1);
}
/**
@@ -774,13 +751,14 @@ fail:
void stop()
{
if (g_dev != nullptr) {
warnx("stopping driver");
PX4_INFO("stopping driver");
delete g_dev;
g_dev = nullptr;
warnx("driver stopped");
PX4_INFO("driver stopped");
} else {
errx(1, "driver not running");
PX4_ERR("driver not running");
exit(1);
}
exit(0);
@@ -814,7 +792,8 @@ test()
/* start the sensor polling at 2 Hz rate */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
PX4_ERR("failed to set 2Hz poll rate");
exit(1);
}
/* read the sensor 5x and report each value */
@@ -827,7 +806,7 @@ test()
int ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
warnx("timed out");
PX4_ERR("timed out");
break;
}
@@ -835,7 +814,7 @@ test()
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
PX4_ERR("read failed: got %d vs exp. %d", sz, sizeof(report));
break;
}
@@ -844,33 +823,11 @@ test()
/* reset the sensor polling to the default rate */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "ERR: DEF RATE");
PX4_ERR("failed to set default poll rate");
exit(1);
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
PX4_INFO("PASS");
}
/**
@@ -880,7 +837,8 @@ void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
PX4_ERR("driver not running");
exit(1);
}
printf("state @ %p\n", g_dev);
@@ -958,13 +916,6 @@ tfmini_main(int argc, char *argv[])
tfmini::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
tfmini::reset();
}
/*
* Print driver information.
*/
@@ -973,6 +924,6 @@ tfmini_main(int argc, char *argv[])
}
out_error:
PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
PX4_ERR("unrecognized command, try 'start', 'test', or 'info'");
return -1;
}