46 #include <semaphore.h> 52 #include <sys/types.h> 60 #include <mathlib/mathlib.h> 62 #include <px4_platform_common/px4_config.h> 63 #include <px4_platform_common/defines.h> 64 #include <px4_platform_common/getopt.h> 65 #include <px4_platform_common/workqueue.h> 66 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 74 #define ULANDING_MEASURE_INTERVAL 10_ms 75 #define ULANDING_MAX_DISTANCE 50.0f 76 #define ULANDING_MIN_DISTANCE 0.315f 77 #define ULANDING_VERSION 1 79 #if defined(__PX4_POSIX_OCPOC) 80 #define RADAR_DEFAULT_PORT "/dev/ttyS6" // Default uLanding port on OCPOC. 82 #define RADAR_DEFAULT_PORT "/dev/ttyS2" // Default serial port on Pixhawk (TELEM2), baudrate 115200 85 #if ULANDING_VERSION == 1 86 #define ULANDING_PACKET_HDR 254 87 #define ULANDING_BUFFER_LENGTH 18 89 #define ULANDING_PACKET_HDR 72 90 #define ULANDING_BUFFER_LENGTH 9 98 #define SENS_VARIANCE 0.045f * 0.045f 118 virtual int init()
override;
136 int open_serial_port(
const speed_t speed = B115200);
158 int _file_descriptor{-1};
159 int _orb_class_instance{-1};
161 unsigned int _head{0};
162 unsigned int _tail{0};
175 ScheduledWorkItem(MODULE_NAME,
px4::serial_port_to_wq(port)),
198 int bytes_processed = 0;
199 int distance_cm = -1;
202 float distance_m = -1.0f;
204 bool checksum_passed =
false;
209 if (bytes_read > 0) {
210 index = bytes_read - 6;
212 while (index >= 0 && !checksum_passed) {
214 bytes_processed = index;
216 while (bytes_processed < bytes_read && !checksum_passed) {
219 uint8_t checksum_byte =
_buffer[index + 5];
221 if (checksum_value == checksum_byte) {
222 checksum_passed =
true;
224 distance_m =
static_cast<float>(distance_cm) / 100.
f;
228 checksum_passed =
true;
229 distance_cm = (
_buffer[index + 1] & 0x7F);
230 distance_cm += ((
_buffer[index + 2] & 0x7F) << 7);
231 distance_m =
static_cast<float>(distance_cm) * 0.045
f;
243 if (!checksum_passed) {
257 report.
type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR;
284 PX4_ERR(
"failed to create distance_sensor object");
301 int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
307 PX4_ERR(
"open failed (%i)", errno);
312 PX4_WARN(
"not a serial device");
316 termios uart_config = {};
321 uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
324 uart_config.c_oflag &= ~ONLCR;
327 uart_config.c_cflag &= ~(CSTOPB | PARENB);
330 uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
333 int termios_state = cfsetispeed(&uart_config, speed);
335 if (termios_state < 0) {
336 PX4_ERR(
"CFG: %d ISPD", termios_state);
342 termios_state = cfsetospeed(&uart_config, speed);
344 if (termios_state < 0) {
345 PX4_ERR(
"CFG: %d OSPD", termios_state);
353 if (termios_state < 0) {
354 PX4_ERR(
"baud %d ATTR", termios_state);
359 PX4_INFO(
"successfully opened UART port %s",
_port);
387 PX4_INFO(
"driver started");
406 const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
418 if (
stop() == PX4_OK) {
429 start(
const char *port,
const uint8_t rotation)
431 if (g_dev !=
nullptr) {
432 PX4_INFO(
"already started");
437 g_dev =
new Radar(port, rotation);
439 if (g_dev ==
nullptr) {
440 PX4_ERR(
"object instantiate failed");
444 if (g_dev->
init() != PX4_OK) {
445 PX4_ERR(
"driver start failed");
460 if (g_dev ==
nullptr) {
461 PX4_ERR(
"driver not running");
465 printf(
"state @ %p\n", g_dev);
476 if (g_dev !=
nullptr) {
492 int fd =
open(port, O_RDONLY);
495 PX4_ERR(
"%s open failed (try 'radar start' if the driver is not running", port);
501 ssize_t bytes_read =
read(fd, &report,
sizeof(report));
503 if (bytes_read !=
sizeof(report)) {
504 PX4_ERR(
"immediate read failed");
508 print_message(report);
515 PX4_INFO(
"usage: radar command [options]");
516 PX4_INFO(
"command:");
517 PX4_INFO(
"\treset|start|status|stop|test");
518 PX4_INFO(
"options:");
519 PX4_INFO(
"\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
520 PX4_INFO(
"\t-d --device_path");
532 uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
536 const char *myoptarg =
nullptr;
538 while ((ch = px4_getopt(argc, argv,
"R:d:", &myoptind, &myoptarg)) != EOF) {
541 rotation = (uint8_t)atoi(myoptarg);
545 device_path = myoptarg;
549 PX4_WARN(
"Unknown option!");
554 if (myoptind >= argc) {
559 if (!strcmp(argv[myoptind],
"reset")) {
564 if (!strcmp(argv[myoptind],
"start")) {
569 if (!strcmp(argv[myoptind],
"status")) {
574 if (!strcmp(argv[myoptind],
"stop")) {
579 if (!strcmp(argv[myoptind],
"test")) {
#define ULANDING_PACKET_HDR
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
void Run() override
Perform a reading cycle; collect from the previous measurement and start a new one.
orb_advert_t _distance_sensor_topic
virtual int open(file_t *filep)
Handle an open of the device.
measure the time elapsed performing an event
__EXPORT int ulanding_radar_main(int argc, char *argv[])
Driver 'main' command.
API for the uORB lightweight object broker.
perf_counter_t _sample_perf
Radar(const char *port=RADAR_DEFAULT_PORT, uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Default Constructor.
int test(const char *port=RADAR_DEFAULT_PORT)
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
#define ULANDING_BUFFER_LENGTH
void usage(const char *reason)
Print the correct usage.
int usage()
Prints info about the driver argument usage.
virtual int init() override
Method : init() This method initializes the general driver for a range finder sensor.
count the number of times an event occurs
virtual ssize_t read(file_t *filep, char *buffer, size_t buflen)
Perform a read from the device.
int open_serial_port(const speed_t speed=B115200)
Opens and configures the UART serial communications port.
High-resolution timer with callouts and timekeeping.
virtual int close(file_t *filep)
Handle a close of the device.
virtual void poll_notify(pollevent_t events)
Report new poll events.
virtual ~Radar()
Virtual destructor.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Abstract class for any character device.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
int stop()
Stop the driver.
#define ULANDING_MIN_DISTANCE
#define ULANDING_MAX_DISTANCE
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
void perf_end(perf_counter_t handle)
End a performance event.
#define SENS_VARIANCE
Assume standard deviation to be equal to sensor resolution.
void start()
Initialise the automatic measurement state machine and start it.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
void print_info()
Diagnostics - print some basic information about the driver.
int reset(const char *port=RADAR_DEFAULT_PORT)
Reset the driver.
perf_counter_t _comms_errors
int start(const char *port=RADAR_DEFAULT_PORT, const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Start the driver.
#define RADAR_DEFAULT_PORT
void stop()
Stops the automatic measurement state machine.
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
#define ULANDING_MEASURE_INTERVAL
uint8_t _buffer[ULANDING_BUFFER_LENGTH]
int status()
Print the driver status.
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
#define RANGE_FINDER0_DEVICE_PATH
int collect()
Reads data from serial UART and places it into a buffer.
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
Performance measuring tools.