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

Removed dismissed folder. 

TODO : Adjust work_queue rescheduling (to always have a 10Hz cycle)

Signed-off-by: Claudio Micheli <claudio@auterion.com>
sbg
Claudio Micheli 6 years ago committed by Beat Küng
parent
b0ac8fe963
commit
5d70ac7fd9
3 changed files with 11 additions and 1151 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. 824
      src/drivers/distance_sensor/airlango/isl2950.cpp
  2. 205
      src/drivers/distance_sensor/airlango/isl2950_parser.cpp
  3. 133
      src/drivers/distance_sensor/isl2950/isl2950.cpp

824
src/drivers/distance_sensor/airlango/isl2950.cpp
Unescape View File

@ -1,824 +0,0 @@ @@ -1,824 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2014-2015 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 landbao15L2950.cpp
* @author Claudio Micheli <claudio@auterion.com>
*
* Driver for the Lanbao ISL2950
*/
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_workqueue.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <termios.h>
#include <perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include "isl2950_parser.h"
/* Configuration Constants */
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
#define ISL2950_TAKE_RANGE_REG 'd'
// designated serial port on Pixhawk
#define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200
// normal conversion wait time
#define ISL2950_CONVERSION_INTERVAL 100*1000UL/* 100ms */
class ISL2950 : public cdev::CDev
{
public:
// Constructor
ISL2950(const char *port = ISL2950_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
// Virtual destructor
virtual ~ISL2950();
virtual int init();
//virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
private:
char _port[20];
uint8_t _rotation;
float _min_distance;
float _max_distance;
int _conversion_interval;
work_s _work{};
ringbuffer::RingBuffer *_reports;
int _measure_ticks;
bool _collect_phase;
int _fd;
uint8_t _linebuf[20];
unsigned _linebuf_index;
enum ISL2950_PARSE_STATE _parse_state;
unsigned char _frame_data[4];
uint16_t _crc16;
hrt_abstime _last_read;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic;
unsigned _consecutive_fail_count;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
/**
* 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();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE
* and SF0X_MAX_DISTANCE
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int isl2950_main(int argc, char *argv[]);
/**
* Method : Constructor
*
* @note This method initializes the class variables
*/
ISL2950::ISL2950(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
_rotation(rotation),
_min_distance(0.10f),
_max_distance(40.0f),
_conversion_interval(ISL2950_CONVERSION_INTERVAL),
_reports(nullptr),
_measure_ticks(0),
_collect_phase(false),
_fd(-1),
_linebuf_index(0),
_parse_state(TFS_NOT_STARTED),
_frame_data{TOF_SFD1, TOF_SFD2, 0, 0},
_crc16(0),
_last_read(0),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(nullptr),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "isl2950_read")),
_comms_errors(perf_alloc(PC_COUNT, "isl2950_com_err"))
{
/* store port name */
strncpy(_port, port, sizeof(_port));
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
}
// Destructor
ISL2950::~ISL2950()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
}
perf_free(_sample_perf);
perf_free(_comms_errors);
}
/**
* Method : init()
*
* This method setup the general driver for a range finder sensor.
*/
int
ISL2950::init()
{
/* status */
int ret = 0;
do { /* create a scope to handle exit conditions using break */
/* do regular cdev init */
ret = CDev::init();
if (ret != OK) { break; }
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
PX4_ERR("alloc failed");
ret = -1;
break;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct 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");
}
} while (0);
return ret;
}
void
ISL2950::set_minimum_distance(float min)
{
_min_distance = min;
}
void
ISL2950::set_maximum_distance(float max)
{
_max_distance = max;
}
float
ISL2950::get_minimum_distance()
{
return _min_distance;
}
float
ISL2950::get_maximum_distance()
{
return _max_distance;
}
int
ISL2950::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(_conversion_interval);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
int ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(_conversion_interval)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
/*
ssize_t
ISL2950::read(device::file_t *filp, char *buffer, size_t buflen)
{
// SOME STUFFS
}*/
int
ISL2950::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
char cmd = ISL2950_TAKE_RANGE_REG;
ret = ::write(_fd, &cmd, 1);
if (ret != sizeof(cmd)) {
perf_count(_comms_errors);
printf("write fail %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
/*
* Method: collect()
*
* This method reads data from serial UART and places it into a buffer
*/
ISL2950::collect()
{
int bytes_read = 0;
int distance_mm = -1.0f;
bool full_frame = false;
perf_begin(_sample_perf);
/* the buffer for read chars is buflen minus null termination */
uint8_t readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf);
/* read from the sensor (uart buffer) */
bytes_read = ::read(_fd, &readbuf[0], readlen);
if (bytes_read < 0) {
stop_serial_read = true;
PX4_DEBUG("read err: %d \n", bytes_read);
perf_count(_comms_errors);
perf_end(_sample_perf);
} else if (bytes_read > 0){
_last_read = hrt_absolute_time();
for (int i = 0; i < bytes_read; i++) {
if (OK == isl2950_parser(readbuf[i],_frame_data, &_parse_state,&_crc16, &distance_mm)){
full_frame = true;
}
}
}
if (!full_frame) {
return -EAGAIN;
}
printf("val (int): %d, raw: 0x%08X, valid: %s \n", distance_mm, _frame_data, ((full_frame) ? "OK" : "NO"));
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
report.current_distance = distance_mm/1000.0f;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
bytes_read = OK;
perf_end(_sample_perf);
return bytes_read;
}
void
ISL2950::start()
{
PX4_INFO("ISL2950::start() - launch the work queue");
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&ISL2950::cycle_trampoline, this, 1);
}
void
ISL2950::stop()
{
work_cancel(HPWORK, &_work);
}
void
ISL2950::cycle_trampoline(void *arg)
{
ISL2950 *dev = static_cast<ISL2950 *>(arg);
dev->cycle();
}
void
ISL2950::cycle()
{
PX4_DEBUG("ISL2950::cycle() - in the cycle");
/* fds initialized? */
if (_fd < 0) {
/* open fd */
_fd = ::open(_port,O_RDWR);
if (_fd < 0) {
PX4_ERR("ISL2950::cycle() - open failed (%i)", errno);
return;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(_fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* no parity, one stop bit */
uart_config.c_cflag &= ~(CSTOPB | PARENB);
unsigned speed = B115200;
/* 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);
}
}
/* perform collection */
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
/* We are missing bytes to complete the packet, re-cycle at 1ms */
work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(1000LL));
return;
}
/* schedule a fresh cycle call when a complete packet has been received */
work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(_conversion_interval));
}
void
ISL2950::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %d ticks\n", _measure_ticks);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace isl2950
{
ISL2950 *g_dev;
int start(const char *port, uint8_t rotation);
int stop();
int test();
int reset();
int info();
/**
* Start the driver.
*/
int
start(const char *port, uint8_t rotation)
{
int fd;
if (g_dev != nullptr) {
PX4_WARN("already started");
return -1;
}
/* create the driver */
g_dev = new ISL2950(port, rotation);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(RANGE_FINDER0_DEVICE_PATH, 0);
if (fd < 0) {
PX4_ERR("device open fail (%i)", errno);
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("failed to set baudrate %d", B115200);
goto fail;
}
PX4_DEBUG("isl2950::start() succeeded");
return 0;
fail:
PX4_DEBUG("isl2950::start() failed");
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return -1;
}
/**
* Stop the driver
*/
int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
return -1;
}
return 0;
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
int
test()
{
struct distance_sensor_s report;
ssize_t sz;
int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'isl2950 start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH);
return -1;
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("immediate read failed");
return -1;
}
print_message(report);
/* start the sensor polling at 2 Hz rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
PX4_ERR("failed to set 2Hz poll rate");
return -1;
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
int ret = poll(&fds, 1, 2000);
if (ret != 1) {
PX4_ERR("timed out");
break;
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report));
break;
}
print_message(report);
}
/* reset the sensor polling to the default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
PX4_ERR("ioctl SENSORIOCSPOLLRATE failed");
return -1;
}
return 0;
}
/**
* Reset the driver.
*/
int
reset()
{
int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("open failed (%i)", errno);
return -1;
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
return -1;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
return -1;
}
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return -1;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
return 0;
}
} // namespace
int
isl2950_main(int argc, char *argv[])
{
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = ISL2950_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 -1;
}
}
if (myoptind >= argc) {
goto out_error;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
return isl2950::start(device_path, rotation);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return isl2950::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[myoptind], "test")) {
return isl2950::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
return isl2950::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
return isl2950::info();
}
out_error:
PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
return -1;
}

205
src/drivers/distance_sensor/airlango/isl2950_parser.cpp
Unescape View File

@ -1,205 +0,0 @@ @@ -1,205 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2014 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 isl2950_parser.cpp
* @author Claudio Micheli
* claudio@auterion.com
*
*/
#include "isl2950_parser.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "isl2950_parser.h"
#include <string.h>
#include <stdlib.h>
typedef unsigned char UCHAR;
typedef unsigned short USHORT;
// Note : No clue what those static variables are
static const UCHAR aucCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40
};
static const UCHAR aucCRCLo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
0x41, 0x81, 0x80, 0x40
};
// TOF frame format
//
// 1B 1B 1B 1B 2B
// | 0xA5 | 0x5A | distance-MSB | distance-LSB | crc-16 |
//
// Frame data saved for CRC calculation
const static int TOF_DISTANCE_MSB_POS = 2;
const static int TOF_DISTANCE_LSB_POS = 3;
const static int TOF_CRC_CALC_DATA_LEN = 4;
USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) {
UCHAR ucCRCHi = 0xFF;
UCHAR ucCRCLo = 0xFF;
int iIndex;
while (usLen--) {
iIndex = ucCRCLo ^ *(pucFrame++);
ucCRCLo = (UCHAR)(ucCRCHi ^ aucCRCHi[iIndex]);
ucCRCHi = aucCRCLo[iIndex];
}
return (USHORT)(ucCRCHi << 8 | ucCRCLo);
}
int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, uint16_t *crc16, int *dist)
{
int ret = -1;
// int bytes_processed = 0;
// uint8_t b = buffer[bytes_processed++]; // Can be removed
// printf("parse byte 0x%02X \n", b);
switch (*state) {
case TFS_NOT_STARTED:
if (c == TOF_SFD1) {
*state = TFS_GOT_SFD1;
//printf("Got SFD1 \n");
}
break;
case TFS_GOT_SFD1:
if (c == TOF_SFD2) {
*state = TFS_GOT_SFD2;
//printf("Got SFD2 \n");
}
// @NOTE (claudio@auterion.com): Strange thing, if second byte is wrong we skip all the frame !!
else if (c == TOF_SFD1) {
*state = TFS_GOT_SFD1;
// printf("Discard previous SFD1, Got new SFD1 \n");
} else {
*state = TFS_NOT_STARTED;
}
break;
case TFS_GOT_SFD2:
*state = TFS_GOT_DATA1;
parserbuf[TOF_DISTANCE_MSB_POS] = c; // MSB Data
//printf("Got DATA1 0x%02X \n", c);
break;
case TFS_GOT_DATA1:
*state = TFS_GOT_DATA2;
parserbuf[TOF_DISTANCE_LSB_POS] = c; // LSB Data
//printf("Got DATA2 0x%02X \n", c);
// do crc calculation
*crc16 = usMBCRC16(parserbuf, TOF_CRC_CALC_DATA_LEN);
// convert endian
*crc16 = (*crc16 >> 8) | (*crc16 << 8);
break;
case TFS_GOT_DATA2:
if (c == (*crc16 >> 8)) {
*state = TFS_GOT_CHECKSUM1;
} else {
printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
//*state = TFS_NOT_STARTED;
*state = TFS_GOT_CHECKSUM1; // Forcing to print the value anyway
}
break;
case TFS_GOT_CHECKSUM1:
// Here, reset state to `NOT-STARTED` no matter crc ok or not
*state = TFS_NOT_STARTED;
/*if (c == (*crc16 & 0xFF)) {
//printf("Checksum verified \n");
*dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS];
return OK;
}*/
*dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS];
return OK;
break;
default:
printf("This should never happen. \n");
break;
}
// SOME STUFFS
return ret;
}

133
src/drivers/distance_sensor/isl2950/isl2950.cpp
Unescape View File

@ -35,7 +35,7 @@ @@ -35,7 +35,7 @@
* @file landbao15L2950.cpp
* @author Claudio Micheli <claudio@auterion.com>
*
* Driver for the Lanbao ISL2950
* Driver for the ISL2950
*/
#include <px4_config.h>
@ -399,46 +399,39 @@ int @@ -399,46 +399,39 @@ int
ISL2950::collect()
{
int bytes_read = 0;
int bytes_available = 0;
int distance_mm = -1.0f;
bool full_frame = false;
bool stop_serial_read = false;
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
int64_t read_elapsed = hrt_absolute_time();
read_elapsed = read_elapsed - _last_read;
/* the buffer for read chars is buflen minus null termination */
uint8_t readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf);
while ((!stop_serial_read)) {
/* read from the sensor (uart buffer) */
bytes_read = ::read(_fd, &readbuf[0], readlen);
if (bytes_read < 0) {
stop_serial_read = true;
PX4_DEBUG("read err: %d \n", bytes_read);
perf_count(_comms_errors);
perf_end(_sample_perf);
} else if (bytes_read > 0){
_last_read = hrt_absolute_time();
bytes_available += bytes_read;
//printf("Got a buffer with %d bytes,read %d \n", bytes_available,bytes_read);
for (int i = 0; i < bytes_read; i++) {
if (OK == isl2950_parser(readbuf[i],_frame_data, &_parse_state,&_crc16, &distance_mm)){
stop_serial_read = true;
full_frame = true;
}
}
}
}
if (!full_frame) {
return -EAGAIN;
@ -472,86 +465,6 @@ ISL2950::collect() @@ -472,86 +465,6 @@ ISL2950::collect()
perf_end(_sample_perf);
return bytes_read;
// ----------------------- LANBAO SPECIFIC ---------------------------
/*
uint8_t buffer[50];
int bytes_available = 0;
int bytes_processed = 0;
int bytes_read = 0;
int distance_mm = -1.0f;
bytes_read = ::read(_fd, buffer + bytes_available, 50 - bytes_available);
//printf("read() returns %02X %02X %02X %02X \n", buffer[0], buffer[1],buffer[2],buffer[3] );
//--------------------------------------------------------------------
if (bytes_read < 0) {
PX4_ERR("isl2950 - read() error: %d \n", bytes_read);
perf_count(_comms_errors);
perf_end(_sample_perf);
// only throw an error if we time out
if (read_elapsed > (_conversion_interval * 2)) {
printf("read elapsed %d , conversion interval %d",read_elapsed,_conversion_interval * 2);
return bytes_read;
} else {
printf("EAGAIN",read_elapsed,_conversion_interval * 2);
return -EAGAIN;
}
} else if (bytes_read == 0) {
return OK; // If we dont read any bites we simply exit from collecting
}
_last_read = hrt_absolute_time();
bytes_available += bytes_read;
// parse the buffer data
full_frame = false;
bytes_processed = isl2950_parser(buffer, bytes_available, &full_frame,&distance_mm);
tempo = tempo - hrt_absolute_time();
//printf("isl2950_parser() processed %d bytes, full_frame %d \n", bytes_processed, full_frame);
// discard the processed bytes and move the buffer content to the head
bytes_available -= bytes_processed;
memcpy(buffer, buffer + bytes_processed, bytes_available);
if (full_frame) {
printf("Measured Distance %d mm\n",distance_mm);
}
else if (!full_frame) { // If the frame is not valid we avoid publishing it
return OK;
}
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
report.current_distance = distance_mm/1000; // To meters
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_quality = -1;
// TODO: set proper ID
report.id = 0;
// publish it
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
// notify anyone waiting for data
poll_notify(POLLIN);
printf("tempo %d \n",tempo);
perf_end(_sample_perf);
return OK; */
}
void
@ -623,44 +536,20 @@ ISL2950::cycle() @@ -623,44 +536,20 @@ ISL2950::cycle()
PX4_ERR("baud %d ATTR", termios_state);
}
}
//printf("COLLECT \n");
/* perform collection */
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(1042 * 8));
/* We are missing bytes to complete the packet, re-cycle at 1ms */
work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(500LL));
return;
}
// ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL
// if (OK != collect_ret) {
/* we know the sensor needs about four seconds to initialize */
// if (hrt_absolute_time() > 1 * 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;
// }
// ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&ISL2950::cycle_trampoline,
this,
USEC2TICK(_conversion_interval));
/* schedule a fresh cycle call when a complete packet has been received */
work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(_conversion_interval));
}
void

Write Preview
Loading…
Cancel
Save