mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-30 15:04:07 +08:00
crazyflie: clean up
This commit is contained in:
DanielePettenuzzo
Beat Küng
parent
22868dd5a4
commit
9cad11d832
@ -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 :
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1,8 +1,6 @@
|
||||
px4_add_module(
|
||||
MODULE drivers__pmw3901
|
||||
MAIN pmw3901
|
||||
COMPILE_FLAGS
|
||||
-Wno-sign-compare
|
||||
SRCS
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user