From 9cad11d8320a8aeb9b3fe27d51162f93f005b526 Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Sat, 19 May 2018 13:28:43 +0200 Subject: [PATCH] crazyflie: clean up --- .../distance_sensor/vl53lxx/CMakeLists.txt | 4 -- .../distance_sensor/vl53lxx/vl53lxx.cpp | 65 +++++++++---------- src/drivers/pmw3901/CMakeLists.txt | 2 - src/drivers/pmw3901/pmw3901.cpp | 57 ++++++++-------- 4 files changed, 62 insertions(+), 66 deletions(-) diff --git a/src/drivers/distance_sensor/vl53lxx/CMakeLists.txt b/src/drivers/distance_sensor/vl53lxx/CMakeLists.txt index 4d53ea31a9..e76c5f184c 100644 --- a/src/drivers/distance_sensor/vl53lxx/CMakeLists.txt +++ b/src/drivers/distance_sensor/vl53lxx/CMakeLists.txt @@ -1,11 +1,7 @@ px4_add_module( MODULE drivers__vl53lxx MAIN vl53lxx - COMPILE_FLAGS - -Wno-sign-compare SRCS vl53lxx.cpp - DEPENDS - platforms__common ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : \ No newline at end of file diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index 6c0be20e49..ed3adbb760 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 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 @@ -103,7 +103,7 @@ #define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0 #define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA -#define VL53LXX_MS 1000 /* 1ms */ +#define VL53LXX_US 1000 /* 1ms */ #define VL53LXX_SAMPLE_RATE 50000 /* 50ms */ #define VL53LXX_MAX_RANGING_DISTANCE 2.0f @@ -149,6 +149,7 @@ private: int _orb_class_instance; orb_advert_t _distance_sensor_topic; + orb_advert_t _subsystem_pub; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -213,6 +214,7 @@ VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) : _class_instance(-1), _orb_class_instance(-1), _distance_sensor_topic(nullptr), + _subsystem_pub(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "vl53lxx_read")), _comms_errors(perf_alloc(PC_COUNT, "vl53lxx_com_err")) { @@ -472,8 +474,8 @@ int VL53LXX::readRegister(uint8_t reg_address, uint8_t &value) { int ret; - uint8_t val = 0; + /* write register address to the sensor */ ret = transfer(®_address, sizeof(reg_address), nullptr, 0); if (OK != ret) { @@ -483,17 +485,14 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value) } /* read from the sensor */ - ret = transfer(nullptr, 0, &val, 1); + ret = transfer(nullptr, 0, &value, 1); - if (ret < 0) { + if (OK != ret) { DEVICE_LOG("error reading from sensor: %d", ret); perf_count(_comms_errors); return ret; } - ret = OK; - value = val; - return ret; } @@ -503,7 +502,8 @@ int VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length) { int ret; - uint8_t val[6] = {0, 0, 0, 0, 0, 0}; + + /* write register address to the sensor */ ret = transfer(®_address, 1, nullptr, 0); if (OK != ret) { @@ -512,18 +512,14 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length) } /* read from the sensor */ - ret = transfer(nullptr, 0, &val[0], length); + ret = transfer(nullptr, 0, &value[0], length); - if (ret < 0) { + if (OK != ret) { DEVICE_LOG("error reading from sensor: %d", ret); perf_count(_comms_errors); return ret; } - memcpy(&value[0], &val[0], length); - - ret = OK; - return ret; } @@ -546,8 +542,6 @@ VL53LXX::writeRegister(uint8_t reg_address, uint8_t value) return ret; } - ret = OK; - return ret; } @@ -555,11 +549,16 @@ VL53LXX::writeRegister(uint8_t reg_address, uint8_t value) int VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t *value, - uint8_t length) // bytes are send in order as they are in the array + uint8_t length) /* bytes are send in order as they are in the array */ { - // be careful for uint16_t to send first higher byte + /* be careful: for uint16_t to send first higher byte */ int ret; - uint8_t cmd[6] = {0, 0, 0, 0, 0, 0}; + uint8_t cmd[7] = {0, 0, 0, 0, 0, 0, 0}; + + if (length > 6 || length < 1) { + DEVICE_LOG("VL53LXX::writeRegisterMulti length out of range"); + return PX4_ERROR; + } cmd[0] = reg_address; @@ -573,8 +572,6 @@ VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t *value, return ret; } - ret = OK; - return ret; } @@ -610,7 +607,7 @@ VL53LXX::measure() if ((system_start & 0x01) == 1) { work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - USEC2TICK(VL53LXX_MS)); // reschedule every 1 ms until measurement is ready + USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready ret = OK; return ret; @@ -626,7 +623,7 @@ VL53LXX::measure() if ((system_start & 0x01) == 1) { work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - USEC2TICK(VL53LXX_MS)); // reschedule every 1 ms until measurement is ready + USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready ret = OK; return ret; @@ -639,7 +636,7 @@ VL53LXX::measure() if ((wait_for_measurement & 0x07) == 0) { work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - USEC2TICK(VL53LXX_MS)); // reschedule every 1 ms until measurement is ready + USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready ret = OK; return ret; } @@ -722,22 +719,22 @@ VL53LXX::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, USEC2TICK(VL53LXX_MS)); + work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, USEC2TICK(VL53LXX_US)); /* notify about state change */ struct subsystem_info_s info = {}; + + info.timestamp = hrt_absolute_time(); info.present = true; info.enabled = true; info.ok = true; info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER; - static orb_advert_t pub = nullptr; - - if (pub != nullptr) { - orb_publish(ORB_ID(subsystem_info), pub, &info); + if (_subsystem_pub != nullptr) { + orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); + _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); } } @@ -879,7 +876,7 @@ VL53LXX::spadCalculations() writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // restore config - return true; + return OK; } @@ -969,7 +966,7 @@ VL53LXX::sensorTuning() writeRegister(0xFF, 0x00); writeRegister(0x80, 0x00); - return true; + return OK; } @@ -987,7 +984,7 @@ VL53LXX::singleRefCalibration(uint8_t byte) writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); writeRegister(SYSRANGE_START_REG, 0x00); - return true; + return OK; } diff --git a/src/drivers/pmw3901/CMakeLists.txt b/src/drivers/pmw3901/CMakeLists.txt index e6c65ab87c..8ec5b5da2c 100644 --- a/src/drivers/pmw3901/CMakeLists.txt +++ b/src/drivers/pmw3901/CMakeLists.txt @@ -1,8 +1,6 @@ px4_add_module( MODULE drivers__pmw3901 MAIN pmw3901 - COMPILE_FLAGS - -Wno-sign-compare SRCS pmw3901.cpp ) diff --git a/src/drivers/pmw3901/pmw3901.cpp b/src/drivers/pmw3901/pmw3901.cpp index e0b35ab4fa..2463833da5 100644 --- a/src/drivers/pmw3901/pmw3901.cpp +++ b/src/drivers/pmw3901/pmw3901.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 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 @@ -97,7 +97,7 @@ #define PMW3901_DEVICE_PATH "/dev/pmw3901" /* PMW3901 Registers addresses */ -#define PMW3901_MS 1000 /* 1 ms */ +#define PMW3901_US 1000 /* 1 ms */ #define PMW3901_SAMPLE_RATE 10000 /* 10 ms */ @@ -123,6 +123,9 @@ public: */ void print_info(); +protected: + virtual int probe(); + private: uint8_t _rotation; work_s _work; @@ -133,6 +136,7 @@ private: int _orb_class_instance; orb_advert_t _optical_flow_pub; + orb_advert_t _subsystem_pub; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -198,13 +202,13 @@ PMW3901::PMW3901(uint8_t rotation, int bus) : _class_instance(-1), _orb_class_instance(-1), _optical_flow_pub(nullptr), + _subsystem_pub(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901_read")), _comms_errors(perf_alloc(PC_COUNT, "pmw3901_com_err")), _previous_collect_timestamp(0), _sensor_rotation((enum Rotation)rotation) { - // enable debug() calls _debug_enabled = false; @@ -231,20 +235,12 @@ PMW3901::~PMW3901() int PMW3901::sensorInit() { - int ret; uint8_t data[5]; - // initialize pmw3901 flow sensor - readRegister(0x00, &data[0], 1); // chip id - readRegister(0x5F, &data[1], 1); // inverse chip id - // Power on reset writeRegister(0x3A, 0x5A); usleep(5000); - // Test the SPI communication, checking chipId and inverse chipId - if (data[0] != 0x49 && data[1] != 0xB8) { return false; } - // Reading the motion registers one time readRegister(0x02, &data[0], 1); readRegister(0x03, &data[1], 1); @@ -336,9 +332,7 @@ PMW3901::sensorInit() writeRegister(0x5A, 0x10); writeRegister(0x54, 0x00); - ret = OK; - - return ret; + return OK; } @@ -355,8 +349,6 @@ PMW3901::init() goto out; } - set_frequency(PMW3901_SPI_BUS_SPEED); - sensorInit(); /* allocate basic report buffers */ @@ -376,6 +368,23 @@ out: } +int +PMW3901::probe() +{ + uint8_t data[2] = { 0, 0 }; + + readRegister(0x00, &data[0], 1); // chip id + + // Test the SPI communication, checking chipId and inverse chipId + if (data[0] == 0x49) { + return OK; + } + + // not found on any address + return -EIO; +} + + int PMW3901::ioctl(device::file_t *filp, int cmd, unsigned long arg) { @@ -512,8 +521,6 @@ PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count) memcpy(&data[0], &cmd[1], count); - ret = OK; - return ret; } @@ -536,8 +543,6 @@ PMW3901::writeRegister(unsigned reg, uint8_t data) return ret; } - ret = OK; - return ret; } @@ -644,22 +649,22 @@ PMW3901::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, USEC2TICK(PMW3901_MS)); + work_queue(LPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, USEC2TICK(PMW3901_US)); /* notify about state change */ struct subsystem_info_s info = {}; + + info.timestamp = hrt_absolute_time(); info.present = true; info.enabled = true; info.ok = true; info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW; - static orb_advert_t pub = nullptr; - - if (pub != nullptr) { - orb_publish(ORB_ID(subsystem_info), pub, &info); + if (_subsystem_pub != nullptr) { + orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); + _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); } }