fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 04:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

tfmini: move part from constructor to init()

Browse Source 
This makes sure the driver fails if the device path is invalid (::open fails)
This commit is contained in:
ChristophTobler
2018-01-27 14:13:28 +01:00
committed by Lorenz Meier
parent f294445cd0
commit 7bf3425b7e
1 changed files with 63 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/tfmini/tfmini.cpp
+63 -49
View File
@@ -198,54 +198,6 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) :
/* enforce null termination */ /* enforce null termination */
_port[sizeof(_port) - 1] = '\0'; _port[sizeof(_port) - 1] = '\0';
/* open fd */
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);
/*baudrate 115200, 8 bits, no parity, 1 stop bit */
unsigned speed = B115200;
struct termios uart_config;
int termios_state;
tcgetattr(_fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
warnx("ERR CFG: %d ISPD", termios_state);
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERR CFG: %d OSPD\n", termios_state);
}
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR baud %d ATTR", termios_state);
}
uart_config.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
uart_config.c_cflag &= ~CSIZE;
uart_config.c_cflag |= CS8; /* 8-bit characters */
uart_config.c_cflag &= ~PARENB; /* no parity bit */
uart_config.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
uart_config.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */
/* setup for non-canonical mode */
uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
uart_config.c_oflag &= ~OPOST;
/* fetch bytes as they become available */
uart_config.c_cc[VMIN] = 1;
uart_config.c_cc[VTIME] = 1;
if (_fd < 0) {
warnx("FAIL: laser fd");
}
// disable debug() calls // disable debug() calls
_debug_enabled = false; _debug_enabled = false;
@@ -298,6 +250,67 @@ TFMINI::init()
do { /* create a scope to handle exit conditions using break */ do { /* create a scope to handle exit conditions using break */
/* open fd */
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);
if (_fd < 0) {
warnx("Error opening fd");
return -1;
}
/*baudrate 115200, 8 bits, no parity, 1 stop bit */
unsigned speed = B115200;
struct termios uart_config;
int termios_state;
tcgetattr(_fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
warnx("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);
ret = -1;
break;
}
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR baud %d ATTR", termios_state);
ret = -1;
break;
}
uart_config.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
uart_config.c_cflag &= ~CSIZE;
uart_config.c_cflag |= CS8; /* 8-bit characters */
uart_config.c_cflag &= ~PARENB; /* no parity bit */
uart_config.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
uart_config.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */
/* setup for non-canonical mode */
uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
uart_config.c_oflag &= ~OPOST;
/* fetch bytes as they become available */
uart_config.c_cc[VMIN] = 1;
uart_config.c_cc[VTIME] = 1;
if (_fd < 0) {
warnx("FAIL: laser fd");
ret = -1;
break;
}
/* do regular cdev init */ /* do regular cdev init */
ret = CDev::init(); ret = CDev::init();
@@ -330,7 +343,7 @@ TFMINI::init()
::close(_fd); ::close(_fd);
_fd = -1; _fd = -1;
return OK; return ret;
} }
void void
@@ -785,6 +798,7 @@ void stop()
warnx("stopping driver"); warnx("stopping driver");
delete g_dev; delete g_dev;
g_dev = nullptr; g_dev = nullptr;
warnx("driver stopped");
} else { } else {
errx(1, "driver not running"); errx(1, "driver not running");