diff --git a/Tools/px4moduledoc/srcparser.py b/Tools/px4moduledoc/srcparser.py index 00b2e6ab0408..8de15afd5273 100644 --- a/Tools/px4moduledoc/srcparser.py +++ b/Tools/px4moduledoc/srcparser.py @@ -16,7 +16,7 @@ class ModuleDocumentation(object): valid_categories = ['driver', 'estimator', 'controller', 'system', 'communication', 'command', 'template', 'simulation'] valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor', - 'magnetometer', 'baro'] + 'magnetometer', 'baro', 'optical_flow'] max_line_length = 80 # wrap lines that are longer than this diff --git a/src/drivers/optical_flow/CMakeLists.txt b/src/drivers/optical_flow/CMakeLists.txt index 63a6932056a6..ab6c47729013 100644 --- a/src/drivers/optical_flow/CMakeLists.txt +++ b/src/drivers/optical_flow/CMakeLists.txt @@ -34,3 +34,4 @@ add_subdirectory(paw3902) add_subdirectory(pmw3901) add_subdirectory(px4flow) +add_subdirectory(thoneflow) diff --git a/src/drivers/optical_flow/thoneflow/CMakeLists.txt b/src/drivers/optical_flow/thoneflow/CMakeLists.txt new file mode 100644 index 000000000000..4fcbd5e4ffd1 --- /dev/null +++ b/src/drivers/optical_flow/thoneflow/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ +px4_add_module( + MODULE drivers__thoneflow + MAIN thoneflow + SRCS + thoneflow.cpp + thoneflow_parser.cpp + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/optical_flow/thoneflow/module.yaml b/src/drivers/optical_flow/thoneflow/module.yaml new file mode 100644 index 000000000000..262a23e048bd --- /dev/null +++ b/src/drivers/optical_flow/thoneflow/module.yaml @@ -0,0 +1,6 @@ +module_name: ThoneFlow-3901U optical flow sensor +serial_config: + - command: thoneflow start -d ${SERIAL_DEV} + port_config_param: + name: SENS_TFLOW_CFG + group: Sensors \ No newline at end of file diff --git a/src/drivers/optical_flow/thoneflow/thoneflow.cpp b/src/drivers/optical_flow/thoneflow/thoneflow.cpp new file mode 100644 index 000000000000..a469f2a324b1 --- /dev/null +++ b/src/drivers/optical_flow/thoneflow/thoneflow.cpp @@ -0,0 +1,546 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +/** + * @author Mohammed Kabir + * + * Driver for the ThoneFlow-3901U optical flow sensor + */ + +#include +#ifdef __PX4_CYGWIN +#include +#endif + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "thoneflow_parser.h" + +class Thoneflow : public px4::ScheduledWorkItem +{ +public: + Thoneflow(const char *port); + virtual ~Thoneflow(); + + virtual int init(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + char _port[20]; + Rotation _rotation; + int _cycle_interval; + int _fd; + char _linebuf[5]; + unsigned _linebuf_index; + THONEFLOW_PARSE_STATE _parse_state; + + hrt_abstime _last_read; + + optical_flow_s _report; + orb_advert_t _optical_flow_pub; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + + /** + * Initialise the automatic measurement state machine and start it. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void Run() override; + int collect(); + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]); + +Thoneflow::Thoneflow(const char *port) : + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), + _rotation(Rotation(0)), + _cycle_interval(15150), + _fd(-1), + _linebuf_index(0), + _parse_state(THONEFLOW_PARSE_STATE0_UNSYNC), + _last_read(0), + _report(), + _optical_flow_pub(nullptr), + _sample_perf(perf_alloc(PC_ELAPSED, "thoneflow_read")), + _comms_errors(perf_alloc(PC_COUNT, "thoneflow_com_err")) +{ + /* store port name */ + strncpy(_port, port, sizeof(_port) - 1); + + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; +} + +Thoneflow::~Thoneflow() +{ + stop(); + + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int +Thoneflow::init() +{ + int ret = PX4_OK; + + do { /* create a scope to handle exit conditions using break */ + + /* open fd */ + _fd = ::open(_port, O_RDONLY | O_NOCTTY); + + if (_fd < 0) { + PX4_ERR("Error opening fd"); + return -1; + } + + /* Baudrate 19200, 8 bits, no parity, 1 stop bit */ + unsigned speed = B19200; + + 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) { + PX4_ERR("CFG: %d ISPD", termios_state); + ret = PX4_ERROR; + break; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d OSPD\n", termios_state); + ret = PX4_ERROR; + break; + } + + if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("baud %d ATTR", termios_state); + ret = PX4_ERROR; + 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) { + PX4_ERR("FAIL: flow fd"); + ret = PX4_ERROR; + break; + } + + /* get yaw rotation from sensor frame to body frame */ + param_t rot = param_find("SENS_FLOW_ROT"); + + if (rot != PARAM_INVALID) { + int32_t val = 0; + param_get(rot, &val); + + _rotation = Rotation(val); + } + + /* Initialise report structure */ + /* No gyro on this board */ + _report.gyro_x_rate_integral = NAN; + _report.gyro_y_rate_integral = NAN; + _report.gyro_z_rate_integral = NAN; + + /* Conservative specs according to datasheet */ + _report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s + _report.min_ground_distance = 0.1f; // Datasheet: 80mm + _report.max_ground_distance = 30.0f; // Datasheet: infinity + + /* Integrated flow is sent at 66fps */ + _report.frame_count_since_last_readout = 1; + _report.integration_timespan = 15151; // microseconds + + /* Get a publish handle on the optical flow topic */ + _optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report); + + if (_optical_flow_pub == nullptr) { + PX4_ERR("Failed to create optical_flow object"); + ret = PX4_ERROR; + break; + } + + } while (0); + + /* Close the fd */ + ::close(_fd); + _fd = -1; + + /* Start measurement */ + start(); + + return ret; +} + +int +Thoneflow::collect() +{ + perf_begin(_sample_perf); + + /* clear buffer if last read was too long ago */ + int64_t read_elapsed = hrt_elapsed_time(&_last_read); + + /* the buffer for read chars is the packet size */ + char readbuf[10]; + const unsigned readlen = 9; + + int ret = 0; + + /* Check the number of bytes available in the buffer*/ + int bytes_available = 0; + ::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available); + + if (!bytes_available) { + return -EAGAIN; + } + + bool valid = false; + + do { + /* Read from UART buffer) */ + ret = ::read(_fd, &readbuf[0], readlen); + + if (ret < 0) { + PX4_ERR("read err: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + + /* only throw an error if we time out */ + if (read_elapsed > (_cycle_interval * 2)) { + /* flush anything in RX buffer */ + tcflush(_fd, TCIFLUSH); + return ret; + + } else { + return -EAGAIN; + } + } + + _last_read = hrt_absolute_time(); + + /* Parse each byte of read buffer */ + for (int i = 0; i < ret; i++) { + valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &_report); + } + + /* Publish most recent valid measurement */ + if (valid) { + _report.timestamp = hrt_absolute_time(); + + /* Rotate measurements from sensor frame to body frame */ + float zeroval = 0.0f; + rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval); + + orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report); + } + + /* Bytes left to parse */ + bytes_available -= ret; + + } while (bytes_available > 0); + + /* no valid measurement after parsing all available bytes, or partial packet parsed */ + if (!valid || _parse_state != THONEFLOW_PARSE_STATE9_FOOTER) { + return -EAGAIN; + } + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +Thoneflow::start() +{ + ScheduleNow(); +} + +void +Thoneflow::stop() +{ + ScheduleClear(); +} + +void +Thoneflow::Run() +{ + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(_port, O_RDONLY | O_NOCTTY); + } + + if (collect() == -EAGAIN) { + /* Reschedule earlier to grab the missing bits, time to transmit 9 bytes @ 19200 bps */ + ScheduleDelayed(520 * 9); + return; + } + + ScheduleDelayed(_cycle_interval); +} + +void +Thoneflow::print_info() +{ + PX4_INFO("Using port '%s'", _port); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +/** + * Local functions in support of the shell command. + */ +namespace thoneflow +{ + +Thoneflow *g_dev; + +int start(const char *port); +int stop(); +int info(); +void usage(); + +/** + * Start the driver. + */ +int +start(const char *port) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return 1; + } + + /* create the driver */ + g_dev = new Thoneflow(port); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + return 0; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_ERR("driver start failed"); + return 1; +} + +/** + * Stop the driver + */ +int stop() +{ + if (g_dev != nullptr) { + PX4_INFO("stopping driver"); + delete g_dev; + g_dev = nullptr; + PX4_INFO("driver stopped"); + + } else { + PX4_ERR("driver not running"); + return 1; + } + + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + g_dev->print_info(); + + return 0; +} + +/** + * Print a little info on how to use the driver. + */ +void +usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Serial bus driver for the ThoneFlow-3901U optical flow sensor. + +Most boards are configured to enable/start the driver on a specified UART using the SENS_TFLOW_CFG parameter. + +Setup/usage information: https://docs.px4.io/en/sensor/thoneflow.html + +### Examples + +Attempt to start driver on a specified serial device. +$ thoneflow start -d /dev/ttyS1 +Stop driver +$ thoneflow stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("thoneflow", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("optical_flow"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information"); +} + +} // namespace + +int +thoneflow_main(int argc, char *argv[]) +{ + int ch; + const char *device_path = ""; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_path = myoptarg; + break; + + default: + PX4_WARN("Unknown option!"); + return -1; + } + } + + if (myoptind >= argc) { + goto out_error; + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[myoptind], "start")) { + if (strcmp(device_path, "") != 0) { + return thoneflow::start(device_path); + + } else { + PX4_WARN("Please specify device path!"); + thoneflow::usage(); + return -1; + } + } + + /* + * Stop the driver + */ + if (!strcmp(argv[myoptind], "stop")) { + return thoneflow::stop(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { + thoneflow::info(); + return 0; + } + +out_error: + PX4_ERR("unrecognized command"); + thoneflow::usage(); + return -1; +} diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp new file mode 100644 index 000000000000..28cc5a56936e --- /dev/null +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +/** + * @author Mohammed Kabir + * + * Declarations of parser for the ThoneFlow-3901U optical flow sensor + */ + +#include "thoneflow_parser.h" +#include +#include + +//#define THONEFLOW_DEBUG + +#ifdef THONEFLOW_DEBUG +#include + +const char *parser_state[] = { + "0_UNSYNC", + "1_HEADER", + "2_NBYTES", + "3_XM_L", + "4_XM_H", + "5_YM_L", + "6_YM_H", + "7_CHECKSUM", + "8_QUALITY", + "9_FOOTER" +}; +#endif + +bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, + optical_flow_s *flow) +{ + bool parsed_packet = false; + + switch (*state) { + case THONEFLOW_PARSE_STATE9_FOOTER: + if (c == 0xFE) { + *state = THONEFLOW_PARSE_STATE1_HEADER; + + } else { + *state = THONEFLOW_PARSE_STATE0_UNSYNC; + } + + break; + + case THONEFLOW_PARSE_STATE0_UNSYNC: + if (c == 0xFE) { + *state = THONEFLOW_PARSE_STATE1_HEADER; + } + + break; + + case THONEFLOW_PARSE_STATE1_HEADER: + if (c == 0x04) { + *state = THONEFLOW_PARSE_STATE2_NBYTES; + + } else { + *state = THONEFLOW_PARSE_STATE0_UNSYNC; + } + + break; + + case THONEFLOW_PARSE_STATE2_NBYTES: + *state = THONEFLOW_PARSE_STATE3_XM_L; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case THONEFLOW_PARSE_STATE3_XM_L: + *state = THONEFLOW_PARSE_STATE4_XM_H; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case THONEFLOW_PARSE_STATE4_XM_H: + *state = THONEFLOW_PARSE_STATE5_YM_L; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case THONEFLOW_PARSE_STATE5_YM_L: + *state = THONEFLOW_PARSE_STATE6_YM_H; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case THONEFLOW_PARSE_STATE6_YM_H: { + unsigned char cksm = 0; + + // Calculate checksum over motion values + for (int i = 0; i < 4; i++) { + cksm += parserbuf[i]; + } + + if (c == cksm) { + // Checksum valid, populate sensor report + int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0]; + int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2]; + flow->pixel_flow_x_integral = static_cast(delta_x) * (3.52e-3f); + flow->pixel_flow_y_integral = static_cast(delta_y) * (3.52e-3f); + *state = THONEFLOW_PARSE_STATE7_CHECKSUM; + + } else { + *state = THONEFLOW_PARSE_STATE0_UNSYNC; + } + + *parserbuf_index = 0; + } + + break; + + case THONEFLOW_PARSE_STATE7_CHECKSUM: + *state = THONEFLOW_PARSE_STATE8_QUALITY; + flow->quality = uint8_t(c); + + break; + + case THONEFLOW_PARSE_STATE8_QUALITY: + if (c == 0xAA) { + *state = THONEFLOW_PARSE_STATE9_FOOTER; + parsed_packet = true; + + } else { + *state = THONEFLOW_PARSE_STATE0_UNSYNC; + } + + break; + + } + +#ifdef THONEFLOW_DEBUG + printf("state: THONEFLOW_PARSE_STATE%s, got char: %#02x\n", parser_state[*state], c); +#endif + + return parsed_packet; +} diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h new file mode 100644 index 000000000000..fd27b8c35539 --- /dev/null +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#pragma once + +#include + +// Data Format for ThoneFlow 3901U +// =============================== +// 9 bytes total per message: +// 1) Header (0xFE) +// 2) Number of data bytes (0x04) +// 3) X motion low byte +// 4) X motion high byte +// 5) Y motion low byte +// 6) Y motion high byte +// 7) Checksum (byte3+...+byte6) +// 8) Quality +// 9) Footer (0xAA) + +enum THONEFLOW_PARSE_STATE { + THONEFLOW_PARSE_STATE0_UNSYNC = 0, + THONEFLOW_PARSE_STATE1_HEADER, + THONEFLOW_PARSE_STATE2_NBYTES, + THONEFLOW_PARSE_STATE3_XM_L, + THONEFLOW_PARSE_STATE4_XM_H, + THONEFLOW_PARSE_STATE5_YM_L, + THONEFLOW_PARSE_STATE6_YM_H, + THONEFLOW_PARSE_STATE7_CHECKSUM, + THONEFLOW_PARSE_STATE8_QUALITY, + THONEFLOW_PARSE_STATE9_FOOTER +}; + +bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, + optical_flow_s *report);