mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 01:12:11 +00:00
distance_sensor/sf0x: rename to lightware_laser_serial
This commit is contained in:
@@ -28,7 +28,7 @@ px4_add_board(
|
|||||||
differential_pressure/ms4525
|
differential_pressure/ms4525
|
||||||
#distance_sensor # all available distance sensor drivers
|
#distance_sensor # all available distance sensor drivers
|
||||||
#distance_sensor/ll40ls
|
#distance_sensor/ll40ls
|
||||||
#distance_sensor/sf0x
|
#distance_sensor/lightware_laser_serial
|
||||||
#dshot
|
#dshot
|
||||||
gps
|
gps
|
||||||
#heater
|
#heater
|
||||||
|
|||||||
@@ -26,7 +26,7 @@ px4_add_board(
|
|||||||
differential_pressure # all available differential pressure drivers
|
differential_pressure # all available differential pressure drivers
|
||||||
#distance_sensor # all available distance sensor drivers
|
#distance_sensor # all available distance sensor drivers
|
||||||
distance_sensor/ll40ls
|
distance_sensor/ll40ls
|
||||||
distance_sensor/sf0x
|
distance_sensor/lightware_laser_serial
|
||||||
gps
|
gps
|
||||||
imu/l3gd20
|
imu/l3gd20
|
||||||
imu/lsm303d
|
imu/lsm303d
|
||||||
|
|||||||
@@ -27,7 +27,7 @@ px4_add_board(
|
|||||||
differential_pressure/ms4525
|
differential_pressure/ms4525
|
||||||
#distance_sensor # all available distance sensor drivers
|
#distance_sensor # all available distance sensor drivers
|
||||||
distance_sensor/ll40ls
|
distance_sensor/ll40ls
|
||||||
distance_sensor/sf0x
|
distance_sensor/lightware_laser_serial
|
||||||
#dshot
|
#dshot
|
||||||
gps
|
gps
|
||||||
#heater
|
#heater
|
||||||
|
|||||||
@@ -33,7 +33,7 @@ set(tests
|
|||||||
rc
|
rc
|
||||||
search_min
|
search_min
|
||||||
servo
|
servo
|
||||||
#sf0x
|
#lightware_laser
|
||||||
sleep
|
sleep
|
||||||
uorb
|
uorb
|
||||||
versioning
|
versioning
|
||||||
|
|||||||
@@ -38,8 +38,8 @@ add_subdirectory(ll40ls_pwm)
|
|||||||
add_subdirectory(mappydot)
|
add_subdirectory(mappydot)
|
||||||
add_subdirectory(mb12xx)
|
add_subdirectory(mb12xx)
|
||||||
add_subdirectory(pga460)
|
add_subdirectory(pga460)
|
||||||
add_subdirectory(sf0x)
|
|
||||||
add_subdirectory(lightware_laser_i2c)
|
add_subdirectory(lightware_laser_i2c)
|
||||||
|
add_subdirectory(lightware_laser_serial)
|
||||||
add_subdirectory(srf02)
|
add_subdirectory(srf02)
|
||||||
add_subdirectory(teraranger)
|
add_subdirectory(teraranger)
|
||||||
add_subdirectory(tfmini)
|
add_subdirectory(tfmini)
|
||||||
|
|||||||
@@ -31,15 +31,15 @@
|
|||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE drivers__sf0x
|
MODULE drivers__distance_sensor__lightware_laser_serial
|
||||||
MAIN sf0x
|
MAIN lightware_laser_serial
|
||||||
COMPILE_FLAGS
|
COMPILE_FLAGS
|
||||||
-Wno-cast-align # TODO: fix and enable
|
-Wno-cast-align # TODO: fix and enable
|
||||||
SRCS
|
SRCS
|
||||||
SF0X.cpp
|
lightware_laser_serial.cpp
|
||||||
SF0X.hpp
|
lightware_laser_serial.hpp
|
||||||
sf0x_main.cpp
|
lightware_laser_serial_main.cpp
|
||||||
sf0x_parser.cpp
|
parser.cpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
drivers_rangefinder
|
drivers_rangefinder
|
||||||
px4_work_queue
|
px4_work_queue
|
||||||
@@ -48,5 +48,5 @@ px4_add_module(
|
|||||||
)
|
)
|
||||||
|
|
||||||
if(PX4_TESTING)
|
if(PX4_TESTING)
|
||||||
add_subdirectory(sf0x_tests)
|
add_subdirectory(tests)
|
||||||
endif()
|
endif()
|
||||||
@@ -31,15 +31,15 @@
|
|||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
#include "SF0X.hpp"
|
#include "lightware_laser_serial.hpp"
|
||||||
|
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <termios.h>
|
#include <termios.h>
|
||||||
|
|
||||||
/* Configuration Constants */
|
/* Configuration Constants */
|
||||||
#define SF0X_TAKE_RANGE_REG 'd'
|
#define LW_TAKE_RANGE_REG 'd'
|
||||||
|
|
||||||
SF0X::SF0X(const char *port, uint8_t rotation) :
|
LightwareLaserSerial::LightwareLaserSerial(const char *port, uint8_t rotation) :
|
||||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
|
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
|
||||||
_px4_rangefinder(0 /* device id not yet used */, rotation),
|
_px4_rangefinder(0 /* device id not yet used */, rotation),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
|
||||||
@@ -52,7 +52,7 @@ SF0X::SF0X(const char *port, uint8_t rotation) :
|
|||||||
_port[sizeof(_port) - 1] = '\0';
|
_port[sizeof(_port) - 1] = '\0';
|
||||||
}
|
}
|
||||||
|
|
||||||
SF0X::~SF0X()
|
LightwareLaserSerial::~LightwareLaserSerial()
|
||||||
{
|
{
|
||||||
stop();
|
stop();
|
||||||
|
|
||||||
@@ -61,7 +61,7 @@ SF0X::~SF0X()
|
|||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
SF0X::init()
|
LightwareLaserSerial::init()
|
||||||
{
|
{
|
||||||
int32_t hw_model = 0;
|
int32_t hw_model = 0;
|
||||||
param_get(param_find("SENS_EN_SF0X"), &hw_model);
|
param_get(param_find("SENS_EN_SF0X"), &hw_model);
|
||||||
@@ -108,10 +108,10 @@ SF0X::init()
|
|||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
int SF0X::measure()
|
int LightwareLaserSerial::measure()
|
||||||
{
|
{
|
||||||
// Send the command to begin a measurement.
|
// Send the command to begin a measurement.
|
||||||
char cmd = SF0X_TAKE_RANGE_REG;
|
char cmd = LW_TAKE_RANGE_REG;
|
||||||
int ret = ::write(_fd, &cmd, 1);
|
int ret = ::write(_fd, &cmd, 1);
|
||||||
|
|
||||||
if (ret != sizeof(cmd)) {
|
if (ret != sizeof(cmd)) {
|
||||||
@@ -123,7 +123,7 @@ int SF0X::measure()
|
|||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
int SF0X::collect()
|
int LightwareLaserSerial::collect()
|
||||||
{
|
{
|
||||||
perf_begin(_sample_perf);
|
perf_begin(_sample_perf);
|
||||||
|
|
||||||
@@ -161,7 +161,7 @@ int SF0X::collect()
|
|||||||
bool valid = false;
|
bool valid = false;
|
||||||
|
|
||||||
for (int i = 0; i < ret; i++) {
|
for (int i = 0; i < ret; i++) {
|
||||||
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
|
if (OK == lightware_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
|
||||||
valid = true;
|
valid = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -179,7 +179,7 @@ int SF0X::collect()
|
|||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SF0X::start()
|
void LightwareLaserSerial::start()
|
||||||
{
|
{
|
||||||
/* reset the report ring and state machine */
|
/* reset the report ring and state machine */
|
||||||
_collect_phase = false;
|
_collect_phase = false;
|
||||||
@@ -188,12 +188,12 @@ void SF0X::start()
|
|||||||
ScheduleNow();
|
ScheduleNow();
|
||||||
}
|
}
|
||||||
|
|
||||||
void SF0X::stop()
|
void LightwareLaserSerial::stop()
|
||||||
{
|
{
|
||||||
ScheduleClear();
|
ScheduleClear();
|
||||||
}
|
}
|
||||||
|
|
||||||
void SF0X::Run()
|
void LightwareLaserSerial::Run()
|
||||||
{
|
{
|
||||||
/* fds initialized? */
|
/* fds initialized? */
|
||||||
if (_fd < 0) {
|
if (_fd < 0) {
|
||||||
@@ -293,7 +293,7 @@ void SF0X::Run()
|
|||||||
ScheduleDelayed(_interval);
|
ScheduleDelayed(_interval);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SF0X::print_info()
|
void LightwareLaserSerial::print_info()
|
||||||
{
|
{
|
||||||
perf_print_counter(_sample_perf);
|
perf_print_counter(_sample_perf);
|
||||||
perf_print_counter(_comms_errors);
|
perf_print_counter(_comms_errors);
|
||||||
@@ -32,11 +32,11 @@
|
|||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file SF0X.hpp
|
* @file lightware_laser_serial.hpp
|
||||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||||
* @author Greg Hulands
|
* @author Greg Hulands
|
||||||
*
|
*
|
||||||
* Driver for the Lightware SF0x laser rangefinder series
|
* Driver for the Lightware laser rangefinder series
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
@@ -48,13 +48,13 @@
|
|||||||
#include <lib/parameters/param.h>
|
#include <lib/parameters/param.h>
|
||||||
#include <lib/perf/perf_counter.h>
|
#include <lib/perf/perf_counter.h>
|
||||||
|
|
||||||
#include "sf0x_parser.h"
|
#include "parser.h"
|
||||||
|
|
||||||
class SF0X : public px4::ScheduledWorkItem
|
class LightwareLaserSerial : public px4::ScheduledWorkItem
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
SF0X(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
LightwareLaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
||||||
~SF0X() override;
|
~LightwareLaserSerial() override;
|
||||||
|
|
||||||
int init();
|
int init();
|
||||||
void print_info();
|
void print_info();
|
||||||
@@ -67,7 +67,6 @@ private:
|
|||||||
int measure();
|
int measure();
|
||||||
int collect();
|
int collect();
|
||||||
|
|
||||||
|
|
||||||
PX4Rangefinder _px4_rangefinder;
|
PX4Rangefinder _px4_rangefinder;
|
||||||
|
|
||||||
char _port[20] {};
|
char _port[20] {};
|
||||||
@@ -76,7 +75,7 @@ private:
|
|||||||
int _fd{-1};
|
int _fd{-1};
|
||||||
char _linebuf[10] {};
|
char _linebuf[10] {};
|
||||||
unsigned _linebuf_index{0};
|
unsigned _linebuf_index{0};
|
||||||
enum SF0X_PARSE_STATE _parse_state {SF0X_PARSE_STATE0_UNSYNC};
|
enum LW_PARSE_STATE _parse_state {LW_PARSE_STATE0_UNSYNC};
|
||||||
hrt_abstime _last_read{0};
|
hrt_abstime _last_read{0};
|
||||||
|
|
||||||
unsigned _consecutive_fail_count;
|
unsigned _consecutive_fail_count;
|
||||||
@@ -84,6 +83,4 @@ private:
|
|||||||
perf_counter_t _sample_perf;
|
perf_counter_t _sample_perf;
|
||||||
perf_counter_t _comms_errors;
|
perf_counter_t _comms_errors;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
};
|
};
|
||||||
@@ -31,15 +31,15 @@
|
|||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
#include "SF0X.hpp"
|
#include "lightware_laser_serial.hpp"
|
||||||
|
|
||||||
#include <px4_platform_common/getopt.h>
|
#include <px4_platform_common/getopt.h>
|
||||||
#include <px4_platform_common/module.h>
|
#include <px4_platform_common/module.h>
|
||||||
|
|
||||||
namespace sf0x
|
namespace lightware_laser
|
||||||
{
|
{
|
||||||
|
|
||||||
SF0X *g_dev{nullptr};
|
LightwareLaserSerial *g_dev{nullptr};
|
||||||
|
|
||||||
static int start(const char *port, uint8_t rotation)
|
static int start(const char *port, uint8_t rotation)
|
||||||
{
|
{
|
||||||
@@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation)
|
|||||||
}
|
}
|
||||||
|
|
||||||
/* create the driver */
|
/* create the driver */
|
||||||
g_dev = new SF0X(port, rotation);
|
g_dev = new LightwareLaserSerial(port, rotation);
|
||||||
|
|
||||||
if (g_dev == nullptr) {
|
if (g_dev == nullptr) {
|
||||||
return -1;
|
return -1;
|
||||||
@@ -109,12 +109,12 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
|
|||||||
### Examples
|
### Examples
|
||||||
|
|
||||||
Attempt to start driver on a specified serial device.
|
Attempt to start driver on a specified serial device.
|
||||||
$ sf0x start -d /dev/ttyS1
|
$ lightware_laser_serial start -d /dev/ttyS1
|
||||||
Stop driver
|
Stop driver
|
||||||
$ sf0x stop
|
$ lightware_laser_serial stop
|
||||||
)DESCR_STR");
|
)DESCR_STR");
|
||||||
|
|
||||||
PRINT_MODULE_USAGE_NAME("sf0x", "driver");
|
PRINT_MODULE_USAGE_NAME("lightware_laser_serial", "driver");
|
||||||
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
|
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
|
||||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
|
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
|
||||||
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
|
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
|
||||||
@@ -125,7 +125,7 @@ $ sf0x stop
|
|||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
extern "C" __EXPORT int sf0x_main(int argc, char *argv[])
|
extern "C" __EXPORT int lightware_laser_serial_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
||||||
const char *device_path = nullptr;
|
const char *device_path = nullptr;
|
||||||
@@ -144,26 +144,26 @@ extern "C" __EXPORT int sf0x_main(int argc, char *argv[])
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
sf0x::usage();
|
lightware_laser::usage();
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (myoptind >= argc) {
|
if (myoptind >= argc) {
|
||||||
sf0x::usage();
|
lightware_laser::usage();
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[myoptind], "start")) {
|
if (!strcmp(argv[myoptind], "start")) {
|
||||||
return sf0x::start(device_path, rotation);
|
return lightware_laser::start(device_path, rotation);
|
||||||
|
|
||||||
} else if (!strcmp(argv[myoptind], "stop")) {
|
} else if (!strcmp(argv[myoptind], "stop")) {
|
||||||
return sf0x::stop();
|
return lightware_laser::stop();
|
||||||
|
|
||||||
} else if (!strcmp(argv[myoptind], "status")) {
|
} else if (!strcmp(argv[myoptind], "status")) {
|
||||||
return sf0x::status();
|
return lightware_laser::status();
|
||||||
}
|
}
|
||||||
|
|
||||||
sf0x::usage();
|
lightware_laser::usage();
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
@@ -0,0 +1,7 @@
|
|||||||
|
module_name: Lightware Laser Rangefinder (serial)
|
||||||
|
serial_config:
|
||||||
|
- command: lightware_laser_serial start -d ${SERIAL_DEV}
|
||||||
|
port_config_param:
|
||||||
|
name: SENS_SF0X_CFG
|
||||||
|
group: Sensors
|
||||||
|
|
||||||
@@ -32,7 +32,7 @@
|
|||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Lightware Laser Rangefinder hardware model
|
* Lightware Laser Rangefinder hardware model (serial)
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @reboot_required true
|
||||||
* @group Sensors
|
* @group Sensors
|
||||||
@@ -32,19 +32,19 @@
|
|||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file sf0x_parser.cpp
|
* @file parser.cpp
|
||||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||||
*
|
*
|
||||||
* Driver for the Lightware SF0x laser rangefinder series
|
* Driver for the Lightware laser rangefinder series
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "sf0x_parser.h"
|
#include "parser.h"
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
|
||||||
//#define SF0X_DEBUG
|
//#define LW_DEBUG
|
||||||
|
|
||||||
#ifdef SF0X_DEBUG
|
#ifdef LW_DEBUG
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
const char *parser_state[] = {
|
const char *parser_state[] = {
|
||||||
@@ -58,98 +58,98 @@ const char *parser_state[] = {
|
|||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
|
int lightware_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum LW_PARSE_STATE *state, float *dist)
|
||||||
{
|
{
|
||||||
int ret = -1;
|
int ret = -1;
|
||||||
char *end;
|
char *end;
|
||||||
|
|
||||||
switch (*state) {
|
switch (*state) {
|
||||||
case SF0X_PARSE_STATE0_UNSYNC:
|
case LW_PARSE_STATE0_UNSYNC:
|
||||||
if (c == '\n') {
|
if (c == '\n') {
|
||||||
*state = SF0X_PARSE_STATE1_SYNC;
|
*state = LW_PARSE_STATE1_SYNC;
|
||||||
(*parserbuf_index) = 0;
|
(*parserbuf_index) = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SF0X_PARSE_STATE1_SYNC:
|
case LW_PARSE_STATE1_SYNC:
|
||||||
if (c >= '0' && c <= '9') {
|
if (c >= '0' && c <= '9') {
|
||||||
*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
|
*state = LW_PARSE_STATE2_GOT_DIGIT0;
|
||||||
parserbuf[*parserbuf_index] = c;
|
parserbuf[*parserbuf_index] = c;
|
||||||
(*parserbuf_index)++;
|
(*parserbuf_index)++;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SF0X_PARSE_STATE2_GOT_DIGIT0:
|
case LW_PARSE_STATE2_GOT_DIGIT0:
|
||||||
if (c >= '0' && c <= '9') {
|
if (c >= '0' && c <= '9') {
|
||||||
*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
|
*state = LW_PARSE_STATE2_GOT_DIGIT0;
|
||||||
parserbuf[*parserbuf_index] = c;
|
parserbuf[*parserbuf_index] = c;
|
||||||
(*parserbuf_index)++;
|
(*parserbuf_index)++;
|
||||||
|
|
||||||
} else if (c == '.') {
|
} else if (c == '.') {
|
||||||
*state = SF0X_PARSE_STATE3_GOT_DOT;
|
*state = LW_PARSE_STATE3_GOT_DOT;
|
||||||
parserbuf[*parserbuf_index] = c;
|
parserbuf[*parserbuf_index] = c;
|
||||||
(*parserbuf_index)++;
|
(*parserbuf_index)++;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
*state = SF0X_PARSE_STATE0_UNSYNC;
|
*state = LW_PARSE_STATE0_UNSYNC;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SF0X_PARSE_STATE3_GOT_DOT:
|
case LW_PARSE_STATE3_GOT_DOT:
|
||||||
if (c >= '0' && c <= '9') {
|
if (c >= '0' && c <= '9') {
|
||||||
*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
|
*state = LW_PARSE_STATE4_GOT_DIGIT1;
|
||||||
parserbuf[*parserbuf_index] = c;
|
parserbuf[*parserbuf_index] = c;
|
||||||
(*parserbuf_index)++;
|
(*parserbuf_index)++;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
*state = SF0X_PARSE_STATE0_UNSYNC;
|
*state = LW_PARSE_STATE0_UNSYNC;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SF0X_PARSE_STATE4_GOT_DIGIT1:
|
case LW_PARSE_STATE4_GOT_DIGIT1:
|
||||||
if (c >= '0' && c <= '9') {
|
if (c >= '0' && c <= '9') {
|
||||||
*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
|
*state = LW_PARSE_STATE5_GOT_DIGIT2;
|
||||||
parserbuf[*parserbuf_index] = c;
|
parserbuf[*parserbuf_index] = c;
|
||||||
(*parserbuf_index)++;
|
(*parserbuf_index)++;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
*state = SF0X_PARSE_STATE0_UNSYNC;
|
*state = LW_PARSE_STATE0_UNSYNC;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SF0X_PARSE_STATE5_GOT_DIGIT2:
|
case LW_PARSE_STATE5_GOT_DIGIT2:
|
||||||
if (c == '\r') {
|
if (c == '\r') {
|
||||||
*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;
|
*state = LW_PARSE_STATE6_GOT_CARRIAGE_RETURN;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
*state = SF0X_PARSE_STATE0_UNSYNC;
|
*state = LW_PARSE_STATE0_UNSYNC;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
|
case LW_PARSE_STATE6_GOT_CARRIAGE_RETURN:
|
||||||
if (c == '\n') {
|
if (c == '\n') {
|
||||||
parserbuf[*parserbuf_index] = '\0';
|
parserbuf[*parserbuf_index] = '\0';
|
||||||
*dist = strtod(parserbuf, &end);
|
*dist = strtod(parserbuf, &end);
|
||||||
*state = SF0X_PARSE_STATE1_SYNC;
|
*state = LW_PARSE_STATE1_SYNC;
|
||||||
*parserbuf_index = 0;
|
*parserbuf_index = 0;
|
||||||
ret = 0;
|
ret = 0;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
*state = SF0X_PARSE_STATE0_UNSYNC;
|
*state = LW_PARSE_STATE0_UNSYNC;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef SF0X_DEBUG
|
#ifdef LW_DEBUG
|
||||||
printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
|
printf("state: LW_PARSE_STATE%s\n", parser_state[*state]);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
@@ -32,22 +32,22 @@
|
|||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file sf0x_parser.cpp
|
* @file parser.cpp
|
||||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||||
*
|
*
|
||||||
* Declarations of parser for the Lightware SF0x laser rangefinder series
|
* Declarations of parser for the Lightware laser rangefinder series
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
enum SF0X_PARSE_STATE {
|
enum LW_PARSE_STATE {
|
||||||
SF0X_PARSE_STATE0_UNSYNC = 0,
|
LW_PARSE_STATE0_UNSYNC = 0,
|
||||||
SF0X_PARSE_STATE1_SYNC,
|
LW_PARSE_STATE1_SYNC,
|
||||||
SF0X_PARSE_STATE2_GOT_DIGIT0,
|
LW_PARSE_STATE2_GOT_DIGIT0,
|
||||||
SF0X_PARSE_STATE3_GOT_DOT,
|
LW_PARSE_STATE3_GOT_DOT,
|
||||||
SF0X_PARSE_STATE4_GOT_DIGIT1,
|
LW_PARSE_STATE4_GOT_DIGIT1,
|
||||||
SF0X_PARSE_STATE5_GOT_DIGIT2,
|
LW_PARSE_STATE5_GOT_DIGIT2,
|
||||||
SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
|
LW_PARSE_STATE6_GOT_CARRIAGE_RETURN
|
||||||
};
|
};
|
||||||
|
|
||||||
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);
|
int lightware_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum LW_PARSE_STATE *state, float *dist);
|
||||||
@@ -31,11 +31,11 @@
|
|||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE drivers__sf0x__sf0x_tests
|
MODULE drivers__distance_sensor__lightware_laser_serial__tests
|
||||||
MAIN sf0x_tests
|
MAIN lightware_laser_test
|
||||||
COMPILE_FLAGS
|
COMPILE_FLAGS
|
||||||
SRCS
|
SRCS
|
||||||
SF0XTest.cpp
|
lightware_laser_test.cpp
|
||||||
../sf0x_parser.cpp
|
../parser.cpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
)
|
)
|
||||||
@@ -1,6 +1,6 @@
|
|||||||
#include <unit_test.h>
|
#include <unit_test.h>
|
||||||
|
|
||||||
#include "../sf0x_parser.h"
|
#include "../parser.h"
|
||||||
|
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
|
||||||
@@ -8,25 +8,25 @@
|
|||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
|
||||||
extern "C" __EXPORT int sf0x_tests_main(int argc, char *argv[]);
|
extern "C" __EXPORT int lightware_laser_test_main(int argc, char *argv[]);
|
||||||
|
|
||||||
class SF0XTest : public UnitTest
|
class LightwareLaserTest : public UnitTest
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
virtual bool run_tests();
|
virtual bool run_tests();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
bool sf0xTest();
|
bool runTest();
|
||||||
};
|
};
|
||||||
|
|
||||||
bool SF0XTest::run_tests()
|
bool LightwareLaserTest::run_tests()
|
||||||
{
|
{
|
||||||
ut_run_test(sf0xTest);
|
ut_run_test(runTest);
|
||||||
|
|
||||||
return (_tests_failed == 0);
|
return (_tests_failed == 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SF0XTest::sf0xTest()
|
bool LightwareLaserTest::runTest()
|
||||||
{
|
{
|
||||||
const char _LINE_MAX = 20;
|
const char _LINE_MAX = 20;
|
||||||
//char _linebuf[_LINE_MAX];
|
//char _linebuf[_LINE_MAX];
|
||||||
@@ -50,7 +50,7 @@ bool SF0XTest::sf0xTest()
|
|||||||
"\r\n"
|
"\r\n"
|
||||||
};
|
};
|
||||||
|
|
||||||
enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
|
enum LW_PARSE_STATE state = LW_PARSE_STATE0_UNSYNC;
|
||||||
float dist_m;
|
float dist_m;
|
||||||
char _parserbuf[_LINE_MAX];
|
char _parserbuf[_LINE_MAX];
|
||||||
unsigned _parsebuf_index = 0;
|
unsigned _parsebuf_index = 0;
|
||||||
@@ -61,7 +61,7 @@ bool SF0XTest::sf0xTest()
|
|||||||
int parse_ret = -1;
|
int parse_ret = -1;
|
||||||
|
|
||||||
for (int i = 0; i < (ssize_t)strlen(lines[l]); i++) {
|
for (int i = 0; i < (ssize_t)strlen(lines[l]); i++) {
|
||||||
parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
|
parse_ret = lightware_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
|
||||||
|
|
||||||
if (parse_ret == 0) {
|
if (parse_ret == 0) {
|
||||||
if (l == 0) {
|
if (l == 0) {
|
||||||
@@ -88,4 +88,4 @@ bool SF0XTest::sf0xTest()
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
ut_declare_test_c(sf0x_tests_main, SF0XTest)
|
ut_declare_test_c(lightware_laser_test_main, LightwareLaserTest)
|
||||||
@@ -1,7 +0,0 @@
|
|||||||
module_name: Lightware Laser Rangefinder
|
|
||||||
serial_config:
|
|
||||||
- command: sf0x start -d ${SERIAL_DEV}
|
|
||||||
port_config_param:
|
|
||||||
name: SENS_SF0X_CFG
|
|
||||||
group: Sensors
|
|
||||||
|
|
||||||
@@ -125,7 +125,7 @@ const struct {
|
|||||||
{"controllib", controllib_test_main, 0},
|
{"controllib", controllib_test_main, 0},
|
||||||
{"mavlink", mavlink_tests_main, 0},
|
{"mavlink", mavlink_tests_main, 0},
|
||||||
#ifdef __PX4_NUTTX
|
#ifdef __PX4_NUTTX
|
||||||
{"sf0x", sf0x_tests_main, 0},
|
{"lightware_laser", lightware_laser_test_main, 0},
|
||||||
#endif
|
#endif
|
||||||
{"uorb", uorb_tests_main, 0},
|
{"uorb", uorb_tests_main, 0},
|
||||||
|
|
||||||
|
|||||||
@@ -95,7 +95,7 @@ extern int mavlink_tests_main(int argc, char *argv[]);
|
|||||||
extern int controllib_test_main(int argc, char *argv[]);
|
extern int controllib_test_main(int argc, char *argv[]);
|
||||||
extern int uorb_tests_main(int argc, char *argv[]);
|
extern int uorb_tests_main(int argc, char *argv[]);
|
||||||
extern int rc_tests_main(int argc, char *argv[]);
|
extern int rc_tests_main(int argc, char *argv[]);
|
||||||
extern int sf0x_tests_main(int argc, char *argv[]);
|
extern int lightware_laser_test_main(int argc, char *argv[]);
|
||||||
|
|
||||||
__END_DECLS
|
__END_DECLS
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user