forked from Archive/PX4-Autopilot
drivers/distance_sensor: Lightware Lidar SF45/B rotating sensor serial driver (#19891)
Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
This commit is contained in:
Andrew Brahim
GitHub
parent
bde194fb12
commit
184c7fe79d
|
|
@ -0,0 +1,50 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2022-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__distance_sensor__lightware_sf45_serial
|
||||
MAIN lightware_sf45_serial
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
lightware_sf45_serial.cpp
|
||||
lightware_sf45_serial.hpp
|
||||
lightware_sf45_serial_main.cpp
|
||||
DEPENDS
|
||||
drivers_rangefinder
|
||||
px4_work_queue
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
|
||||
if(PX4_TESTING)
|
||||
add_subdirectory(tests)
|
||||
endif()
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_SF45_SERIAL
|
||||
bool "lightware_sf45_serial"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lightware_sf45_serial
|
||||
|
|
@ -0,0 +1,761 @@
|
|||
/**************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022-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 "lightware_sf45_serial.hpp"
|
||||
#include "sf45_commands.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <lib/systemlib/crc.h>
|
||||
|
||||
#include <float.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
/* Configuration Constants */
|
||||
#define SF45_MAX_PAYLOAD 256
|
||||
#define SF45_SCALE_FACTOR 0.01f
|
||||
|
||||
SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
|
||||
_px4_rangefinder(0, rotation),
|
||||
_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';
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
|
||||
|
||||
uint8_t bus_num = atoi(&_port[strlen(_port) - 1]); // Assuming '/dev/ttySx'
|
||||
|
||||
if (bus_num < 10) {
|
||||
device_id.devid_s.bus = bus_num;
|
||||
}
|
||||
|
||||
_num_retries = 2;
|
||||
_px4_rangefinder.set_device_id(device_id.devid);
|
||||
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
|
||||
|
||||
// populate obstacle map members
|
||||
_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
|
||||
_obstacle_map_msg.increment = 5;
|
||||
_obstacle_map_msg.angle_offset = -2.5;
|
||||
_obstacle_map_msg.min_distance = UINT16_MAX;
|
||||
_obstacle_map_msg.max_distance = 5000;
|
||||
|
||||
}
|
||||
|
||||
SF45LaserSerial::~SF45LaserSerial()
|
||||
{
|
||||
stop();
|
||||
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_comms_errors);
|
||||
}
|
||||
|
||||
int SF45LaserSerial::init()
|
||||
{
|
||||
|
||||
param_get(param_find("SF45_UPDATE_CFG"), &_update_rate);
|
||||
param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
|
||||
param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
|
||||
|
||||
/* SF45/B (50M) */
|
||||
_px4_rangefinder.set_min_distance(0.2f);
|
||||
_px4_rangefinder.set_max_distance(50.0f);
|
||||
_interval = 10000;
|
||||
|
||||
start();
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int SF45LaserSerial::measure()
|
||||
{
|
||||
|
||||
int rate = (int)_update_rate;
|
||||
_data_output = 0x101; // raw distance + yaw readings
|
||||
_stream_data = 5; // enable constant streaming
|
||||
|
||||
// send some packets so the sensor starts scanning
|
||||
switch (_sensor_state) {
|
||||
|
||||
// sensor should now respond
|
||||
case 0:
|
||||
while (_num_retries--) {
|
||||
sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
|
||||
_sensor_state = 0;
|
||||
}
|
||||
|
||||
_sensor_state = 1;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
// Update rate default to 50 readings/s
|
||||
sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t));
|
||||
_sensor_state = 2;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output));
|
||||
_sensor_state = 3;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data));
|
||||
_sensor_state = 4;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int SF45LaserSerial::collect()
|
||||
{
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
/* clear buffer if last read was too long ago */
|
||||
int64_t read_elapsed = hrt_elapsed_time(&_last_read);
|
||||
int ret;
|
||||
/* the buffer for read chars is buflen minus null termination */
|
||||
param_get(param_find("SF45_CP_LIMIT"), &_collision_constraint);
|
||||
uint8_t readbuf[SF45_MAX_PAYLOAD];
|
||||
|
||||
float distance_m = -1.0f;
|
||||
|
||||
/* read from the sensor (uart buffer) */
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
if (_sensor_state == 1) {
|
||||
|
||||
ret = ::read(_fd, &readbuf[0], 22);
|
||||
sf45_request_handle(ret, readbuf);
|
||||
ScheduleDelayed(_interval * 2);
|
||||
|
||||
} else if (_sensor_state == 2) {
|
||||
|
||||
ret = ::read(_fd, &readbuf[0], 7);
|
||||
|
||||
if (readbuf[3] == SF_UPDATE_RATE) {
|
||||
sf45_request_handle(ret, readbuf);
|
||||
ScheduleDelayed(_interval * 5);
|
||||
}
|
||||
|
||||
} else if (_sensor_state == 3) {
|
||||
|
||||
ret = ::read(_fd, &readbuf[0], 8);
|
||||
|
||||
if (readbuf[3] == SF_DISTANCE_OUTPUT) {
|
||||
sf45_request_handle(ret, readbuf);
|
||||
ScheduleDelayed(_interval * 5);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
ret = ::read(_fd, &readbuf[0], 10);
|
||||
uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2);
|
||||
|
||||
if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
|
||||
|
||||
for (uint8_t i = 0; i < ret; ++i) {
|
||||
sf45_request_handle(ret, readbuf);
|
||||
}
|
||||
|
||||
if (_init_complete) {
|
||||
sf45_process_replies(&distance_m);
|
||||
} // end if
|
||||
|
||||
} else {
|
||||
|
||||
ret = ::read(_fd, &readbuf[0], 10);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_DEBUG("read err: %d", ret);
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
|
||||
/* only throw an error if we time out */
|
||||
if (read_elapsed > (_interval * 2)) {
|
||||
PX4_DEBUG("Timing out...");
|
||||
return ret;
|
||||
|
||||
} else {
|
||||
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
} else if (ret == 0) {
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
_last_read = hrt_absolute_time();
|
||||
|
||||
if (!_crc_valid) {
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO"));
|
||||
_px4_rangefinder.update(timestamp_sample, distance_m);
|
||||
|
||||
perf_end(_sample_perf);
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void SF45LaserSerial::start()
|
||||
{
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
void SF45LaserSerial::stop()
|
||||
{
|
||||
ScheduleClear();
|
||||
}
|
||||
|
||||
void SF45LaserSerial::Run()
|
||||
{
|
||||
/* fds initialized? */
|
||||
if (_fd < 0) {
|
||||
/* open fd: non-blocking read mode*/
|
||||
|
||||
_fd = ::open(_port, O_RDWR | O_NOCTTY);
|
||||
|
||||
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);
|
||||
|
||||
uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8;
|
||||
|
||||
uart_config.c_cflag |= (CLOCAL | CREAD);
|
||||
|
||||
// no parity, 1 stop bit, flow control disabled
|
||||
uart_config.c_cflag &= ~(PARENB | PARODD);
|
||||
|
||||
uart_config.c_cflag |= 0;
|
||||
|
||||
uart_config.c_cflag &= ~CSTOPB;
|
||||
|
||||
uart_config.c_cflag &= ~CRTSCTS;
|
||||
|
||||
uart_config.c_iflag &= ~IGNBRK;
|
||||
|
||||
uart_config.c_iflag &= ~ICRNL;
|
||||
|
||||
uart_config.c_iflag &= ~(IXON | IXOFF | IXANY);
|
||||
|
||||
// echo and echo NL off, canonical mode off (raw mode)
|
||||
// extended input processing off, signal chars off
|
||||
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
|
||||
|
||||
uart_config.c_oflag = 0;
|
||||
|
||||
uart_config.c_cc[VMIN] = 0;
|
||||
|
||||
uart_config.c_cc[VTIME] = 1;
|
||||
|
||||
unsigned speed = B921600;
|
||||
|
||||
/* 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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
int collect_ret = collect();
|
||||
|
||||
if (collect_ret == -EAGAIN) {
|
||||
/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
|
||||
ScheduleDelayed(1042 * 8);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (OK != collect_ret) {
|
||||
/* we know the sensor needs about four seconds to initialize */
|
||||
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
|
||||
PX4_ERR("collection error #%u", _consecutive_fail_count);
|
||||
}
|
||||
|
||||
_consecutive_fail_count++;
|
||||
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
|
||||
} else {
|
||||
/* apparently success */
|
||||
_consecutive_fail_count = 0;
|
||||
}
|
||||
|
||||
/* next phase is measurement */
|
||||
_collect_phase = false;
|
||||
}
|
||||
|
||||
/* measurement phase */
|
||||
|
||||
if (OK != measure()) {
|
||||
PX4_DEBUG("measure error");
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
ScheduleDelayed(_interval);
|
||||
|
||||
}
|
||||
|
||||
void SF45LaserSerial::print_info()
|
||||
{
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
}
|
||||
|
||||
void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
|
||||
{
|
||||
|
||||
// SF45 protocol
|
||||
// Start byte is 0xAA and is the start of packet
|
||||
// Payload length sanity check (0-1023) bytes
|
||||
// and represented by 16-bit integer (payload +
|
||||
// read/write status)
|
||||
// ID byte precedes the data in the payload
|
||||
// CRC comprised of 16-bit checksum (not included in checksum calc.)
|
||||
|
||||
uint16_t recv_crc = 0;
|
||||
bool restart_flag = false;
|
||||
|
||||
while (restart_flag != true) {
|
||||
|
||||
switch (_parsed_state) {
|
||||
case 0: {
|
||||
if (input_buf[0] == 0xAA) {
|
||||
// start of frame is valid, continue
|
||||
_sop_valid = true;
|
||||
_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
|
||||
_parsed_state = 1;
|
||||
break;
|
||||
|
||||
} else {
|
||||
_sop_valid = false;
|
||||
_crc_valid = false;
|
||||
_parsed_state = 0;
|
||||
restart_flag = true;
|
||||
_calc_crc = 0;
|
||||
PX4_INFO("INFO: start of packet not valid");
|
||||
break;
|
||||
} // end else
|
||||
} // end case 0
|
||||
|
||||
case 1: {
|
||||
rx_field.flags_lo = input_buf[1];
|
||||
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
|
||||
_parsed_state = 2;
|
||||
break;
|
||||
}
|
||||
|
||||
case 2: {
|
||||
rx_field.flags_hi = input_buf[2];
|
||||
rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6);
|
||||
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi);
|
||||
|
||||
// Check payload length against known max value
|
||||
if (rx_field.data_len > 17) {
|
||||
PX4_INFO("INFO: payload length invalid, restarting data request");
|
||||
_parsed_state = 0;
|
||||
restart_flag = true;
|
||||
_calc_crc = 0;
|
||||
break;
|
||||
|
||||
} else {
|
||||
_parsed_state = 3;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
case 3: {
|
||||
|
||||
rx_field.msg_id = input_buf[3];
|
||||
|
||||
if (rx_field.msg_id == SF_PRODUCT_NAME || rx_field.msg_id == SF_UPDATE_RATE || rx_field.msg_id == SF_DISTANCE_OUTPUT
|
||||
|| rx_field.msg_id == SF_STREAM || rx_field.msg_id == SF_DISTANCE_DATA_CM) {
|
||||
|
||||
if (rx_field.msg_id == SF_DISTANCE_DATA_CM && rx_field.data_len > 1) {
|
||||
_sensor_ready = true;
|
||||
|
||||
} else {
|
||||
_sensor_ready = false;
|
||||
}
|
||||
|
||||
_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
|
||||
|
||||
_parsed_state = 4;
|
||||
break;
|
||||
}
|
||||
|
||||
// Ignore message ID's that aren't defined
|
||||
else {
|
||||
_parsed_state = 0;
|
||||
_calc_crc = 0;
|
||||
restart_flag = true;
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// Data
|
||||
case 4: {
|
||||
// Process commands with & w/out data bytes
|
||||
if (rx_field.data_len > 1) {
|
||||
for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) {
|
||||
|
||||
rx_field.data[_data_bytes_recv] = input_buf[i];
|
||||
_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
|
||||
_data_bytes_recv = _data_bytes_recv + 1;
|
||||
|
||||
} // end for
|
||||
} //end if
|
||||
|
||||
else {
|
||||
|
||||
_parsed_state = 5;
|
||||
_data_bytes_recv = 0;
|
||||
_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
|
||||
|
||||
}
|
||||
|
||||
_parsed_state = 5;
|
||||
_data_bytes_recv = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
// CRC low byte
|
||||
case 5: {
|
||||
rx_field.crc[0] = input_buf[3 + rx_field.data_len];
|
||||
_parsed_state = 6;
|
||||
break;
|
||||
}
|
||||
|
||||
// CRC high byte
|
||||
case 6: {
|
||||
rx_field.crc[1] = input_buf[4 + rx_field.data_len];
|
||||
recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
|
||||
|
||||
// Check the received crc bytes from the sf45 against our own CRC calcuation
|
||||
// If it matches, we can check if sensor ready
|
||||
// Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete
|
||||
if (recv_crc == _calc_crc) {
|
||||
_crc_valid = true;
|
||||
|
||||
// Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM
|
||||
if (_sensor_ready) {
|
||||
_init_complete = true;
|
||||
|
||||
} else {
|
||||
_init_complete = false;
|
||||
}
|
||||
|
||||
_parsed_state = 0;
|
||||
_calc_crc = 0;
|
||||
restart_flag = true;
|
||||
break;
|
||||
|
||||
} else {
|
||||
PX4_INFO("INFO: CRC mismatch");
|
||||
_crc_valid = false;
|
||||
_init_complete = false;
|
||||
_parsed_state = 0;
|
||||
_calc_crc = 0;
|
||||
restart_flag = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} // end switch
|
||||
} //end while
|
||||
}
|
||||
|
||||
void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len)
|
||||
{
|
||||
uint16_t crc_val = 0;
|
||||
uint8_t packet_buff[SF45_MAX_PAYLOAD];
|
||||
uint8_t data_inc = 4;
|
||||
int ret = 0;
|
||||
uint8_t packet_len = 0;
|
||||
// Include payload ID byte in payload len
|
||||
uint16_t flags = (data_len + 1) << 6;
|
||||
|
||||
// If writing to the device, lsb is 1
|
||||
if (write) {
|
||||
flags |= 0x01;
|
||||
}
|
||||
|
||||
else {
|
||||
flags |= 0x0;
|
||||
}
|
||||
|
||||
uint8_t flags_lo = flags & 0xFF;
|
||||
uint8_t flags_hi = (flags >> 8) & 0xFF;
|
||||
|
||||
// Add packet bytes to format into crc based on CRC-16-CCITT 0x1021/XMODEM
|
||||
crc_val = sf45_format_crc(crc_val, _start_of_frame);
|
||||
crc_val = sf45_format_crc(crc_val, flags_lo);
|
||||
crc_val = sf45_format_crc(crc_val, flags_hi);
|
||||
crc_val = sf45_format_crc(crc_val, msg_id);
|
||||
|
||||
// Write the packet header contents + payload msg ID to the output buffer
|
||||
packet_buff[0] = _start_of_frame;
|
||||
packet_buff[1] = flags_lo;
|
||||
packet_buff[2] = flags_hi;
|
||||
packet_buff[3] = msg_id;
|
||||
|
||||
if (msg_id == SF_DISTANCE_OUTPUT) {
|
||||
uint8_t data_convert = data[0] & 0x00FF;
|
||||
// write data bytes to the output buffer
|
||||
packet_buff[data_inc] = data_convert;
|
||||
// Add data bytes to crc add function
|
||||
crc_val = sf45_format_crc(crc_val, data_convert);
|
||||
data_inc = data_inc + 1;
|
||||
data_convert = data[0] >> 8;
|
||||
packet_buff[data_inc] = data_convert;
|
||||
crc_val = sf45_format_crc(crc_val, data_convert);
|
||||
data_inc = data_inc + 1;
|
||||
packet_buff[data_inc] = 0;
|
||||
crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
|
||||
data_inc = data_inc + 1;
|
||||
packet_buff[data_inc] = 0;
|
||||
crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
|
||||
data_inc = data_inc + 1;
|
||||
}
|
||||
|
||||
else if (msg_id == SF_STREAM) {
|
||||
packet_buff[data_inc] = data[0];
|
||||
//pad zeroes
|
||||
crc_val = sf45_format_crc(crc_val, data[0]);
|
||||
data_inc = data_inc + 1;
|
||||
packet_buff[data_inc] = 0;
|
||||
crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
|
||||
data_inc = data_inc + 1;
|
||||
packet_buff[data_inc] = 0;
|
||||
crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
|
||||
data_inc = data_inc + 1;
|
||||
packet_buff[data_inc] = 0;
|
||||
crc_val = sf45_format_crc(crc_val, packet_buff[data_inc]);
|
||||
data_inc = data_inc + 1;
|
||||
}
|
||||
|
||||
else if (msg_id == SF_UPDATE_RATE) {
|
||||
// Update Rate
|
||||
packet_buff[data_inc] = (uint8_t)data[0];
|
||||
// Add data bytes to crc add function
|
||||
crc_val = sf45_format_crc(crc_val, data[0]);
|
||||
data_inc = data_inc + 1;
|
||||
|
||||
}
|
||||
|
||||
else {
|
||||
// Product Name
|
||||
PX4_INFO("INFO: Product name");
|
||||
}
|
||||
|
||||
uint8_t crc_lo = crc_val & 0xFF;
|
||||
uint8_t crc_hi = (crc_val >> 8) & 0xFF;
|
||||
|
||||
packet_buff[data_inc] = crc_lo;
|
||||
data_inc = data_inc + 1;
|
||||
packet_buff[data_inc] = crc_hi;
|
||||
|
||||
size_t len = sizeof(packet_buff[0]) * (data_inc + 1);
|
||||
packet_len = (uint8_t)len;
|
||||
|
||||
// DEBUG
|
||||
for (uint8_t i = 0; i < packet_len; ++i) {
|
||||
PX4_INFO("INFO: Send byte: %d", packet_buff[i]);
|
||||
}
|
||||
|
||||
ret = ::write(_fd, packet_buff, packet_len);
|
||||
|
||||
if (ret != packet_len) {
|
||||
perf_count(_comms_errors);
|
||||
PX4_DEBUG("write fail %d", ret);
|
||||
//return ret;
|
||||
}
|
||||
}
|
||||
|
||||
void SF45LaserSerial::sf45_process_replies(float *distance_m)
|
||||
{
|
||||
|
||||
switch (rx_field.msg_id) {
|
||||
case SF_DISTANCE_DATA_CM: {
|
||||
|
||||
uint16_t obstacle_dist_cm = 0;
|
||||
const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
|
||||
int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
|
||||
int16_t scaled_yaw = 0;
|
||||
|
||||
// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
|
||||
if (raw_yaw > 32000) {
|
||||
raw_yaw = raw_yaw - 65535;
|
||||
|
||||
}
|
||||
|
||||
// The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle
|
||||
if (_orient_cfg == 1) {
|
||||
raw_yaw = raw_yaw * -1;
|
||||
}
|
||||
|
||||
switch (_yaw_cfg) {
|
||||
case 0:
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (raw_yaw > 180) {
|
||||
raw_yaw = raw_yaw - 180;
|
||||
|
||||
} else {
|
||||
raw_yaw = raw_yaw + 180; // rotation facing aft
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 2:
|
||||
raw_yaw = raw_yaw + 90; // rotation facing right
|
||||
break;
|
||||
|
||||
case 3:
|
||||
raw_yaw = raw_yaw - 90; // rotation facing left
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
|
||||
|
||||
// Convert to meters for rangefinder update
|
||||
*distance_m = raw_distance * SF45_SCALE_FACTOR;
|
||||
obstacle_dist_cm = (uint16_t)raw_distance;
|
||||
|
||||
uint8_t current_bin = sf45_convert_angle(scaled_yaw);
|
||||
|
||||
// If we have moved to a new bin
|
||||
|
||||
if (current_bin != _previous_bin) {
|
||||
|
||||
// update the current bin to the distance sensor reading
|
||||
// readings in cm
|
||||
_obstacle_map_msg.distances[current_bin] = obstacle_dist_cm;
|
||||
_obstacle_map_msg.timestamp = hrt_absolute_time();
|
||||
|
||||
}
|
||||
|
||||
_previous_bin = current_bin;
|
||||
|
||||
_obstacle_distance_pub.publish(_obstacle_map_msg);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
// add case for future use
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
|
||||
{
|
||||
|
||||
uint8_t mapped_sector = 0;
|
||||
float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset);
|
||||
mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment);
|
||||
|
||||
return mapped_sector;
|
||||
}
|
||||
|
||||
float SF45LaserSerial::sf45_wrap_360(float f)
|
||||
{
|
||||
return matrix::wrap(f, 0.f, 360.f);
|
||||
}
|
||||
|
||||
uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val)
|
||||
{
|
||||
uint32_t i;
|
||||
const uint16_t poly = 0x1021u;
|
||||
crc ^= (uint16_t)((uint16_t) data_val << 8u);
|
||||
|
||||
for (i = 0; i < 8; i++) {
|
||||
if (crc & (1u << 15u)) {
|
||||
crc = (uint16_t)((crc << 1u) ^ poly);
|
||||
|
||||
} else {
|
||||
crc = (uint16_t)(crc << 1u);
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022-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 lightware_sf45_serial.hpp
|
||||
* @author Andrew Brahim <dirksavage88@gmail.com>
|
||||
*
|
||||
* Serial Protocol driver for the Lightware SF45/B rangefinder series
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/obstacle_distance.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
|
||||
#include "sf45_commands.h"
|
||||
class SF45LaserSerial : public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
||||
~SF45LaserSerial() override;
|
||||
|
||||
int init();
|
||||
void print_info();
|
||||
void sf45_request_handle(int val, uint8_t *value);
|
||||
void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len);
|
||||
uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value);
|
||||
void sf45_process_replies(float *data);
|
||||
uint8_t sf45_convert_angle(const int16_t yaw);
|
||||
float sf45_wrap_360(float f);
|
||||
protected:
|
||||
obstacle_distance_s _obstacle_map_msg{};
|
||||
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */
|
||||
|
||||
private:
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
void Run() override;
|
||||
int measure();
|
||||
int collect();
|
||||
bool _crc_valid{false};
|
||||
PX4Rangefinder _px4_rangefinder;
|
||||
|
||||
char _port[20] {};
|
||||
int _interval{10000};
|
||||
bool _collect_phase{false};
|
||||
int _fd{-1};
|
||||
int _linebuf[256] {};
|
||||
unsigned _linebuf_index{0};
|
||||
hrt_abstime _last_read{0};
|
||||
|
||||
// SF45/B uses a binary protocol to include header,flags
|
||||
// message ID, payload, and checksum
|
||||
bool _is_sf45{false};
|
||||
bool _init_complete{false};
|
||||
bool _sensor_ready{false};
|
||||
uint8_t _sensor_state{0};
|
||||
int _baud_rate{0};
|
||||
int _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
int _stream_data{0};
|
||||
int32_t _update_rate{1};
|
||||
int _data_output{0};
|
||||
const uint8_t _start_of_frame{0xAA};
|
||||
uint16_t _data_bytes_recv{0};
|
||||
uint8_t _parsed_state{0};
|
||||
bool _sop_valid{false};
|
||||
uint16_t _calc_crc{0};
|
||||
uint8_t _num_retries{2};
|
||||
int32_t _yaw_cfg{0};
|
||||
int32_t _orient_cfg{0};
|
||||
int32_t _collision_constraint{0};
|
||||
uint16_t _previous_bin{0};
|
||||
|
||||
// end of SF45/B data members
|
||||
|
||||
unsigned _consecutive_fail_count;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
|
||||
};
|
||||
167
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp
Executable file
167
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp
Executable file
|
|
@ -0,0 +1,167 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022-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 "lightware_sf45_serial.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
namespace lightware_sf45
|
||||
{
|
||||
|
||||
SF45LaserSerial *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("no device specified");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new SF45LaserSerial(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 Lightware SF45/b Laser rangefinder.
|
||||
|
||||
Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
|
||||
|
||||
### Examples
|
||||
|
||||
Attempt to start driver on a specified serial device.
|
||||
$ lightware_sf45_serial start -d /dev/ttyS1
|
||||
Stop driver
|
||||
$ lightware_sf45_serial stop
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("lightware_sf45_serial", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false);
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
|
||||
{
|
||||
uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_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:
|
||||
lightware_sf45::usage();
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (myoptind >= argc) {
|
||||
lightware_sf45::usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[myoptind], "start")) {
|
||||
return lightware_sf45::start(device_path, rotation);
|
||||
|
||||
} else if (!strcmp(argv[myoptind], "stop")) {
|
||||
return lightware_sf45::stop();
|
||||
|
||||
} else if (!strcmp(argv[myoptind], "status")) {
|
||||
return lightware_sf45::status();
|
||||
}
|
||||
|
||||
lightware_sf45::usage();
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
module_name: Lightware SF45 Rangefinder (serial)
|
||||
serial_config:
|
||||
- command: lightware_sf45_serial start -R 0 -d ${SERIAL_DEV}
|
||||
port_config_param:
|
||||
name: SENS_EN_SF45_CFG
|
||||
group: Sensors
|
||||
default: TEL2
|
||||
num_instances: 1
|
||||
supports_networking: false
|
||||
|
||||
parameters:
|
||||
- group: Sensors
|
||||
definitions:
|
||||
SF45_UPDATE_CFG:
|
||||
description:
|
||||
short: Update rate in Hz
|
||||
long: |
|
||||
The SF45 sets the update rate in Hz to allow greater resolution
|
||||
type: enum
|
||||
values:
|
||||
1: 50hz
|
||||
2: 100hz
|
||||
3: 200hz
|
||||
4: 400hz
|
||||
5: 500hz
|
||||
6: 625hz
|
||||
7: 1000hz
|
||||
8: 1250hz
|
||||
9: 1538hz
|
||||
10: 2000hz
|
||||
11: 2500hz
|
||||
12: 5000hz
|
||||
reboot_required: true
|
||||
num_instances: 1
|
||||
default: 1
|
||||
|
||||
SF45_ORIENT_CFG:
|
||||
description:
|
||||
short: Orientation upright or facing downward
|
||||
long: |
|
||||
The SF45 mounted facing upward or downward on the frame
|
||||
type: enum
|
||||
values:
|
||||
0: Rotation upward
|
||||
1: Rotation downward
|
||||
reboot_required: true
|
||||
num_instances: 1
|
||||
default: 0
|
||||
|
||||
SF45_YAW_CFG:
|
||||
description:
|
||||
short: Sensor facing forward or backward
|
||||
long: |
|
||||
The usb port on the sensor indicates 180deg, opposite usb is forward facing
|
||||
type: enum
|
||||
values:
|
||||
0: Rotation forward
|
||||
1: Rotation backward
|
||||
2: Rotation right
|
||||
3: Rotation left
|
||||
reboot_required: true
|
||||
num_instances: 1
|
||||
default: 0
|
||||
|
|
@ -0,0 +1,89 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022-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 sf45_commands.h
|
||||
* @author Andrew Brahim
|
||||
*
|
||||
* Declarations of sf45 serial commands for the Lightware sf45/b series
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#define SF45_MAX_PAYLOAD 256
|
||||
#define SF45_CRC_FIELDS 2
|
||||
|
||||
enum SF_SERIAL_CMD {
|
||||
SF_PRODUCT_NAME = 0,
|
||||
SF_HARDWARE_VERSION = 1,
|
||||
SF_FIRMWARE_VERSION = 2,
|
||||
SF_SERIAL_NUMBER = 3,
|
||||
SF_TEXT_MESSAGE = 7,
|
||||
SF_USER_DATA = 9,
|
||||
SF_TOKEN = 10,
|
||||
SF_SAVE_PARAMETERS = 12,
|
||||
SF_RESET = 14,
|
||||
SF_STAGE_FIRMWARE = 16,
|
||||
SF_COMMIT_FIRMWARE = 17,
|
||||
SF_DISTANCE_OUTPUT = 27,
|
||||
SF_STREAM = 30,
|
||||
SF_DISTANCE_DATA_CM = 44,
|
||||
SF_DISTANCE_DATA_MM = 45,
|
||||
SF_LASER_FIRING = 50,
|
||||
SF_TEMPERATURE = 57,
|
||||
SF_UPDATE_RATE = 66,
|
||||
SF_NOISE = 74,
|
||||
SF_ZERO_OFFSET = 75,
|
||||
SF_LOST_SIGNAL_COUNTER = 76,
|
||||
SF_BAUD_RATE = 79,
|
||||
SF_I2C_ADDRESS = 80,
|
||||
SF_SCAN_SPEED = 85,
|
||||
SF_STEPPER_STATUS = 93,
|
||||
SF_SCAN_ON_STARTUP = 94,
|
||||
SF_SCAN_ENABLE = 96,
|
||||
SF_SCAN_POSITION = 97,
|
||||
SF_SCAN_LOW_ANGLE = 98,
|
||||
SF_HIGH_ANGLE = 99
|
||||
};
|
||||
|
||||
// Store contents of rx'd frame
|
||||
struct {
|
||||
const uint8_t data_fields = 2; // useful for breaking crc's into byte separated fields
|
||||
uint16_t data_len{0}; // last message payload length (1+ bytes in payload)
|
||||
uint8_t data[SF45_MAX_PAYLOAD]; // payload size limited by posix serial
|
||||
uint8_t msg_id{0}; // latest message's message id
|
||||
uint8_t flags_lo{0}; // flags low byte
|
||||
uint8_t flags_hi{0}; // flags high byte
|
||||
uint16_t crc[SF45_CRC_FIELDS] = {0, 0};
|
||||
uint8_t crc_lo{0}; // crc low byte
|
||||
uint8_t crc_hi{0}; // crc high byte
|
||||
} rx_field;
|
||||
Loading…
Reference in New Issue