zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ulanding_radar: move to PX4Rangefinder and cleanup

sbg
Daniel Agar 5 years ago committed by GitHub
parent
eef508cc4a
commit
ef9a930816
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 522 additions and 592 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/drivers/distance_sensor/CMakeLists.txt
  2. 584
      src/drivers/distance_sensor/ulanding/ulanding.cpp
  3. 226
      src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp
  4. 124
      src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp
  5. 13
      src/drivers/distance_sensor/ulanding_radar/CMakeLists.txt
  6. 1
      src/drivers/distance_sensor/ulanding_radar/module.yaml
  7. 164
      src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp

2
src/drivers/distance_sensor/CMakeLists.txt
Unescape View File

@ -42,5 +42,5 @@ add_subdirectory(sf1xx) @@ -42,5 +42,5 @@ add_subdirectory(sf1xx)
add_subdirectory(srf02)
add_subdirectory(teraranger)
add_subdirectory(tfmini)
add_subdirectory(ulanding)
add_subdirectory(ulanding_radar)
add_subdirectory(vl53lxx)

584
src/drivers/distance_sensor/ulanding/ulanding.cpp
Unescape View File

@ -1,584 +0,0 @@ @@ -1,584 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 ulanding.cpp
* @author Jessica Stockham <jessica@aerotenna.com>
* @author Roman Bapst <roman@uaventure.com>
*
* Driver for the uLanding radar from Aerotenna
*/
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <poll.h>
#include <semaphore.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <mathlib/mathlib.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/workqueue.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/err.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/uORB.h>
using namespace time_literals;
#define ULANDING_MEASURE_INTERVAL 10_ms
#define ULANDING_MAX_DISTANCE 50.0f
#define ULANDING_MIN_DISTANCE 0.315f
#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" // Default serial port on Pixhawk (TELEM2), baudrate 115200
#endif
#if ULANDING_VERSION == 1
#define ULANDING_PACKET_HDR 254
#define ULANDING_BUFFER_LENGTH 18
#else
#define ULANDING_PACKET_HDR 72
#define ULANDING_BUFFER_LENGTH 9
#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
class Radar : public cdev::CDev, public px4::ScheduledWorkItem
{
public:
/**
* Default Constructor
* @param port The serial port to open for communicating with the sensor.
* @param rotation The sensor rotation relative to the vehicle body.
*/
Radar(const char *port = RADAR_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
/** Virtual destructor */
virtual ~Radar();
/**
* Method : init()
* This method initializes the general driver for a range finder sensor.
*/
virtual int init() override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
private:
/**
* Reads data from serial UART and places it into a buffer.
*/
int collect();
/**
* Opens and configures the UART serial communications port.
* @param speed The baudrate (speed) to configure the serial UART port.
*/
int open_serial_port(const speed_t speed = B115200);
/**
* Perform a reading cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
/**
* 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();
/**
* Stops the automatic measurement state machine.
*/
void stop();
char _port[20];
int _file_descriptor{-1};
int _orb_class_instance{-1};
unsigned int _head{0};
unsigned int _tail{0};
uint8_t _buffer[ULANDING_BUFFER_LENGTH] {};
uint8_t _rotation;
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "radar_com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "radar_read")};
orb_advert_t _distance_sensor_topic{nullptr};
};
Radar::Radar(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_rotation(rotation)
{
/* store port name */
strncpy(_port, port, sizeof(_port) - 1);
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
}
Radar::~Radar()
{
// Ensure we are truly inactive.
stop();
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int
Radar::collect()
{
perf_begin(_sample_perf);
int bytes_processed = 0;
int distance_cm = -1;
int index = 0;
float distance_m = -1.0f;
bool checksum_passed = false;
// Read from the sensor UART buffer.
int bytes_read = ::read(_file_descriptor, &_buffer[0], sizeof(_buffer));
if (bytes_read > 0) {
index = bytes_read - 6;
while (index >= 0 && !checksum_passed) {
if (_buffer[index] == ULANDING_PACKET_HDR) {
bytes_processed = index;
while (bytes_processed < bytes_read && !checksum_passed) {
if (ULANDING_VERSION == 1) {
uint8_t checksum_value = (_buffer[index + 1] + _buffer[index + 2] + _buffer[index + 3] + _buffer[index + 4]) & 0xFF;
uint8_t checksum_byte = _buffer[index + 5];
if (checksum_value == checksum_byte) {
checksum_passed = true;
distance_cm = (_buffer[index + 3] << 8) | _buffer[index + 2];
distance_m = static_cast<float>(distance_cm) / 100.f;
}
} else {
checksum_passed = true;
distance_cm = (_buffer[index + 1] & 0x7F);
distance_cm += ((_buffer[index + 2] & 0x7F) << 7);
distance_m = static_cast<float>(distance_cm) * 0.045f;
break;
}
bytes_processed++;
}
}
index--;
}
}
if (!checksum_passed) {
return -EAGAIN;
}
distance_m = math::constrain(distance_m, ULANDING_MIN_DISTANCE, ULANDING_MAX_DISTANCE);
distance_sensor_s report;
report.current_distance = distance_m;
report.id = 0; // TODO: set proper ID.
report.max_distance = ULANDING_MAX_DISTANCE;
report.min_distance = ULANDING_MIN_DISTANCE;
report.orientation = _rotation;
report.signal_quality = -1;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR;
report.variance = SENS_VARIANCE;
// Publish the new report.
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
// Notify anyone waiting for data.
poll_notify(POLLIN);
perf_end(_sample_perf);
return PX4_OK;
}
int
Radar::init()
{
// Intitialize the character device.
if (CDev::init() != OK) {
return PX4_ERROR;
}
// Get a publish handle on the range finder topic.
distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
PX4_ERR("failed to create distance_sensor object");
}
start();
return PX4_OK;
}
int
Radar::open_serial_port(const speed_t speed)
{
// File descriptor initialized?
if (_file_descriptor > 0) {
// PX4_INFO("serial port already open");
return PX4_OK;
}
// Configure port flags for read/write, non-controlling, non-blocking.
int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
// Open the serial port.
_file_descriptor = ::open(_port, flags);
if (_file_descriptor < 0) {
PX4_ERR("open failed (%i)", errno);
return PX4_ERROR;
}
if (!isatty(_file_descriptor)) {
PX4_WARN("not a serial device");
return PX4_ERROR;
}
termios uart_config = {};
// Store the current port configuration. attributes.
tcgetattr(_file_descriptor, &uart_config);
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
// 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);
// Set the input baud rate in the uart_config struct.
int termios_state = cfsetispeed(&uart_config, speed);
if (termios_state < 0) {
PX4_ERR("CFG: %d ISPD", termios_state);
::close(_file_descriptor);
return PX4_ERROR;
}
// Set the output baud rate in the uart_config struct.
termios_state = cfsetospeed(&uart_config, speed);
if (termios_state < 0) {
PX4_ERR("CFG: %d OSPD", termios_state);
::close(_file_descriptor);
return PX4_ERROR;
}
// Apply the modified port attributes.
termios_state = tcsetattr(_file_descriptor, TCSANOW, &uart_config);
if (termios_state < 0) {
PX4_ERR("baud %d ATTR", termios_state);
::close(_file_descriptor);
return PX4_ERROR;
}
PX4_INFO("successfully opened UART port %s", _port);
return PX4_OK;
}
void
Radar::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
PX4_INFO("ulanding max distance %.2f m", static_cast<double>(ULANDING_MAX_DISTANCE));
PX4_INFO("ulanding min distance %.2f m", static_cast<double>(ULANDING_MIN_DISTANCE));
PX4_INFO("ulanding update rate: %.2f Hz", static_cast<double>(ULANDING_MEASURE_INTERVAL));
}
void
Radar::Run()
{
// Ensure the serial port is open.
open_serial_port();
collect();
}
void
Radar::start()
{
// Schedule the driver at regular intervals.
ScheduleOnInterval(ULANDING_MEASURE_INTERVAL, 0);
PX4_INFO("driver started");
}
void
Radar::stop()
{
// Ensure the serial port is closed.
::close(_file_descriptor);
// Clear the work queue schedule.
ScheduleClear();
}
namespace radar
{
Radar *g_dev;
int reset(const char *port = RADAR_DEFAULT_PORT);
int start(const char *port = RADAR_DEFAULT_PORT,
const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
int status();
int stop();
int test(const char *port = RADAR_DEFAULT_PORT);
int usage();
/**
* Reset the driver.
*/
int
reset(const char *port)
{
if (stop() == PX4_OK) {
return start(port);
}
return PX4_ERROR;
}
/**
* Start the driver.
*/
int
start(const char *port, const uint8_t rotation)
{
if (g_dev != nullptr) {
PX4_INFO("already started");
return PX4_OK;
}
// Instantiate the driver.
g_dev = new Radar(port, rotation);
if (g_dev == nullptr) {
PX4_ERR("object instantiate failed");
return PX4_ERROR;
}
if (g_dev->init() != PX4_OK) {
PX4_ERR("driver start failed");
delete g_dev;
g_dev = nullptr;
return PX4_ERROR;
}
return PX4_OK;
}
/**
* Print the driver status.
*/
int
status()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return PX4_ERROR;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
return PX4_OK;
}
/**
* Stop the driver
*/
int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return PX4_ERROR;
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
int
test(const char *port)
{
int fd = open(port, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'radar start' if the driver is not running", port);
return PX4_ERROR;
}
// Perform a simple demand read.
distance_sensor_s report;
ssize_t bytes_read = read(fd, &report, sizeof(report));
if (bytes_read != sizeof(report)) {
PX4_ERR("immediate read failed");
return PX4_ERROR;
}
print_message(report);
return PX4_OK;
}
int
usage()
{
PX4_INFO("usage: radar command [options]");
PX4_INFO("command:");
PX4_INFO("\treset|start|status|stop|test");
PX4_INFO("options:");
PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
PX4_INFO("\t-d --device_path");
return PX4_OK;
}
} // namespace radar
/**
* Driver 'main' command.
*/
extern "C" __EXPORT int ulanding_radar_main(int argc, char *argv[])
{
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = RADAR_DEFAULT_PORT;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'd':
device_path = myoptarg;
break;
default:
PX4_WARN("Unknown option!");
return radar::usage();
}
}
if (myoptind >= argc) {
return radar::usage();
}
// Reset the driver.
if (!strcmp(argv[myoptind], "reset")) {
return radar::reset(device_path);
}
// Start/load the driver.
if (!strcmp(argv[myoptind], "start")) {
return radar::start(device_path, rotation);
}
// Print driver information.
if (!strcmp(argv[myoptind], "status")) {
return radar::status();
}
// Stop the driver
if (!strcmp(argv[myoptind], "stop")) {
return radar::stop();
}
// Test the driver/device.
if (!strcmp(argv[myoptind], "test")) {
return radar::test();
}
return radar::usage();
}

226
src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp
Unescape View File

@ -0,0 +1,226 @@ @@ -0,0 +1,226 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 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 "AerotennaULanding.hpp"
AerotennaULanding::AerotennaULanding(const char *port, uint8_t rotation) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_px4_rangefinder(0 /* TODO: device ids */, ORB_PRIO_DEFAULT, rotation)
{
/* store port name */
strncpy(_port, port, sizeof(_port) - 1);
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
_px4_rangefinder.set_min_distance(ULANDING_MIN_DISTANCE);
_px4_rangefinder.set_max_distance(ULANDING_MAX_DISTANCE);
}
AerotennaULanding::~AerotennaULanding()
{
stop();
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int AerotennaULanding::init()
{
start();
return PX4_OK;
}
int AerotennaULanding::collect()
{
perf_begin(_sample_perf);
int bytes_processed = 0;
int distance_cm = -1;
int index = 0;
float distance_m = -1.0f;
bool checksum_passed = false;
// Read from the sensor UART buffer.
const hrt_abstime timestamp_sample = hrt_absolute_time();
int bytes_read = ::read(_file_descriptor, &_buffer[0], sizeof(_buffer));
if (bytes_read > 0) {
index = bytes_read - 6;
while (index >= 0 && !checksum_passed) {
if (_buffer[index] == ULANDING_PACKET_HDR) {
bytes_processed = index;
while (bytes_processed < bytes_read && !checksum_passed) {
if (ULANDING_VERSION == 1) {
uint8_t checksum_value = (_buffer[index + 1] + _buffer[index + 2] + _buffer[index + 3] + _buffer[index + 4]) & 0xFF;
uint8_t checksum_byte = _buffer[index + 5];
if (checksum_value == checksum_byte) {
checksum_passed = true;
distance_cm = (_buffer[index + 3] << 8) | _buffer[index + 2];
distance_m = static_cast<float>(distance_cm) / 100.f;
}
} else {
checksum_passed = true;
distance_cm = (_buffer[index + 1] & 0x7F);
distance_cm += ((_buffer[index + 2] & 0x7F) << 7);
distance_m = static_cast<float>(distance_cm) * 0.045f;
break;
}
bytes_processed++;
}
}
index--;
}
}
if (!checksum_passed) {
return -EAGAIN;
}
_px4_rangefinder.update(timestamp_sample, distance_m);
perf_end(_sample_perf);
return PX4_OK;
}
int AerotennaULanding::open_serial_port(const speed_t speed)
{
// File descriptor initialized?
if (_file_descriptor > 0) {
PX4_DEBUG("serial port already open");
return PX4_OK;
}
// Configure port flags for read/write, non-controlling, non-blocking.
int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
// Open the serial port.
_file_descriptor = ::open(_port, flags);
if (_file_descriptor < 0) {
PX4_ERR("open failed (%i)", errno);
return PX4_ERROR;
}
if (!isatty(_file_descriptor)) {
PX4_WARN("not a serial device");
return PX4_ERROR;
}
termios uart_config{};
// Store the current port configuration. attributes.
tcgetattr(_file_descriptor, &uart_config);
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
// 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);
// Set the input baud rate in the uart_config struct.
int termios_state = cfsetispeed(&uart_config, speed);
if (termios_state < 0) {
PX4_ERR("CFG: %d ISPD", termios_state);
::close(_file_descriptor);
return PX4_ERROR;
}
// Set the output baud rate in the uart_config struct.
termios_state = cfsetospeed(&uart_config, speed);
if (termios_state < 0) {
PX4_ERR("CFG: %d OSPD", termios_state);
::close(_file_descriptor);
return PX4_ERROR;
}
// Apply the modified port attributes.
termios_state = tcsetattr(_file_descriptor, TCSANOW, &uart_config);
if (termios_state < 0) {
PX4_ERR("baud %d ATTR", termios_state);
::close(_file_descriptor);
return PX4_ERROR;
}
PX4_INFO("successfully opened UART port %s", _port);
return PX4_OK;
}
void AerotennaULanding::Run()
{
// Ensure the serial port is open.
open_serial_port();
collect();
}
void AerotennaULanding::start()
{
// Schedule the driver at regular intervals.
ScheduleOnInterval(ULANDING_MEASURE_INTERVAL, 0);
}
void AerotennaULanding::stop()
{
// Ensure the serial port is closed.
::close(_file_descriptor);
// Clear the work queue schedule.
ScheduleClear();
}
void AerotennaULanding::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
_px4_rangefinder.print_status();
}

124
src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp
Unescape View File

@ -0,0 +1,124 @@ @@ -0,0 +1,124 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 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 AerotennaULanding.hpp
* @author Jessica Stockham <jessica@aerotenna.com>
* @author Roman Bapst <roman@uaventure.com>
*
* Driver for the uLanding radar from Aerotenna
*/
#pragma once
#include <fcntl.h>
#include <poll.h>
#include <termios.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/perf/perf_counter.h>
using namespace time_literals;
#define ULANDING_MEASURE_INTERVAL 10_ms
#define ULANDING_MAX_DISTANCE 50.0f
#define ULANDING_MIN_DISTANCE 0.315f
#define ULANDING_VERSION 1
#if ULANDING_VERSION == 1
#define ULANDING_PACKET_HDR 254
#define ULANDING_BUFFER_LENGTH 18
#else
#define ULANDING_PACKET_HDR 72
#define ULANDING_BUFFER_LENGTH 9
#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
class AerotennaULanding : public px4::ScheduledWorkItem
{
public:
/**
* Default Constructor
* @param port The serial port to open for communicating with the sensor.
* @param rotation The sensor rotation relative to the vehicle body.
*/
AerotennaULanding(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
~AerotennaULanding() override;
int init();
void print_info();
private:
/**
* Reads data from serial UART and places it into a buffer.
*/
int collect();
/**
* Opens and configures the UART serial communications port.
* @param speed The baudrate (speed) to configure the serial UART port.
*/
int open_serial_port(const speed_t speed = B115200);
void Run() override;
void start();
void stop();
PX4Rangefinder _px4_rangefinder;
char _port[20] {};
int _file_descriptor{-1};
unsigned int _head{0};
unsigned int _tail{0};
uint8_t _buffer[ULANDING_BUFFER_LENGTH] {};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
};

13
src/drivers/distance_sensor/ulanding/CMakeLists.txt → src/drivers/distance_sensor/ulanding_radar/CMakeLists.txt
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2016 PX4 Development Team. All rights reserved.
# Copyright (c) 2016-2020 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
@ -31,14 +31,15 @@ @@ -31,14 +31,15 @@
#
############################################################################
px4_add_module(
MODULE drivers__ulanding
MODULE drivers__distance_sensor__ulanding_radar
MAIN ulanding_radar
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
ulanding.cpp
AerotennaULanding.cpp
AerotennaULanding.hpp
ulanding_radar_main.cpp
MODULE_CONFIG
module.yaml
DEPENDS
drivers_rangefinder
px4_work_queue
)

1
src/drivers/distance_sensor/ulanding/module.yaml → src/drivers/distance_sensor/ulanding_radar/module.yaml
Unescape View File

@ -4,4 +4,3 @@ serial_config: @@ -4,4 +4,3 @@ serial_config:
port_config_param:
name: SENS_ULAND_CFG
group: Sensors

164
src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp
Unescape View File

@ -0,0 +1,164 @@ @@ -0,0 +1,164 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 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 "AerotennaULanding.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace ulanding_radar
{
AerotennaULanding *g_dev{nullptr};
static int start(const char *port, uint8_t rotation)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
return -1;
}
if (port == nullptr) {
PX4_ERR("serial port required");
return -1;
}
// Instantiate the driver.
g_dev = new AerotennaULanding(port, rotation);
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 Aerotenna uLanding radar.
Setup/usage information: https://docs.px4.io/v1.9.0/en/sensor/ulanding_radar.html
### Examples
Attempt to start driver on a specified serial device.
$ ulanding_radar start -d /dev/ttyS1
Stop driver
$ ulanding_radar stop
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("ulanding_radar", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "Serial device", false);
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
return PX4_OK;
}
} // namespace ulanding_radar
extern "C" __EXPORT int ulanding_radar_main(int argc, char *argv[])
{
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = nullptr;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'd':
device_path = myoptarg;
break;
default:
return ulanding_radar::usage();
}
}
if (myoptind >= argc) {
return ulanding_radar::usage();
}
if (!strcmp(argv[myoptind], "start")) {
return ulanding_radar::start(device_path, rotation);
} else if (!strcmp(argv[myoptind], "stop")) {
return ulanding_radar::stop();
} else if (!strcmp(argv[myoptind], "status")) {
return ulanding_radar::status();
}
return ulanding_radar::usage();
}
Write Preview
Loading…
Cancel
Save