From e3ff2df7a0fe18416f32da4295313514f3881fe6 Mon Sep 17 00:00:00 2001 From: jgs2185 Date: Mon, 14 Aug 2017 16:59:43 -0500 Subject: [PATCH] Ulanding: add POSIX support for Aerotenna ulanding radar --- src/drivers/ulanding/ulanding.cpp | 236 +++++++++++++++++++++++++++--- 1 file changed, 219 insertions(+), 17 deletions(-) diff --git a/src/drivers/ulanding/ulanding.cpp b/src/drivers/ulanding/ulanding.cpp index 89b88c62de..cc4b80716f 100644 --- a/src/drivers/ulanding/ulanding.cpp +++ b/src/drivers/ulanding/ulanding.cpp @@ -33,31 +33,84 @@ /** * @file ulanding.cpp + * @author Jessica Stockham * @author Roman Bapst * * Driver for the uLanding radar from Aerotenna */ +#if defined(__PX4_POSIX) +#include +#include +#include + +//this group added from hc_sr04 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + #include + #include #include #include #include +#if defined(__PX4_POSIX) +namespace ulanding +{ +#endif + #define ULANDING_MIN_DISTANCE 0.315f #define ULANDING_MAX_DISTANCE 50.0f -#define RADAR_RANGE_DATA 0x48 +#define RADAR_RANGE_DATA 0x48 +#define ULANDING_VERSION 1 +#if defined(__PX4_POSIX_OCPOC) +#define RADAR_DEFAULT_PORT "/dev/ttyS6" // default uLanding port on OCPOC +#else #define RADAR_DEFAULT_PORT "/dev/ttyS2" // telem2 on Pixhawk +#endif + +#if ULANDING_VERSION == 1 +uint8_t ulanding_hdr = 254; +#define BUF_LEN 18 +#else +uint8_t ulanding_hdr = 72; #define BUF_LEN 9 -#define SENS_VARIANCE 0.045f * 0.045f // assume standard deviation to be equal to sensor resolution. Static bench tests have shown that -// the sensor ouput does not vary if the unit is not moved. +#endif + +/* assume standard deviation to be equal to sensor resolution. + Static bench tests have shown that the sensor ouput does + not vary if the unit is not moved. */ +#define SENS_VARIANCE 0.045f * 0.045f + extern "C" __EXPORT int ulanding_radar_main(int argc, char *argv[]); +#if defined(__PX4_POSIX) +class Radar : public device::VDev +#else class Radar : public device::CDev +#endif { public: Radar(const char *port = RADAR_DEFAULT_PORT); @@ -83,7 +136,11 @@ private: void task_main(); bool read_and_parse(uint8_t *buf, int len, float *range); +#if defined(__PX4_POSIX) + bool is_header_byte(uint8_t c) {return (c == ulanding_hdr);}; +#else bool is_header_byte(uint8_t c) {return ((c & 0x80) == 0x00 && (c & 0x7F) == RADAR_RANGE_DATA);}; +#endif }; namespace radar @@ -92,6 +149,16 @@ Radar *g_dev; } Radar::Radar(const char *port) : +#if defined(__PX4_POSIX) + VDev("Radar", RANGE_FINDER0_DEVICE_PATH), + _task_should_exit(false), + _task_handle(-1), + _class_instance(-1), + _orb_class_instance(-1), + _distance_sensor_topic(nullptr), + _head(0), + _tail(0) +#else CDev("Radar", RANGE_FINDER0_DEVICE_PATH), _task_should_exit(false), _task_handle(-1), @@ -100,6 +167,8 @@ Radar::Radar(const char *port) : _distance_sensor_topic(nullptr), _head(0), _tail(0) +#endif + { /* store port name */ strncpy(_port, port, sizeof(_port)); @@ -153,6 +222,93 @@ Radar::init() do { /* create a scope to handle exit conditions using break */ +#if defined(__PX4_POSIX) + /* do regular vdev init */ + ret = VDev::init(); + + if (ret != OK) { + PX4_WARN("vdev init failed"); + break; + } + + /* open fd */ + int fd = ::open(_port, O_RDWR | O_NOCTTY); + PX4_INFO("passed open port"); + + if (fd < 0) { + PX4_WARN("failed to open serial device"); + ret = 1; + break; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(fd, &uart_config); + + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + //// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + + uart_config.c_oflag = 0; + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + + unsigned speed = B115200; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_WARN("ERR CFG: %d ISPD", termios_state); + ret = 1; + break; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_WARN("ERR CFG: %d OSPD\n", termios_state); + ret = 1; + break; + } + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + PX4_WARN("ERR baud %d ATTR", termios_state); + ret = 1; + break; + } + + ::close(fd); +#else /* do regular cdev init */ ret = CDev::init(); @@ -215,7 +371,7 @@ Radar::init() } px4_close(fd); - +#endif _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); struct distance_sensor_s ds_report = {}; @@ -290,25 +446,37 @@ bool Radar::read_and_parse(uint8_t *buf, int len, float *range) // check how many bytes are in the buffer, return if it's lower than the size of one package int num_bytes = _head >= _tail ? (_head - _tail + 1) : (_head + 1 + BUF_LEN - _tail); - if (num_bytes < 3) { + if (num_bytes < BUF_LEN / 3) { + PX4_DEBUG("not enough bytes"); return false; } int index = _head; uint8_t no_header_counter = 0; // counter for bytes which are non header bytes - uint8_t byte1 = _buf[_head]; - uint8_t byte2 = _buf[_head]; // go through the buffer backwards starting from the newest byte // if we find a header byte and the previous two bytes weren't header bytes // then we found the newest package. for (unsigned i = 0; i < num_bytes; i++) { if (is_header_byte(_buf[index])) { - if (no_header_counter >= 2) { - int raw = (byte1 & 0x7F); - raw += ((byte2 & 0x7F) << 7); + if (no_header_counter >= BUF_LEN / 3 - 1) { + if (ULANDING_VERSION == 1) { + if (((_buf[index + 1] + _buf[index + 2] + _buf[index + 3] + _buf[index + 4])) != (_buf[index + 5]) + || (_buf[index + 1] <= 0)) { + ret = false; + break; + } - *range = ((float)raw) * 0.045f; + int raw = _buf[index + 3] * 256 + _buf[index + 2]; + + *range = ((float)(raw / 100.)); // raw is in cm, converts range to meters; + + } else { + int raw = (_buf[index + 1] & 0x7F); + raw += ((_buf[index + 2] & 0x7F) << 7); + + *range = ((float)raw) * 0.045f; + } // set the tail to one after the index because we neglect // any data before the one we just read @@ -324,9 +492,6 @@ bool Radar::read_and_parse(uint8_t *buf, int len, float *range) no_header_counter++; } - byte2 = byte1; - byte1 = _buf[index]; - index--; if (index < 0) { @@ -341,20 +506,40 @@ bool Radar::read_and_parse(uint8_t *buf, int len, float *range) void Radar::task_main() { +#if defined(__PX4_POSIX) + int fd = ::open(_port, O_RDWR | O_NOCTTY); + if (fd < 0) { + PX4_WARN("serial port not open"); + } + + if (!isatty(fd)) { + PX4_WARN("not a serial device"); + } + +#else int fd = px4_open(_port, O_RDWR | O_NOCTTY); +#endif // we poll on data from the serial port +#if defined(__PX4_POSIX) + pollfd fds[1]; +#else px4_pollfd_struct_t fds[1]; +#endif fds[0].fd = fd; fds[0].events = POLLIN; - // read buffer, one measurement consists of three bytes + // read buffer uint8_t buf[BUF_LEN]; while (!_task_should_exit) { // wait for up to 100ms for data +#if defined(__PX4_POSIX) + int pret = ::poll(fds, (sizeof(fds) / sizeof(fds[0])), 100); +#else int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); +#endif // timed out if (pret == 0) { @@ -370,7 +555,11 @@ Radar::task_main() if (fds[0].revents & POLLIN) { memset(&buf[0], 0, sizeof(buf)); +#if defined(__PX4_POSIX) + int len = ::read(fd, &buf[0], sizeof(buf)); +#else int len = px4_read(fd, &buf[0], sizeof(buf)); +#endif if (len <= 0) { PX4_DEBUG("error reading radar"); @@ -395,18 +584,27 @@ Radar::task_main() report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR; report.id = 0; - // publish it + // publish radar data orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } } } +#if define__PX4_POSIX + ::close(fd); +#else px4_close(fd); +#endif } int ulanding_radar_main(int argc, char *argv[]) { + if (argc <= 1) { + PX4_WARN("not enough arguments, usage: start [device_path], stop, info "); + return 1; + } + /* * Start/load the driver. */ @@ -463,10 +661,14 @@ int ulanding_radar_main(int argc, char *argv[]) PX4_INFO("min distance %.2f m", (double)ULANDING_MIN_DISTANCE); PX4_INFO("max distance %.2f m", (double)ULANDING_MAX_DISTANCE); PX4_INFO("update rate: 500 Hz"); + return 0; } - PX4_WARN("unrecognized arguments, try: start [device_name], stop, info "); + PX4_WARN("unrecognized arguments, try: start [device_path], stop, info "); return 1; } +#if defined(__PX4_POSIX) +}; //namespace +#endif