From 93b3cf241bc0fbb22c55785dd24b48c0c7a7b609 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Mon, 17 Dec 2018 15:42:43 +0100 Subject: [PATCH] Renamed driver folder. Signed-off-by: Claudio Micheli --- src/drivers/distance_sensor/CMakeLists.txt | 2 +- .../airlango/repo/CMakeLists_old.txt | 44 - .../airlango/repo/lanbao_isl.cpp | 305 ------ .../airlango/repo/lanbao_isl.h | 155 --- .../airlango/repo/lanbao_isl_v2.h | 23 - .../distance_sensor/airlango/repo/tof.cpp | 345 ------- .../distance_sensor/airlango/repo/tof.h | 173 ---- .../airlango/repo/tof_main.cpp | 370 ------- .../{airlango => isl2950}/CMakeLists.txt | 0 .../distance_sensor/isl2950/isl2950.cpp | 934 ++++++++++++++++++ .../isl2950_parser.cpp} | 183 ++-- .../{airlango => isl2950}/isl2950_parser.h | 0 12 files changed, 1040 insertions(+), 1494 deletions(-) delete mode 100644 src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt delete mode 100644 src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp delete mode 100644 src/drivers/distance_sensor/airlango/repo/lanbao_isl.h delete mode 100644 src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h delete mode 100644 src/drivers/distance_sensor/airlango/repo/tof.cpp delete mode 100644 src/drivers/distance_sensor/airlango/repo/tof.h delete mode 100644 src/drivers/distance_sensor/airlango/repo/tof_main.cpp rename src/drivers/distance_sensor/{airlango => isl2950}/CMakeLists.txt (100%) create mode 100644 src/drivers/distance_sensor/isl2950/isl2950.cpp rename src/drivers/distance_sensor/{airlango/repo/lanbao_isl_v2.cpp => isl2950/isl2950_parser.cpp} (53%) rename src/drivers/distance_sensor/{airlango => isl2950}/isl2950_parser.h (100%) diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index 093e6cf573..c6707d6228 100644 --- a/src/drivers/distance_sensor/CMakeLists.txt +++ b/src/drivers/distance_sensor/CMakeLists.txt @@ -43,4 +43,4 @@ add_subdirectory(ulanding) add_subdirectory(leddar_one) add_subdirectory(vl53lxx) add_subdirectory(pga460) -add_subdirectory(airlango) +add_subdirectory(isl2950) diff --git a/src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt b/src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt deleted file mode 100644 index 2194da350b..0000000000 --- a/src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt +++ /dev/null @@ -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" -# ) diff --git a/src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp b/src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp deleted file mode 100644 index 36fbbee934..0000000000 --- a/src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// -// Copyright (c) 2016 Airlango Ltd. All rights reserved. -// -// @file lanbao_isl.cpp -// -// Device driver implementaion for Lanbao ISL29501 -// -#include -#include -#include -#include -#include -#include -#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(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; -} diff --git a/src/drivers/distance_sensor/airlango/repo/lanbao_isl.h b/src/drivers/distance_sensor/airlango/repo/lanbao_isl.h deleted file mode 100644 index 9f52686454..0000000000 --- a/src/drivers/distance_sensor/airlango/repo/lanbao_isl.h +++ /dev/null @@ -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 -#include -#include -#include "tof.h" -#include "adiag.h" - -// Average filter -template -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 distance_filter_; -}; diff --git a/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h b/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h deleted file mode 100644 index 4247a19b2d..0000000000 --- a/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h +++ /dev/null @@ -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 diff --git a/src/drivers/distance_sensor/airlango/repo/tof.cpp b/src/drivers/distance_sensor/airlango/repo/tof.cpp deleted file mode 100644 index cdc5220535..0000000000 --- a/src/drivers/distance_sensor/airlango/repo/tof.cpp +++ /dev/null @@ -1,345 +0,0 @@ -// -// Copyright (c) 2016 Airlango Ltd. All rights reserved. -// -// @file tof.cpp -// -// Basic implementation for TOF device driver. -// -#include -#include -#include -#include -#include -#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; -} diff --git a/src/drivers/distance_sensor/airlango/repo/tof.h b/src/drivers/distance_sensor/airlango/repo/tof.h deleted file mode 100644 index 7564ca4f39..0000000000 --- a/src/drivers/distance_sensor/airlango/repo/tof.h +++ /dev/null @@ -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 - -// 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_; -}; diff --git a/src/drivers/distance_sensor/airlango/repo/tof_main.cpp b/src/drivers/distance_sensor/airlango/repo/tof_main.cpp deleted file mode 100644 index abd8d1ba72..0000000000 --- a/src/drivers/distance_sensor/airlango/repo/tof_main.cpp +++ /dev/null @@ -1,370 +0,0 @@ -/**************************************************************************** - * Copyright (c) 2016 Airlango, Inc. All rights reserved. - * - ****************************************************************************/ - -/** - * @file tof_main.cpp - * - * TOF device driver task - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//#define NO_ADIAG_LOG -//#define NO_ADIAG_STATS -#include - -/** 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(_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(_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; -} diff --git a/src/drivers/distance_sensor/airlango/CMakeLists.txt b/src/drivers/distance_sensor/isl2950/CMakeLists.txt similarity index 100% rename from src/drivers/distance_sensor/airlango/CMakeLists.txt rename to src/drivers/distance_sensor/isl2950/CMakeLists.txt diff --git a/src/drivers/distance_sensor/isl2950/isl2950.cpp b/src/drivers/distance_sensor/isl2950/isl2950.cpp new file mode 100644 index 0000000000..30dbe88308 --- /dev/null +++ b/src/drivers/distance_sensor/isl2950/isl2950.cpp @@ -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 + * + * Driver for the Lanbao ISL2950 + */ + + #include + #include + #include + + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + + #include + + #include + #include + #include + #include + + #include + #include + + #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(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; +} diff --git a/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.cpp b/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp similarity index 53% rename from src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.cpp rename to src/drivers/distance_sensor/isl2950/isl2950_parser.cpp index ab1d3b350e..468e02299a 100644 --- a/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.cpp +++ b/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp @@ -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 -#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 +#include +#include + +#include "isl2950_parser.h" +#include +#include 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; } diff --git a/src/drivers/distance_sensor/airlango/isl2950_parser.h b/src/drivers/distance_sensor/isl2950/isl2950_parser.h similarity index 100% rename from src/drivers/distance_sensor/airlango/isl2950_parser.h rename to src/drivers/distance_sensor/isl2950/isl2950_parser.h