dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Renamed driver folder. 

Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2018-12-17 15:42:43 +01:00 committed by Beat Küng
parent dd300dca0a
commit 93b3cf241b
12 changed files with 1040 additions and 1494 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,4 +43,4 @@ add_subdirectory(ulanding)
add_subdirectory(leddar_one)
add_subdirectory(vl53lxx)
add_subdirectory(pga460)
add_subdirectory(airlango)
add_subdirectory(isl2950)

44
src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt
View File

@ -1,44 +0,0 @@
############################################################################
#
# Copyright (c) 2016 Airlango Inc. All rights reserved.
#
############################################################################
# set(TOOLS_ERROR_MSG
# "The AIRLANGO_SDK must be installed and the environment variable AIRLANG_SDK must be set"
# "(e.g. export AIRLANGO_SDK=/opt/airlangoSDK)")
# if ("$ENV{AIRLANGO_SDK}" STREQUAL "")
# message(FATAL_ERROR ${TOOLS_ERROR_MSG})
# else()
# set(AIRLANGO_SDK $ENV{AIRLANGO_SDK})
# endif()
# AirlangoSDK library header files
# include_directories(
# ${AIRLANGO_SDK}/hexagon/include
# )
# add_library(libtof SHARED IMPORTED GLOBAL)
# set_target_properties(libtof PROPERTIES IMPORTED_LOCATION ${AIRLANGO_SDK}/hexagon/lib/libtof.so)
px4_add_module(
MODULE drivers__airlangotof
MAIN tof_main
STACK_MAIN 1200
COMPILE_FLAGS
-Weffc++
SRCS
lanbao_isl_v2.cpp
lanbao_isl.cpp
tof.cpp
tof_main.cpp
DEPENDS
)
# set(module_external_libraries
# ${module_external_libraries}
# libtof
# CACHE INTERNAL "module_external_libraries"
# )

305
src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp
View File

@ -1,305 +0,0 @@
//
// Copyright (c) 2016 Airlango Ltd. All rights reserved.
//
// @file lanbao_isl.cpp
//
// Device driver implementaion for Lanbao ISL29501
//
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/ioctl.h>
#include <dev_fs_lib_serial.h>
#include <systemlib/err.h>
#include "lanbao_isl.h"
static const char _height_instructions[] = {
0xA5, 0xF0, 0x0F, 0x5A, 0xFE, 0xA5, 0x05, 0x02, 0x5A, 0x06
};
static const int _height_instructions_len =
sizeof(_height_instructions) / sizeof(_height_instructions[0]);
static enum DSPAL_SERIAL_BITRATES IntToDspalSerialBitratesEnum(int bitrate) {
// NOTE: here we only support the data rate we possibly use
switch (bitrate) {
case 115200:
return DSPAL_SIO_BITRATE_115200;
default:
return DSPAL_SIO_BITRATE_MAX;
}
}
LanbaoIsl::LanbaoIsl()
: callback_(NULL),
context_(NULL),
fd_(-1),
rx_should_stop_(false) {
PX4_DEBUG("LanbaoIsl ctor");
}
LanbaoIsl::~LanbaoIsl() {
PX4_DEBUG("LanbaoIsl dtor");
PX4_DEBUG("waiting on rx thread");
rx_should_stop_ = true;
pthread_join(rx_thread_, NULL);
PX4_DEBUG("RX thread done");
Stop();
CloseDevice();
}
int LanbaoIsl::SetDeviceMode(IslWorkingMode mode) {
if (mode == KEEP_HEIGHT) { // actively config mode to HEIGHT
int ret = write(fd_, _height_instructions, _height_instructions_len);
if (ret != _height_instructions_len) {
PX4_ERR("failed to write working mode command to device");
return -1;
}
}
return 0;
}
int LanbaoIsl::InitializeInternal(const char* device, int baudrate) {
PX4_DEBUG("LanbaoIsl::InitializeInternal()");
fd_ = open(device, O_RDWR);
if (fd_ < 0) {
PX4_ERR("failed to open %s", device);
return -1;
}
// set the serial baud rate
struct dspal_serial_ioctl_data_rate rate_cfg = {
.bit_rate = IntToDspalSerialBitratesEnum(baudrate)
};
if (rate_cfg.bit_rate == DSPAL_SIO_BITRATE_MAX) {
PX4_ERR("baudrate not supported: %d", baudrate);
goto failure;
}
if (ioctl(fd_, SERIAL_IOCTL_SET_DATA_RATE, (void*)&rate_cfg) < 0) {
PX4_ERR("failed to set baudrate %d on %s", baudrate, device);
goto failure;
}
#if 0
{
// This device requires us to manually change its working mode
IslWorkingMode mode = KEEP_HEIGHT;
if (SetDeviceMode(mode) < 0) {
PX4_ERR("failed to set device working mode as KEEP_HEIGHT");
goto failure;
}
}
#endif
PX4_DEBUG("LanbaoIsl::InitializeInternal() succeeded");
return 0;
failure:
PX4_DEBUG("LanbaoIsl::InitializeInternal() failed");
CloseDevice();
return -1;
}
void* LanbaoIsl::RxTrampoline(void *arg) {
LanbaoIsl* instance = reinterpret_cast<LanbaoIsl*>(arg);
return instance->RxMain();
}
void* LanbaoIsl::RxMain() {
PX4_DEBUG("enter LanbaoIsl::RxMain()");
uint8_t buffer[50];
int bytes_available = 0;
int bytes_processed = 0;
int bytes_read = 0;
bool full_frame;
while (!rx_should_stop_) {
// sleep for a short while. This is b/c the
// dspal serial driver does not support blocking
// read.
usleep(10 * 1000);
// read incoming bytes into buffer
// On Eagle board, `read()` returns:
// 0 means there's no more data available to read
// < 0 means something error on hardware
// > 0 means successfully get data
bytes_read = read(fd_, buffer + bytes_available, 50 - bytes_available);
PX4_DEBUG("read() returns %d", bytes_read);
if (bytes_read < 0) {
PX4_ERR("error on read(): %d", bytes_read);
rx_should_stop_ = true;
} else if (bytes_read == 0) {
continue;
}
bytes_available += bytes_read;
// parse the buffer data
full_frame = false;
bytes_processed = Parse(buffer, bytes_available, &full_frame);
PX4_DEBUG("Parse() processed %d bytes, full_frame %d",
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 is identified, invoke the
// callback_ if available
if (full_frame) {
int raw_measurement = data_.distance_mm;
// TODO:ATTENTION:
//
#if 0
raw_measurement -= 80;
if (raw_measurement < 0) {
raw_measurement = 0;
}
#endif
data_.raw_distance_mm = raw_measurement;
data_.distance_mm = distance_filter_.Filter(raw_measurement);
PX4_DEBUG("tof measurement data, raw: %d mm, filtered: %d mm",
raw_measurement, data_.distance_mm);
if (callback_) {
callback_(&data_, context_);
}
}
}
PX4_DEBUG("exit RxMain()");
return NULL;
}
void LanbaoIsl::CloseDevice() {
PX4_DEBUG("LanbaoIsl::CloseDevice()");
if (fd_ >= 0) {
PX4_DEBUG("close device handle %d", fd_);
close(fd_);
fd_ = -1;
}
}
int LanbaoIsl::StartRxThread(DataReadyCallback callback, void* context) {
PX4_DEBUG("LanbaoIsl::StartRxThread()");
int ret;
callback_ = callback;
context_ = context;
rx_should_stop_ = false;
pthread_attr_t attr;
size_t stacksize = -1;
pthread_attr_init(&attr);
pthread_attr_getstacksize(&attr, &stacksize);
PX4_DEBUG("RX thread stack size: %d", stacksize);
stacksize = 8 * 1024;
PX4_DEBUG("setting the thread stack size to[%d]", stacksize);
pthread_attr_setstacksize(&attr, stacksize);
ret = pthread_create(&rx_thread_, &attr, &LanbaoIsl::RxTrampoline, this);
if (ret != 0) {
PX4_ERR("Failed to create RX thread in LanbaoIsl: %d", ret);
return -1;
}
PX4_DEBUG("RX thread created in LanbaoIsl");
return 0;
}
int LanbaoIsl::SendMeasurementCommand() {
PX4_DEBUG("SendMeasurementCommand(), nothing need to do since device will.");
return 0;
}
int LanbaoIsl::Parse(const uint8_t* buffer, int length, bool* full_frame) {
static TofFramingState state = TFS_NOT_STARTED;
static uint16_t crc16 = 0;
int bytes_processed = 0;
PX4_DEBUG("LanbaoTof::Parse()");
while (bytes_processed < length) {
uint8_t b = buffer[bytes_processed++];
PX4_DEBUG("parse byte 0x%02X", b);
switch (state) {
case TFS_NOT_STARTED:
if (b == TOF_SFD1) {
crc16 = b;
state = TFS_GOT_SFD1;
PX4_DEBUG("Got SFD1");
}
break;
case TFS_GOT_SFD1:
if (b == TOF_SFD2) {
crc16 += b;
state = TFS_GOT_SFD2;
PX4_DEBUG("Got SFD2");
} else if (b == TOF_SFD1) {
crc16 = b;
state = TFS_GOT_SFD1;
PX4_DEBUG("Discard previous SFD1, Got new SFD1");
} else {
state = TFS_NOT_STARTED;
}
break;
case TFS_GOT_SFD2:
crc16 += b;
data_.distance_mm = b;
state = TFS_GOT_DATA1;
PX4_DEBUG("Got DATA1 0x%02X", b);
break;
case TFS_GOT_DATA1:
crc16 += b;
data_.distance_mm = (data_.distance_mm << 8) + b;
state = TFS_GOT_DATA2;
PX4_DEBUG("Got DATA2 0x%02X", b);
break;
case TFS_GOT_DATA2:
if (b == (crc16 >> 8)) {
state = TFS_GOT_CHECKSUM1;
} else {
PX4_DEBUG("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X",
b, crc16);
state = TFS_NOT_STARTED;
}
break;
case TFS_GOT_CHECKSUM1:
// Here, reset state to `NOT-STARTED` no matter crc ok or not
state = TFS_NOT_STARTED;
if (b == (crc16 & 0xFF)) {
PX4_DEBUG("Checksum verified");
*full_frame = true;
return bytes_processed;
} else {
PX4_DEBUG("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X",
b, crc16);
}
break;
default:
PX4_DEBUG("This should never happen.")
break;
}
}
return bytes_processed;
}

155
src/drivers/distance_sensor/airlango/repo/lanbao_isl.h
View File

@ -1,155 +0,0 @@
//
// Copyright (c) 2016 Airlango Ltd. All rights reserved.
//
// @file lanbao_isl.h
//
// Device driver implementation for ISL29501 from www.shlanbao.com
//
#pragma once
#include <pthread.h>
#include <stdint.h>
#include <string.h>
#include "tof.h"
#include "adiag.h"
// Average filter
template <typename T>
class AverageFilter {
public:
AverageFilter(int window_size = 12)
: window_full_(false),
window_size_(window_size),
item_index_(0),
items_(nullptr) {
// ASSERT(window_size > 0);
items_ = new T[window_size];
if (items_ != nullptr) {
for (int i = 0; i < window_size; i++) {
items_[i] = 0;
}
}
}
~AverageFilter() {
if (items_ != nullptr) {
delete[] items_;
}
}
public:
T Filter(const T& item) {
#if 0
sum_ -= items_[item_index_];
sum_ += item;
items_[item_index_] = item;
if (++item_index_ == window_size_) {
item_index_ = 0;
window_full_ = true;
}
T result = sum_ / (window_full_ ? window_size_ : item_index_);
return result;
#else
items_[item_index_] = item;
if (++item_index_ == window_size_) {
item_index_ = 0;
window_full_ = true;
}
int num = NumOfSamplingForCalc(item);
T result = CalcAverage(num);
return result;
#endif
}
protected:
T CalcAverage(int num) {
int valid_index; // (latest) valid index
if (item_index_ == 0) {
valid_index = window_size_ - 1;
} else {
valid_index = item_index_ - 1;
}
// IF data not fully collected, we return the original sampling data,
// this wil only occur at the very earlier stage
if (!window_full_) {
return items_[valid_index];
}
sum_ = 0;
for (int i = 0; i < num; i++) {
sum_ += items_[valid_index];
if (--valid_index < 0) {
valid_index = window_size_ - 1;
}
}
return sum_ / num;
}
int NumOfSamplingForCalc(const T& item) {
int num = (item / 1000) + 3;
if (num > window_size_) {
num = window_size_;
}
return num;
}
protected:
bool window_full_ = false;
int window_size_ = 0;
int item_index_ = 0;
T* items_ = nullptr;
T sum_ = 0;
};
// frame start delimiter
#define TOF_SFD1 0xA5
#define TOF_SFD2 0x5A
typedef enum {
TFS_NOT_STARTED = 0,
TFS_GOT_SFD1,
TFS_GOT_SFD2,
TFS_GOT_DATA1,
TFS_GOT_DATA2,
TFS_GOT_CHECKSUM1,
TFS_GOT_CHECKSUM2,
} TofFramingState;
enum IslWorkingMode {
KEEP_HEIGHT = 0,
NUM_WORKING_MODE
};
// Lanbao ISL29501 to provide tof functionality
class LanbaoIsl : public Tof {
public:
LanbaoIsl();
virtual ~LanbaoIsl();
virtual TofModel model() const { return LANBAO_ISL; };
protected:
virtual int InitializeInternal(const char* device, int baudrate);
virtual int StartRxThread(DataReadyCallback callback, void* context);
virtual int SendMeasurementCommand();
virtual int Parse(const uint8_t* buffer, int length, bool* full_frame);
static void* RxTrampoline(void* arg);
void* RxMain();
void CloseDevice();
int SetDeviceMode(IslWorkingMode mode = KEEP_HEIGHT);
DataReadyCallback callback_;
void* context_;
int fd_;
pthread_t rx_thread_;
volatile bool rx_should_stop_;
struct TofData data_;
AverageFilter<int> distance_filter_;
};

23
src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h
View File

@ -1,23 +0,0 @@
//
// Copyright (c) 2016 Airlango Ltd. All rights reserved.
//
// @file lanbao_isl_v2.h
//
// Lanbao TOF, hw version 2, with CRC-16 support
//
#ifndef EAGLE_TOF_LANBAO_ISL_V2_H_
#define EAGLE_TOF_LANBAO_ISL_V2_H_
#include "lanbao_isl.h"
class LanbaoIslV2 : public LanbaoIsl {
public:
LanbaoIslV2();
virtual ~LanbaoIslV2();
virtual TofModel model() const { return LANBAO_ISL_V2; }
protected:
virtual int Parse(const uint8_t* buffer, int length, bool* full_frame);
};
#endif

345
src/drivers/distance_sensor/airlango/repo/tof.cpp
View File

@ -1,345 +0,0 @@
//
// Copyright (c) 2016 Airlango Ltd. All rights reserved.
//
// @file tof.cpp
//
// Basic implementation for TOF device driver.
//
#include <errno.h>
#include <systemlib/err.h>
#include <pthread.h>
#include <unistd.h>
#include <signal.h>
#include "tof.h"
#include "lanbao_isl.h"
#include "lanbao_isl_v2.h"
#define TOF_MEASUREMENT_TIMEOUT_USEC 200000
#define TOF_MEASUREMENT_TIMER_SIGNAL (SIGRTMAX-1)
#define TIME_DIFF_USEC(start, end) \
(((end).tv_sec - (start).tv_sec)*1E6 + ((end).tv_nsec - (start).tv_nsec)/1E3)
Tof* Tof::instance_ = NULL;
Tof::Tof()
: is_initialized_(false),
callback_(NULL),
context_(NULL),
tx_thread_(0),
measurement_interval_ms_(0),
measurement_should_stop_(true),
periodic_measurement_running_(false) {
PX4_DEBUG("Tof ctor");
memset(&last_measurement_ts_, 0, sizeof(struct timespec));
memset(&last_measurement_, 0, sizeof(struct TofData));
// NOTE(jintao): Must initialize mutex with the following functions.
// Using INITIALIZER results in DSP crash while calling
// and pthread_mutex_destory.
pthread_mutex_init(&mutex_, NULL);
}
Tof::~Tof() {
PX4_DEBUG("Tof dtor");
instance_ = NULL;
pthread_mutex_destroy(&mutex_);
}
Tof* Tof::GetInstance(TofModel model) {
PX4_DEBUG("Tof::GetInstance() for model %s", TofModelToStr(model));
if (model >= TOF_MODEL_NUM) {
PX4_ERR("Unknown model");
return NULL;
}
if (instance_ != NULL) {
TofModel curr_model = instance_->model();
if (curr_model != model) {
PX4_ERR("Tof already initialized for model %s",
TofModelToStr(curr_model));
return NULL;
}
PX4_DEBUG("return existing instance %s", TofModelToStr(curr_model));
return instance_;
}
switch (model) {
case LANBAO_ISL:
instance_ = new LanbaoIsl();
break;
case LANBAO_ISL_V2:
instance_ = new LanbaoIslV2();
break;
default:
return NULL;
}
if (instance_ == NULL) {
PX4_ERR("Failed to instantiate Tof model %s", TofModelToStr(model));
return NULL;
}
PX4_DEBUG("Created instance for %s", TofModelToStr(model));
return instance_;
}
int Tof::Initialize(const char* device, int baudrate) {
int ret;
if (is_initialized_) {
PX4_ERR("Initialize() can be called only once!");
return -1;
}
// Initialize the specified serial device with baudrate
ret = InitializeInternal(device, baudrate);
if (ret < 0) {
PX4_ERR("Failed to initialize device %s baudrate %d", device, baudrate);
return -1;
}
// Start RX thread to parse the incoming frame
ret = StartRxThread(Tof::DataReadyCb, (void*)this);
if (ret < 0) {
PX4_ERR("Failed to register Data Ready Callback. Fatal!");
return -1;
}
is_initialized_ = true;
return 0;
}
void Tof::DataReadyCb(const struct TofData* data, void* context) {
PX4_DEBUG("Tof::DataReadyCb(): new data %d mm", data->distance_mm);
Tof* obj = (Tof*)context;
obj->HandleNewMeasurement(data);
}
void Tof::HandleNewMeasurement(const struct TofData* data) {
PX4_DEBUG("Tof::HandleNewMeasurement()");
struct TofData m;
int ret;
struct timespec ts;
// remember the latest measurement result. Call user callback
// if available.
pthread_mutex_lock(&mutex_);
memcpy(&last_measurement_, data, sizeof(struct TofData));
clock_gettime(CLOCK_REALTIME, &last_measurement_ts_);
// save a snapshot to avoid race condition.
m = last_measurement_;
ts = last_measurement_ts_;
pthread_mutex_unlock(&mutex_);
PX4_DEBUG("last_measurement updated distance %d mm, ts %llu ms",
m.distance_mm, ts.tv_sec*1000+(uint64_t)(ts.tv_nsec/1E6));
if (callback_) {
PX4_DEBUG("Notifying user of new measurement result");
callback_(&m, context_);
}
}
int Tof::Start(int interval_ms, DataReadyCallback callback, void* context) {
PX4_DEBUG("Tof::Start() internval %d ms", interval_ms);
int ret;
if (!is_initialized_) {
PX4_ERR("Start() cannot be called without device initialized!");
return -1;
}
if (interval_ms <= 0) {
PX4_ERR("Invalid measurement interval %d", interval_ms);
return -1;
}
callback_ = callback;
context_ = context;
measurement_interval_ms_ = interval_ms;
measurement_should_stop_ = false;
periodic_measurement_running_ = true;
// If device requires user to actively send measurement commands, we create
// a thread (timer) to do that periodically
if (NeedTriggerManually()) {
pthread_attr_t attr;
size_t stacksize = -1;
pthread_attr_init(&attr);
pthread_attr_getstacksize(&attr, &stacksize);
PX4_DEBUG("TX thread stack size: %d", stacksize);
stacksize = 8 * 1024;
PX4_DEBUG("setting the thread stack size to[%d]", stacksize);
pthread_attr_setstacksize(&attr, stacksize);
ret = pthread_create(&tx_thread_, &attr, &Tof::TxTrampoline, this);
if (ret != 0) {
periodic_measurement_running_ = false;
PX4_ERR("Failed to create TX thread in Tof: %d", ret);
return -1;
}
}
return 0;
}
void* Tof::TxTrampoline(void* arg) {
Tof* obj = (Tof*)arg;
return obj->DoPeriodicMeasurement();
}
void* Tof::DoPeriodicMeasurement() {
struct itimerspec timer_spec;
struct sigevent sigev;
sigset_t set;
timer_t timer_id;
int sig;
int rv;
sigev.sigev_notify = SIGEV_SIGNAL;
sigev.sigev_signo = TOF_MEASUREMENT_TIMER_SIGNAL;
sigev.sigev_value.sival_int = TOF_MEASUREMENT_TIMER_SIGNAL;
sigev.sigev_notify_function = 0;
sigev.sigev_notify_attributes = 0;
// create timer
if (timer_create(CLOCK_REALTIME, &sigev, &timer_id) != 0) {
PX4_ERR("timer_create failed");
return NULL;
}
timer_spec.it_value.tv_sec = 0;
timer_spec.it_value.tv_nsec = measurement_interval_ms_*1E6;
timer_spec.it_interval.tv_sec = 0;
timer_spec.it_interval.tv_nsec = measurement_interval_ms_*1E6;
// start the timer
if (timer_settime(timer_id, 0, &timer_spec, NULL) != 0) {
PX4_ERR("timer_settime failed");
timer_delete(timer_id);
return NULL;
}
sigemptyset(&set);
sigaddset(&set, TOF_MEASUREMENT_TIMER_SIGNAL);
PX4_DEBUG("start periodic measurement");
while(!measurement_should_stop_) {
rv = sigwait(&set, &sig);
if (rv != 0 || sig != TOF_MEASUREMENT_TIMER_SIGNAL) {
PX4_ERR("sigwait failed rv %d sig %d", rv, sig);
continue;
}
PX4_DEBUG("waken up by signal %d", sig);
rv = SendMeasurementCommand();
if (rv < 0) {
PX4_ERR("SendMeasurementCommand() failed: %d", rv);
} else {
PX4_DEBUG("Sent measurement command");
}
}
PX4_DEBUG("stop periodic measurement");
// delete the timer
timer_delete(timer_id);
return NULL;
}
int Tof::Stop() {
PX4_DEBUG("Tof::Stop()");
PX4_DEBUG("stopping measurement thread");
measurement_should_stop_ = true;
if (tx_thread_ != 0) {
pthread_join(tx_thread_, NULL);
tx_thread_ = 0;
}
periodic_measurement_running_ = false;
PX4_DEBUG("measurement thread stopped");
return 0;
}
int Tof::DoMeasurement() {
int ret;
struct timespec ts_start, ts_now, old_ts;
bool stop_wait = false;
uint64_t time_lapse_usec;
int i;
PX4_DEBUG("Tof::DoMeasurement()");
if (!is_initialized_) {
PX4_ERR("DoMeasurement() cannot be called without device initialized!");
return -1;
}
// get a snapshot of previous last_measurement_ts_
pthread_mutex_lock(&mutex_);
old_ts = last_measurement_ts_;
pthread_mutex_unlock(&mutex_);
// Send measurement command only if the device is not in
// periodic measurement mode
if (!periodic_measurement_running_) {
ret = SendMeasurementCommand();
if (ret < 0) {
PX4_DEBUG("SendMeasurementCommand() failed: %d", ret);
return -1;
}
}
clock_gettime(CLOCK_REALTIME, &ts_start);
// Wait on new data arrival until new measurement is ready or time out occurs.
// Periodically poll the last measurement result. If the last measurement
// timestamp is within max RTT 20ms, it is considered the new measurement
// data.
while (!stop_wait) {
pthread_mutex_lock(&mutex_);
if (last_measurement_ts_.tv_sec != old_ts.tv_sec ||
last_measurement_ts_.tv_nsec != old_ts.tv_nsec) {
stop_wait = true;
ret = last_measurement_.distance_mm;
pthread_mutex_unlock(&mutex_);
break;
}
pthread_mutex_unlock(&mutex_);
clock_gettime(CLOCK_REALTIME, &ts_now);
time_lapse_usec = TIME_DIFF_USEC(ts_start, ts_now);
PX4_DEBUG("time_lapse_usec %llu", time_lapse_usec);
if (time_lapse_usec > TOF_MEASUREMENT_TIMEOUT_USEC) {
stop_wait = true;
ret = -2;
}
usleep(10000);
}
PX4_DEBUG("DoMeasurement() return %d", ret);
return ret;
}

173
src/drivers/distance_sensor/airlango/repo/tof.h
View File

@ -1,173 +0,0 @@
//
// Copyright (c) 2016 Airlango Ltd. All rights reserved.
//
// @file tof.h
//
// TOF (time of flight) device driver interfaces
//
#pragma once
#include <pthread.h>
// Data info reported to up-layer user
struct TofData {
int distance_mm;
int raw_distance_mm;
};
// Supported TOF device model
enum TofModel {
LANBAO_ISL,
LANBAO_ISL_V2, // hw version 2, with crc16
BENEWAKE_TF_MINI, // Benewake TF_MINI
TOF_MODEL_NUM
};
static inline const char* TofModelToStr(enum TofModel model) {
switch (model) {
case LANBAO_ISL:
return "LANBAO_ISL";
case LANBAO_ISL_V2:
return "LANBAO_ISL_V2";
case BENEWAKE_TF_MINI:
return "BENEWAKE_TF_MINI";
default:
return "Unknown";
}
}
typedef void (*DataReadyCallback)(const struct TofData* data, void* context);
// Tof is a base class to define interfaces for TOF device driver. Sub-classes
// shall provide detailed implementation for specific device to complete
// the measurement functionality.
class Tof {
public:
// Tof class method to get the singleton Tof instance.
// If no device model has been instantiated, this function instantiates
// specified model and return the pointer to the new instance. If the
// specified type has been instantiated, this function returns the
// pointer to the existing instance. If a different model has been
// instantiated, this function returns NULL.
// NOTE:
// 1. This function only instantiates the singleton object. To initialize
// the tof device, need to call Initialize().
// 2. Tof driver is currently not implemented to be thread-safe. We assume
// there is up to 1 user that uses tof driver at a moment.
//
// @param[in] model the tof model enumeration value
//
// @return
// - pointer to the instance on success
// - NULL on error.
static Tof* GetInstance(TofModel model);
virtual ~Tof();
// @brief
// Initialize the Tof driver instance for the specified device path. On
// successful initialization, the Tof device is ready to do measurement.
// For periodic measurement, see Start() function.
//
// @param[in] device
// the dspal serial device path which the tof is connected to
// @param[in] baudrate
// the UART baud rate in bit per second
// @return
// - 0 on success
// - -1 on error
int Initialize(const char* device, int baudrate);
// @brief
// Start periodic measurement at specified interval_ms.
// @param[in] interval_ms measurement interval_ms in millisecond
// @param[in] callback data ready interrupt service routine. This is
// the callback function to be invoked when new measurement data
// is ready.
// @param[in] context address where the context data for callback is stored at
// @return
// - 0 successfully started the measurement
// - -1 device not initialized
// - -2 other errors
int Start(int interval_ms, DataReadyCallback callback, void* context);
// Stop the periodic measurement. If this function is called when measurement
// is not running, this function takes no effect.
// @return
// - 0 on success
// - -1 on error
int Stop();
// Do one time measurement and return the measurement results. This is a
// blocking call.
//
// @return
// - positive integer indicating the distance to the object in millimeter
// - 0 if no object is detected
// - -1 on error
// - -2 on timeout
int DoMeasurement();
virtual TofModel model() const = 0;
protected:
Tof();
static void DataReadyCb(const struct TofData* data, void* context);
void HandleNewMeasurement(const struct TofData* data);
// Initialize the specified serial device with given baudrate. The
// actual initialization operation is model specific, thus this virtual
// method should be implemented by the tof subclass.
// This function is called in Initialize().
//
// @return
// - 0 if device is successfully initialized.
// - -1 on error.
virtual int InitializeInternal(const char* device, int baudrate) = 0;
// Start RX thread to process incoming byte stream amd register the provided
// callback function as data ready callback. On success, when
// As the tof data parser is model dependent, thus this virtual method
// shall be implemented by the tof subclass. When new tof measurement
// data is ready, the subclass instance invokes the callback immediately.
// context is pass to the callback function as argument.
// This method is called in Initialize().
//
// @return
// - 0 on success
// -1 on error.
virtual int StartRxThread(DataReadyCallback callback, void* context) = 0;
// Send one time measurement command to tof device. This tof communication
// protocol is device specific and thus this virtual method needs to be
// implemented by tof subclass.
//
// @return
// - 0 on success,
// - -1 on error
virtual int SendMeasurementCommand() = 0;
// Some devices will periodically response measurement data once it gets
// initialized. While some other devices might need user trigger measurement
// operation manually.
virtual bool NeedTriggerManually() const { return false; }
static void* TxTrampoline(void* arg);
void* DoPeriodicMeasurement();
private:
static Tof* instance_;
bool is_initialized_;
pthread_mutex_t mutex_;
struct TofData last_measurement_;
struct timespec last_measurement_ts_;
DataReadyCallback callback_;
void* context_;
pthread_t tx_thread_;
int measurement_interval_ms_;
bool measurement_should_stop_;
bool periodic_measurement_running_;
};

370
src/drivers/distance_sensor/airlango/repo/tof_main.cpp
View File

@ -1,370 +0,0 @@
/****************************************************************************
* Copyright (c) 2016 Airlango, Inc. All rights reserved.
*
****************************************************************************/
/**
* @file tof_main.cpp
*
* TOF device driver task
*/
#include <px4_includes.h>
#include <px4_getopt.h>
#include <px4_tasks.h>
#include <px4_log.h>
#include <stdlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_hrt.h>
#include <tof.h>
#include <systemlib/param/param.h>
//#define NO_ADIAG_LOG
//#define NO_ADIAG_STATS
#include <adiag.h>
/** driver 'main' command */
extern "C" { __EXPORT int tof_main(int argc, char *argv[]); }
/*
* Default parameters for tof driver
*/
#define TOF_DEFAULT_BAUDRATE 115200
#define TOF_DEFAULT_INTERVAL 100 /* milliseconds */
#define MAX_LEN_DEV_PATH 32
static param_t algo_aes;
namespace tof
{
/** device path that TOF is connected to */
static char _device[MAX_LEN_DEV_PATH];
/** serial device speed (uart baudrate) */
static int _baudrate = TOF_DEFAULT_BAUDRATE;
/** sampling frequency in Hz */
static int _frequency = 10;
/** tof device model */
static int _device_model = 0;
/** flag indicating if TOF driver task is running */
static bool _is_running = false;
/** flag indicating if TOF driver task should stop */
static bool _task_should_stop = false;
/** handle to the task main thread */
static px4_task_t _task_handle = -1;
/** TOF measurement data */
static struct TofData _data;
/** TOF data publication */
static orb_advert_t _tof_pub = nullptr;
/** Print out the usage information */
static void usage();
/** TOF start measurement */
static void start();
/** TOF stop measurement */
static void stop();
/** task main trampoline function */
static void task_main_trampoline(int argc, char *argv[]);
/** TOF measurement thread primary entry point */
static void task_main(int argc, char *argv[]);
void tof_rx_callback(const TofData* data, void* context)
{
(void)context;
/* copy out the TOF data */
_data = *data;
/* send signal to measurement thread */
px4_task_kill(_task_handle, SIGRTMIN);
}
void publish_reports()
{
algo_aes = param_find("ALGO_AES");
int aes;
if (param_get(algo_aes, &aes) == 0) {
// AD_DEBUG("ALGO_AES %d", aes);
}
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.current_distance =
static_cast<float>(_data.distance_mm) / 1000; /* in metre */
// PX4_WARN("tof-avg %d, tof-raw %d", _data.distance_mm, _data.raw_distance_mm);
AD_STATS("tof_time %llu, tof-avg %d, tof-raw %d",report.timestamp, _data.distance_mm, _data.raw_distance_mm);
// TODO: subject to tune
if (report.current_distance < 0.17f) {
// NOTE(xiaoming@airlango.com): add a random noise to avoid round up error in blockstats deviation calculation
report.current_distance = 0.15f + float(rand())/100000000000000;
// AD_DEBUG("RANDOM %f", float(rand())/100000000000000);
}
switch (aes) {
case 0:
report.min_distance = 0.1f;
break;
case 1:
report.min_distance = 0.01f;
break;
default:
report.min_distance = 0.17f;
break;
}
report.max_distance = 10.0f;
report.orientation = 25;//downward facing
report.covariance = 0.002f;
// report.covariance = 0.01f;
/* TODO: set proper ID */
report.id = 90;
if (_tof_pub == nullptr) {
_tof_pub = orb_advertise(ORB_ID(distance_sensor), &report);
} else {
orb_publish(ORB_ID(distance_sensor), _tof_pub, &report);
}
// PX4_WARN("Published distance sensor data: %.3f m", report.current_distance);
}
void task_main(int argc, char *argv[])
{
PX4_WARN("enter tof task_main");
int interval_ms = TOF_DEFAULT_INTERVAL;
if (_frequency > 0) {
interval_ms = 1000 / _frequency;
}
/*
* initialize signal
*/
sigset_t set;
sigemptyset(&set);
sigaddset(&set, SIGRTMIN);
/*
* start tof driver
*/
TofModel tof_model = static_cast<TofModel>(_device_model);
Tof* driver = Tof::GetInstance(tof_model);
if (driver == nullptr) {
PX4_ERR("fail to instantiate tof driver");
goto _failure;
}
if (driver->Initialize(_device, _baudrate) < 0) {
PX4_ERR("fail to initialize tof driver");
goto _failure;
}
if (driver->Start(interval_ms, tof_rx_callback, nullptr) < 0) {
PX4_ERR("fail to start tof driver");
goto _failure;
}
/*
* enter working loop
*/
while (!_task_should_stop) {
/* wait on signal */
int sig = 0;
int rv = sigwait(&set, &sig);
/* check if we are waken up by the proper signal */
if (rv != 0 || sig != SIGRTMIN) {
PX4_WARN("sigwait failed rv %d sig %d", rv, sig);
continue;
}
/* publish distance sensor reports */
publish_reports();
}
_failure:
PX4_WARN("closing tof");
if (driver != nullptr) {
driver->Stop();
delete driver;
}
_is_running = false;
}
void task_main_trampoline(int argc, char *argv[])
{
PX4_WARN("task_main_trampoline");
task_main(argc, argv);
}
void start()
{
ASSERT(_task_handle == -1);
_task_handle = px4_task_spawn_cmd("tof_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
(px4_main_t)&task_main_trampoline,
nullptr);
if (_task_handle < 0) {
PX4_WARN("tof task start failed");
return;
}
_is_running = true;
}
void stop()
{
_task_should_stop = true;
_is_running = false;
_task_handle = -1;
}
void usage()
{
PX4_WARN("missing command: try 'start', 'stop', 'status'");
PX4_WARN("options:");
PX4_WARN(" -D device device path, e.g. /dev/tty-1");
PX4_WARN(" -F frequency sampling frequency (Hz), default to 10");
PX4_WARN(" -M model device hardware model (0: LANBAO_ISL)");
}
}; // namespace tof
int tof_main(int argc, char* argv[])
{
int ch;
int myoptind = 1;
const char* myoptarg = nullptr;
const char* device = nullptr;
const char* frequency = nullptr;
const char* device_model = nullptr;
const char* baudrate = nullptr;
if (argc < 2) {
tof::usage();
return -1;
}
while ((ch = px4_getopt(argc, argv, "D:F:M:B:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'D':
device = myoptarg;
break;
case 'F':
frequency = myoptarg;
break;
case 'M':
device_model = myoptarg;
break;
case 'B':
baudrate = myoptarg;
break;
default:
tof::usage();
return -1;
}
}
if (device == NULL || strlen(device) == 0) {
tof::usage();
return -1;
}
memset(tof::_device, 0, MAX_LEN_DEV_PATH);
strncpy(tof::_device, device, MAX_LEN_DEV_PATH - 1);
if (frequency != nullptr) {
char *endptr;
long val = strtoul(frequency, &endptr, 0);
if ((errno == ERANGE && (val == LONG_MAX || val == LONG_MIN))
|| (errno != 0 && val == 0)
|| (*endptr != '\0')) {
PX4_WARN("Invalid parameter for frequency, ignore");
} else {
tof::_frequency = val;
}
}
if (device_model != nullptr) {
char *endptr;
long val = strtoul(device_model, &endptr, 0);
if ((errno == ERANGE && (val == LONG_MAX || val == LONG_MIN))
|| (errno != 0 && val == 0)
|| (*endptr != '\0')) {
PX4_WARN("Invalid parameter for device_model, ignore");
} else {
tof::_device_model = val;
}
}
if (baudrate != nullptr) {
char *endptr;
long val = strtoul(baudrate, &endptr, 0);
if ((errno == ERANGE && (val == LONG_MAX || val == LONG_MIN))
|| (errno != 0 && val == 0)
|| (*endptr != '\0')) {
PX4_WARN("Invalid parameter for baudrate, ignore");
} else {
tof::_baudrate = val;
}
}
const char* verb = argv[myoptind];
if (!strcmp(verb, "start")) {
if (tof::_is_running) {
PX4_WARN("tof already running");
return 1;
}
tof::start();
} else if (!strcmp(verb, "stop")) {
if (!tof::_is_running) {
PX4_WARN("tof is not running");
return 1;
}
tof::stop();
} else if (!strcmp(verb, "status")) {
PX4_WARN("tof is %s", tof::_is_running ? "running" : "stopped");
return 0;
} else {
tof::usage();
return -1;
}
return 0;
}

0
src/drivers/distance_sensor/airlango/CMakeLists.txt → src/drivers/distance_sensor/isl2950/CMakeLists.txt
View File

934
src/drivers/distance_sensor/isl2950/isl2950.cpp Normal file
View File

@ -0,0 +1,934 @@
/****************************************************************************
*
* 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 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;
}
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;
// ----------------------- 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
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);
}
}
//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));
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));
}
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;
}

183
src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.cpp → src/drivers/distance_sensor/isl2950/isl2950_parser.cpp
View File

@ -1,18 +1,56 @@
//
// Copyright (c) 2016 Airlango Ltd. All rights reserved.
//
// @file lanbao_isl.cpp
//
// Device driver implementaion for Lanbao ISL29501 hardware version 2, which
// uses crc-16 for checksum calculation.
// Also see http://zhangxu1018.blog.sohu.com/161752060.html
//
#include <systemlib/err.h>
#include "lanbao_isl_v2.h"
/****************************************************************************
*
* 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,
@ -63,6 +101,18 @@ static const UCHAR aucCRCLo[] = {
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;
@ -75,104 +125,81 @@ USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) {
return (USHORT)(ucCRCHi << 8 | ucCRCLo);
}
LanbaoIslV2::LanbaoIslV2() {
PX4_DEBUG("LanbaoIslV2 ctor");
}
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;
LanbaoIslV2::~LanbaoIslV2() {
PX4_DEBUG("LanbaoIslV2 dtor");
}
// 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;
static unsigned char frame_data[TOF_CRC_CALC_DATA_LEN] = {
TOF_SFD1, TOF_SFD2, 0, 0
};
// uint8_t b = buffer[bytes_processed++]; // Can be removed
// printf("parse byte 0x%02X \n", b);
int LanbaoIslV2::Parse(const uint8_t* buffer, int length, bool* full_frame) {
static TofFramingState state = TFS_NOT_STARTED;
static uint16_t crc16 = 0;
int bytes_processed = 0;
PX4_DEBUG("LanbaoTofV2::Parse()");
while (bytes_processed < length) {
uint8_t b = buffer[bytes_processed++];
PX4_DEBUG("parse byte 0x%02X", b);
switch (state) {
switch (*state) {
case TFS_NOT_STARTED:
if (b == TOF_SFD1) {
state = TFS_GOT_SFD1;
PX4_DEBUG("Got SFD1");
if (c == TOF_SFD1) {
*state = TFS_GOT_SFD1;
//printf("Got SFD1 \n");
}
break;
case TFS_GOT_SFD1:
if (b == TOF_SFD2) {
state = TFS_GOT_SFD2;
PX4_DEBUG("Got SFD2");
} else if (b == TOF_SFD1) {
state = TFS_GOT_SFD1;
PX4_DEBUG("Discard previous SFD1, Got new 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;
*state = TFS_NOT_STARTED;
}
break;
case TFS_GOT_SFD2:
frame_data[TOF_DISTANCE_MSB_POS] = b;
state = TFS_GOT_DATA1;
PX4_DEBUG("Got DATA1 0x%02X", b);
*state = TFS_GOT_DATA1;
parserbuf[TOF_DISTANCE_MSB_POS] = c; // MSB Data
//printf("Got DATA1 0x%02X \n", c);
break;
case TFS_GOT_DATA1:
frame_data[TOF_DISTANCE_LSB_POS] = b;
state = TFS_GOT_DATA2;
PX4_DEBUG("Got DATA2 0x%02X", b);
*state = TFS_GOT_DATA2;
parserbuf[TOF_DISTANCE_LSB_POS] = c; // LSB Data
//printf("Got DATA2 0x%02X \n", c);
// do crc calculation
crc16 = usMBCRC16(frame_data, TOF_CRC_CALC_DATA_LEN);
*crc16 = usMBCRC16(parserbuf, TOF_CRC_CALC_DATA_LEN);
// convert endian
crc16 = (crc16 >> 8) | (crc16 << 8);
*crc16 = (*crc16 >> 8) | (*crc16 << 8);
break;
case TFS_GOT_DATA2:
if (b == (crc16 >> 8)) {
state = TFS_GOT_CHECKSUM1;
if (c == (*crc16 >> 8)) {
*state = TFS_GOT_CHECKSUM1;
} else {
PX4_DEBUG("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X",
b, crc16);
state = TFS_NOT_STARTED;
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 (b == (crc16 & 0xFF)) {
PX4_DEBUG("Checksum verified");
data_.distance_mm = (frame_data[TOF_DISTANCE_MSB_POS] << 8)
| frame_data[TOF_DISTANCE_LSB_POS];
*full_frame = true;
return bytes_processed;
} else {
PX4_DEBUG("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X",
b, crc16);
}
*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:
PX4_DEBUG("This should never happen.")
printf("This should never happen. \n");
break;
}
}
return bytes_processed;
// SOME STUFFS
return ret;
}

0
src/drivers/distance_sensor/airlango/isl2950_parser.h → src/drivers/distance_sensor/isl2950/isl2950_parser.h
View File