src/drivers: new FT7 series wind/airflow sensor support (#22471)

- new uorb topic sensor_airflow
 - log sensor_airflow optionally
This commit is contained in:
Henry Kotzé 2024-01-22 21:42:38 +02:00 committed by GitHub
parent 8be64278be
commit 736a730bb1
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
10 changed files with 697 additions and 0 deletions

View File

@ -192,6 +192,7 @@ set(msg_files
SensorsStatus.msg
SensorsStatusImu.msg
SensorUwb.msg
SensorAirflow.msg
SystemPower.msg
TakeoffStatus.msg
TaskStackInfo.msg

5
msg/SensorAirflow.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 speed # the speed being reported by the wind / airflow sensor
float32 direction # the direction bein report by the wind / airflow sensor
uint8 status # Status code from the sensor

View File

@ -0,0 +1,10 @@
menu "Wind Sensors"
menuconfig COMMON_WIND_SENSOR
bool "Common wind sensor's"
default n
select DRIVERS_WIND_SENSOR_FT_TECHNOLOGIES
---help---
Enable default set of wind sensor drivers
rsource "*/Kconfig"
endmenu

View File

@ -0,0 +1,46 @@
############################################################################
#
# Copyright (c) 2023 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__wind_sensor__ft_technologies
MAIN ft7_technologies
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
ft7_technologies.cpp
ft7_technologies.hpp
ft7_technologies_main.cpp
DEPENDS
px4_work_queue
MODULE_CONFIG
module.yaml
)

View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_WIND_SENSOR_FT_TECHNOLOGIES
bool "ft7_technologies"
default n
---help---
Enable support for ft7_technologies

View File

@ -0,0 +1,359 @@
/****************************************************************************
*
* Copyright (c) 2023 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.
*
****************************************************************************/
#include "ft7_technologies.hpp"
#include <inttypes.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
Ft7Technologies::Ft7Technologies(const char *port) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
{
/* store port name */
strncpy(_port, port, sizeof(_port) - 1);
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
_sensor_airflow_pub.advertise();
}
Ft7Technologies::~Ft7Technologies()
{
stop();
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int
Ft7Technologies::init()
{
_interval = 1e6 / 10; // 10 Hz, The maximum query frequency is 10Hz
start();
return PX4_OK;
}
int Ft7Technologies::measure()
{
// Send the command to begin a measurement.
const char cmd[] = {'$', '/', '/', ',', 'W', 'V', '?', '*', '/', '/', '\r', '\n', '\0'};
int ret = ::write(_fd, cmd, 12);
perf_begin(_sample_perf);
if (ret != 12) {
perf_count(_comms_errors);
return ret;
}
_last_measure = hrt_absolute_time();
return PX4_OK;
}
uint8_t Ft7Technologies::hex2int(char ch)
{
if (ch >= '0' && ch <= '9') {
return ch - '0';
}
if (ch >= 'A' && ch <= 'F') {
return ch - 'A' + 10;
}
if (ch >= 'a' && ch <= 'f') {
return ch - 'a' + 10;
}
return -1;
}
bool Ft7Technologies::checksum(char *buf, uint32_t checksum)
{
uint32_t checksum_verify = 0;
for (int i = 0; i < _byte_counter; i++) {
if (!(buf[i] == '$' || buf[i] == '*')) {
checksum_verify ^= buf[i];
}
if (buf[i] == '*') {
i = _byte_counter;
}
}
return checksum_verify == checksum;
}
int Ft7Technologies::collect()
{
// PX4_INFO("collect");
/* clear buffer if last read was too long ago */
// int64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
// const hrt_abstime timestamp_sample = hrt_absolute_time();
int ret = ::read(_fd, readbuf, readlen);
// PX4_INFO("collect() ret: %d \n", ret);
if (ret < 0) {
// PX4_INFO("read err: %d", ret);
return -EAGAIN;
} else if (ret == 0) {
perf_count(_comms_errors);
return -EAGAIN;
}
_last_read = hrt_absolute_time();
bool valid = false;
//$<talkerID>,WVP=<speed>,<angle>,<status>*<checksum><cr><lf>
for (int i = 0; i < ret; i++) {
// _px4_windsensor.update(timestamp_sample, (double)ret, 13.0f, _status);
// received a full message
_readbuf[_byte_counter] = readbuf[i];
_byte_counter += 1;
if (readbuf[i] == '\n') {
_checksum = (uint32_t)atoi((char *)_raw_checksum);
_hex_checksum = 0;
sensor_airflow_s sensor_airflow{};
sensor_airflow.timestamp = hrt_absolute_time();
sensor_airflow.speed = (float)atoi((char *)_raw_speed) / 10.0f;
sensor_airflow.direction = ((float)atoi((char *)_raw_angle) - 180.0f) * M_PI_F / 180.0f;
sensor_airflow.status = (uint8_t)atoi((char *)_raw_status);
if (_checksum_counter == 2) {
_hex_checksum = hex2int(_raw_checksum[0]) << 4 | hex2int(_raw_checksum[1]);
} else {
_hex_checksum = hex2int(_raw_checksum[0]);
}
// checksum is verified
if (checksum(_readbuf, _hex_checksum)) {
_sensor_airflow_pub.publish(sensor_airflow);
valid = true;
}
// reset counters
_msg_part_counter = 0;
_byte_counter = 0;
_msg_byte_counter = 0;
_checksum_counter = 0;
memset(readbuf, 0, sizeof(_linebuf));
memset(_readbuf, 0, sizeof(_linebuf));
memset(_raw_speed, 0, 5);
memset(_raw_angle, 0, 5);
memset(_raw_status, 0, 2);
memset(_raw_checksum, 0, 3);
}
else if (readbuf[i] == '$' || readbuf[i] == ',' || readbuf[i] == '=' || readbuf[i] == '*' || readbuf[i] == '\r') {
_msg_part_counter += 1;
_msg_byte_counter = 0;
} else {
if (readbuf[i] != '.') {
if (_msg_part_counter == 3) { // speed measurement
_raw_speed[_msg_byte_counter] = readbuf[i];
} else if (_msg_part_counter == 4) { // angle measurement
_raw_angle[_msg_byte_counter] = readbuf[i];
} else if (_msg_part_counter == 5) { // status
_raw_status[_msg_byte_counter] = readbuf[i];
} else if (_msg_part_counter == 6) { // checksum
_checksum_counter += 1;
_raw_checksum[_msg_byte_counter] = readbuf[i];
}
_msg_byte_counter += 1;
}
}
}
if (!valid) {
return -EAGAIN;
}
perf_end(_sample_perf);
return PX4_OK;
}
void Ft7Technologies::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
/* schedule a cycle to start things */
ScheduleNow();
}
void Ft7Technologies::stop()
{
ScheduleClear();
}
void Ft7Technologies::Run()
{
/* fds initialized? */
if (_fd < 0) {
/* open fd */
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (_fd < 0) {
PX4_ERR("open failed (%i)", errno);
return;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(_fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
// uart_config.c_oflag &= ~ONLCR;
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
/* no parity, one stop bit */
uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
// 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);
// Turn off character processing
// clear current char size mask, no parity checking,
// no output processing, force 8 bit input
uart_config.c_cflag &= ~(CSIZE | PARENB);
uart_config.c_cflag |= CS8;
unsigned speed = B9600;
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d ISPD", termios_state);
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d OSPD", termios_state);
}
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("baud %d ATTR", termios_state);
}
}
/* collection phase? */
if (_collect_phase) {
/* perform collection */
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
if (hrt_elapsed_time(&_last_measure) > (_interval * 15) / 10) {
// resend the command again
// we waited to long to receive a response from the sensor
// so we are resending the command again
_collect_phase = false;
}
ScheduleNow();
return;
// we received a valid response from the sensor. we need to
// send another command through
} else if (collect_ret == PX4_OK) {
/* next phase is measurement */
_collect_phase = false;
}
}
/* measurement phase */
if (OK != measure()) {
PX4_INFO("measure error");
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(_interval);
}
void Ft7Technologies::print_info()
{
PX4_INFO_RAW("%s: port: %s\n", MODULE_NAME, _port);
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
}

View File

@ -0,0 +1,102 @@
/****************************************************************************
*
* Copyright (c) 2023 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 ft7_technolofies.hpp
* @author Henry Kotze <henry@flycloudline.com>
*
* Driver for the FT Technology Wind Sensor. FT742
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/sensor_airflow.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/device.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;
class Ft7Technologies : public px4::ScheduledWorkItem
{
public:
Ft7Technologies(const char *port);
~Ft7Technologies() override;
int init();
void print_info();
private:
void start();
void stop();
void Run() override;
int measure();
int collect();
bool checksum(char *buf, uint32_t checksum);
uint8_t hex2int(char ch);
uORB::Publication<sensor_airflow_s> _sensor_airflow_pub{ORB_ID(sensor_airflow)};
char _port[20] {};
char _readbuf[30] {};
uint64_t _interval{100000};
bool _collect_phase{false};
int _fd{-1};
char _linebuf[20] {};
hrt_abstime _last_read{0};
hrt_abstime _last_measure{0};
char _raw_angle[5];
char _raw_speed[5];
char _raw_status[2];
char _raw_checksum[3];
uint16_t _checksum{0};
uint32_t _hex_checksum{0};
int _msg_part_counter{0};
int _byte_counter{0};
int _msg_byte_counter{0};
uint32_t _checksum_counter{0};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
};

View File

@ -0,0 +1,162 @@
/****************************************************************************
*
* Copyright (c) 2023 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.
*
****************************************************************************/
#include "ft7_technologies.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace ft7_technologies_wind_sensor
{
Ft7Technologies *g_dev{nullptr};
static int start(const char *port)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
return -1;
}
if (port == nullptr) {
PX4_ERR("no device specified");
return -1;
}
/* create the driver */
g_dev = new Ft7Technologies(port);
PX4_INFO("port name: %s", port);
if (g_dev == nullptr) {
return -1;
}
if (g_dev->init() != PX4_OK) {
delete g_dev;
g_dev = nullptr;
return -1;
}
return 0;
}
static int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
return -1;
}
return 0;
}
static int status()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return -1;
}
g_dev->print_info();
return 0;
}
static int usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Serial bus driver for the FT Technologies Digital Wind Sensor FT742. This driver is required to operate alongside
a RS485 to UART signal transfer module.
Most boards are configured to enable/start the driver on a specified UART using the SENS_FTX_CFG parameter.
### Examples
Attempt to start driver on a specified serial device.
$ ft_technologies_serial start -d /dev/ttyS1
Stop driver
$ ft_technologies_serial stop
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("ft_technologies_serial", "driver");
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");
return PX4_OK;
}
} // namespace
extern "C" __EXPORT int ft7_technologies_main(int argc, char *argv[])
{
const char *device_path = nullptr;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'd':
device_path = myoptarg;
break;
default:
ft7_technologies_wind_sensor::usage();
return -1;
}
}
if (myoptind >= argc) {
ft7_technologies_wind_sensor::usage();
return -1;
}
if (!strcmp(argv[myoptind], "start")) {
return ft7_technologies_wind_sensor::start(device_path);
} else if (!strcmp(argv[myoptind], "stop")) {
return ft7_technologies_wind_sensor::stop();
} else if (!strcmp(argv[myoptind], "status")) {
return ft7_technologies_wind_sensor::status();
}
ft7_technologies_wind_sensor::usage();
return -1;
}

View File

@ -0,0 +1,6 @@
module_name: FT Technologies Digital Wind Sensor (serial)
serial_config:
- command: ft7_technologies start -d ${SERIAL_DEV}
port_config_param:
name: SENS_FTX_CFG
group: Sensors

View File

@ -100,6 +100,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("px4io_status");
add_topic("radio_status");
add_topic("rtl_time_estimate", 1000);
add_optional_topic("sensor_airflow", 100);
add_topic("sensor_combined");
add_optional_topic("sensor_correction");
add_optional_topic("sensor_gyro_fft", 50);