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

vl53lxx and pmw3901 drivers: style fix

sbg
DanielePettenuzzo 7 years ago committed by Beat Küng
parent
4098d50ff9
commit
2d20f31a70
2 changed files with 368 additions and 365 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 451
      src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp
  2. 282
      src/drivers/pmw3901/pmw3901.cpp

451
src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp
Unescape View File

@ -92,7 +92,7 @@
#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6 #define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6
#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 #define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0
#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A #define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A
#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 #define SYSTEM_SEQUENCE_CONFIG_REG 0x01
#define SYSRANGE_START_REG 0x00 #define SYSRANGE_START_REG 0x00
#define RESULT_INTERRUPT_STATUS_REG 0x13 #define RESULT_INTERRUPT_STATUS_REG 0x13
#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B
@ -115,7 +115,7 @@ class VL53LXX : public device::I2C
{ {
public: public:
VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
int bus = VL53LXX_BUS, int address = VL53LXX_BASEADDR); int bus = VL53LXX_BUS, int address = VL53LXX_BASEADDR);
virtual ~VL53LXX(); virtual ~VL53LXX();
@ -141,7 +141,7 @@ private:
int _class_instance; int _class_instance;
int _orb_class_instance; int _orb_class_instance;
orb_advert_t _distance_sensor_topic; orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
@ -166,7 +166,7 @@ private:
/** /**
* Perform a poll cycle; collect from the previous measurement * Perform a poll cycle; collect from the previous measurement
* and start a new one. * and start a new one.
*/ */
void cycle(); void cycle();
int measure(); int measure();
int collect(); int collect();
@ -174,7 +174,7 @@ private:
int readRegister(uint8_t reg_address, uint8_t &value); int readRegister(uint8_t reg_address, uint8_t &value);
int writeRegister(uint8_t reg_address, uint8_t value); int writeRegister(uint8_t reg_address, uint8_t value);
int writeRegisterMulti(uint8_t reg_address, uint8_t* value, uint8_t length); int writeRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length);
int readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length); int readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length);
int sensorInit(); int sensorInit();
@ -243,7 +243,7 @@ VL53LXX::~VL53LXX()
} }
int int
VL53LXX::sensorInit() VL53LXX::sensorInit()
{ {
uint8_t val = 0; uint8_t val = 0;
@ -255,7 +255,7 @@ VL53LXX::sensorInit()
readRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val); readRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val);
writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01); writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01);
// set I2C to standard mode // set I2C to standard mode
writeRegister(0x88, 0x00); writeRegister(0x88, 0x00);
writeRegister(0x80, 0x01); writeRegister(0x80, 0x01);
@ -342,52 +342,53 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
switch (arg) { switch (arg) {
case 0: case 0:
return -EINVAL; return -EINVAL;
case SENSOR_POLLRATE_DEFAULT: { case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */ /* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0); bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */ /* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(VL53LXX_CONVERSION_INTERVAL); _measure_ticks = USEC2TICK(VL53LXX_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) { if (want_start) {
start();
}
start(); start();
}
return OK; start();
}
case SENSOR_POLLRATE_MANUAL: { return OK;
stop();
_measure_ticks = 0;
return OK;
} }
/* adjust to a legal polling interval in Hz */ case SENSOR_POLLRATE_MANUAL: {
default: {
/* do we need to start internal polling? */ stop();
bool want_start = (_measure_ticks == 0); _measure_ticks = 0;
return OK;
}
/* convert hz to tick interval via microseconds */ /* adjust to a legal polling interval in Hz */
unsigned ticks = USEC2TICK(1000000 / arg); default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* update interval for next measurement */ /* convert hz to tick interval via microseconds */
_measure_ticks = ticks; unsigned ticks = USEC2TICK(1000000 / arg);
/* if we need to start the poll state machine, do it */ /* update interval for next measurement */
if (want_start) { _measure_ticks = ticks;
start();
}
return OK; /* if we need to start the poll state machine, do it */
if (want_start) {
start();
} }
return OK;
} }
} }
}
default: default:
/* give it to the superclass */ /* give it to the superclass */
@ -454,7 +455,7 @@ VL53LXX::read(device::file_t *filp, char *buffer, size_t buflen)
} }
int int
VL53LXX::readRegister(uint8_t reg_address, uint8_t &value) VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
{ {
int ret; int ret;
@ -469,7 +470,7 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
} }
/* wait for it to complete */ /* wait for it to complete */
usleep(VL53LXX_CONVERSION_INTERVAL); usleep(VL53LXX_CONVERSION_INTERVAL);
/* read from the sensor */ /* read from the sensor */
ret = transfer(nullptr, 0, &val, 1); ret = transfer(nullptr, 0, &val, 1);
@ -488,11 +489,11 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
} }
int int
VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length) VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length)
{ {
int ret; int ret;
uint8_t val[6] = {0, 0, 0, 0, 0, 0}; uint8_t val[6] = {0, 0, 0, 0, 0, 0};
ret = transfer(&reg_address, 1, nullptr, 0); ret = transfer(&reg_address, 1, nullptr, 0);
if (OK != ret) { if (OK != ret) {
@ -515,13 +516,13 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length)
memcpy(&value[0], &val[0], length); memcpy(&value[0], &val[0], length);
ret = OK; ret = OK;
return ret; return ret;
} }
int int
VL53LXX::writeRegister(uint8_t reg_address, uint8_t value) VL53LXX::writeRegister(uint8_t reg_address, uint8_t value)
{ {
int ret; int ret;
@ -545,17 +546,19 @@ VL53LXX::writeRegister(uint8_t reg_address, uint8_t value)
} }
int int
VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t* value, uint8_t length) // bytes are send in order as they are in the array VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t *value,
{ // be careful for uint16_t to send first higher byte uint8_t length) // bytes are send in order as they are in the array
{
// be careful for uint16_t to send first higher byte
int ret; int ret;
uint8_t cmd[6] = {0, 0, 0, 0, 0, 0}; uint8_t cmd[6] = {0, 0, 0, 0, 0, 0};
cmd[0] = reg_address; cmd[0] = reg_address;
memcpy(&cmd[1], &value[0], length); memcpy(&cmd[1], &value[0], length);
ret = transfer(&cmd[0], length+1, nullptr, 0); ret = transfer(&cmd[0], length + 1, nullptr, 0);
if (OK != ret) { if (OK != ret) {
perf_count(_comms_errors); perf_count(_comms_errors);
@ -582,7 +585,7 @@ VL53LXX::measure()
*/ */
const uint8_t cmd = RESULT_RANGE_STATUS_REG + 10; const uint8_t cmd = RESULT_RANGE_STATUS_REG + 10;
if(_new_measurement) { if (_new_measurement) {
_new_measurement = false; _new_measurement = false;
@ -595,11 +598,11 @@ VL53LXX::measure()
writeRegister(0x80, 0x00); writeRegister(0x80, 0x00);
writeRegister(SYSRANGE_START_REG, 0x01); // maybe could be removed by putting sensor writeRegister(SYSRANGE_START_REG, 0x01); // maybe could be removed by putting sensor
// in continuous mode // in continuous mode
readRegister(SYSRANGE_START_REG, system_start); readRegister(SYSRANGE_START_REG, system_start);
while((system_start & 0x01) == 1) { while ((system_start & 0x01) == 1) {
readRegister(SYSRANGE_START_REG, system_start); readRegister(SYSRANGE_START_REG, system_start);
} }
@ -607,8 +610,9 @@ VL53LXX::measure()
readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement); readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement);
if((wait_for_measurement & 0x07) == 0){ if ((wait_for_measurement & 0x07) == 0) {
work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000); // reschedule every 1 ms until measurement is ready work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
1000); // reschedule every 1 ms until measurement is ready
ret = OK; ret = OK;
return ret; return ret;
} }
@ -654,12 +658,13 @@ VL53LXX::collect()
float distance_m = float(distance_mm) * 1e-3f; float distance_m = float(distance_mm) * 1e-3f;
struct distance_sensor_s report; struct distance_sensor_s report;
report.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation; report.orientation = _rotation;
if(distance_m > 2.0f){ if (distance_m > 2.0f) {
report.current_distance = 2.0f; report.current_distance = 2.0f;
} else { } else {
report.current_distance = distance_m; report.current_distance = distance_m;
} }
@ -708,8 +713,9 @@ VL53LXX::start()
static orb_advert_t pub = nullptr; static orb_advert_t pub = nullptr;
if (pub != nullptr) { if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info); orb_publish(ORB_ID(subsystem_info), pub, &info);
} else { } else {
pub = orb_advertise(ORB_ID(subsystem_info), &info); pub = orb_advertise(ORB_ID(subsystem_info), &info);
@ -736,20 +742,20 @@ VL53LXX::cycle_trampoline(void *arg)
void void
VL53LXX::cycle() VL53LXX::cycle()
{ {
measure(); measure();
if(_collect_phase) { if (_collect_phase) {
_collect_phase = false; _collect_phase = false;
_new_measurement = true; _new_measurement = true;
collect(); collect();
work_queue(HPWORK, work_queue(HPWORK,
&_work, &_work,
(worker_t)&VL53LXX::cycle_trampoline, (worker_t)&VL53LXX::cycle_trampoline,
this, this,
USEC2TICK(VL53LXX_SAMPLE_RATE)); USEC2TICK(VL53LXX_SAMPLE_RATE));
} }
} }
@ -765,209 +771,204 @@ VL53LXX::print_info()
} }
bool bool
VL53LXX::spadCalculations() VL53LXX::spadCalculations()
{ {
uint8_t val; uint8_t val;
uint8_t spad_count;
bool spad_type_is_aperture;
uint8_t ref_spad_map[6];
writeRegister(0x80, 0x01);
writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x00);
writeRegister(0xFF, 0x06);
readRegister(0x83, val);
writeRegister(0x83, val | 0x04);
uint8_t spad_count; writeRegister(0xFF, 0x07);
bool spad_type_is_aperture; writeRegister(0x81, 0x01);
writeRegister(0x80, 0x01);
writeRegister(0x94, 0x6b);
writeRegister(0x83, 0x00);
uint8_t ref_spad_map[6]; readRegister(0x83, val);
writeRegister(0x80, 0x01); while (val == 0x00) {
writeRegister(0xFF, 0x01); readRegister(0x83, val);
writeRegister(0x00, 0x00); }
writeRegister(0xFF, 0x06);
readRegister(0x83, val); writeRegister(0x83, 0x01);
writeRegister(0x83, val | 0x04);
writeRegister(0xFF, 0x07); readRegister(0x92, val);
writeRegister(0x81, 0x01);
writeRegister(0x80, 0x01);
writeRegister(0x94, 0x6b);
writeRegister(0x83, 0x00);
readRegister(0x83, val); spad_count = val & 0x7f;
spad_type_is_aperture = (val >> 7) & 0x01;
while (val == 0x00) { writeRegister(0x81, 0x00);
readRegister(0x83, val); writeRegister(0xFF, 0x06);
}
writeRegister(0x83, 0x01); readRegister(0x83, val);
writeRegister(0x83, val & ~0x04);
readRegister(0x92, val); writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x01);
writeRegister(0xFF, 0x00);
writeRegister(0x80, 0x00);
spad_count = val & 0x7f; readRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6);
spad_type_is_aperture = (val >> 7) & 0x01;
writeRegister(0x81, 0x00); writeRegister(0xFF, 0x01);
writeRegister(0xFF, 0x06); writeRegister(DYNAMIC_SPAD_REF_EN_START_OFFSET_REG, 0x00);
writeRegister(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG, 0x2C);
writeRegister(0xFF, 0x00);
writeRegister(GLOBAL_CONFIG_REF_EN_START_SELECT_REG, 0xB4);
readRegister(0x83, val); uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0;
writeRegister(0x83, val & ~0x04); uint8_t spads_enabled = 0;
writeRegister(0xFF, 0x01); for (uint8_t i = 0; i < 48; i++) {
writeRegister(0x00, 0x01); if (i < first_spad_to_enable || spads_enabled == spad_count) {
writeRegister(0xFF, 0x00); ref_spad_map[i / 8] &= ~(1 << (i % 8));
writeRegister(0x80, 0x00);
readRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6);
writeRegister(0xFF, 0x01); } else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1) {
writeRegister(DYNAMIC_SPAD_REF_EN_START_OFFSET_REG, 0x00); spads_enabled++;
writeRegister(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG, 0x2C); }
writeRegister(0xFF, 0x00); }
writeRegister(GLOBAL_CONFIG_REF_EN_START_SELECT_REG, 0xB4);
uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; writeRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6);
uint8_t spads_enabled = 0;
for (uint8_t i = 0; i < 48; i++) sensorTuning();
{
if (i < first_spad_to_enable || spads_enabled == spad_count)
{
ref_spad_map[i / 8] &= ~(1 << (i % 8));
}
else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
{
spads_enabled++;
}
}
writeRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6); writeRegister(SYSTEM_INTERRUPT_CONFIG_GPIO_REG, 4); // 4: GPIO interrupt on new data
sensorTuning(); readRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val);
writeRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val & ~0x10); // active low
writeRegister(SYSTEM_INTERRUPT_CONFIG_GPIO_REG, 4); // 4: GPIO interrupt on new data writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01);
readRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val); writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8);
writeRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val & ~0x10); // active low
writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x01);
singleRefCalibration(0x40);
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8);
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x01); writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x02);
singleRefCalibration(0x40); singleRefCalibration(0x00);
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x02);
singleRefCalibration(0x00);
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // restore config writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // restore config
return true; return true;
} }
bool bool
VL53LXX::sensorTuning() VL53LXX::sensorTuning()
{ {
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x00); writeRegister(0x00, 0x00);
writeRegister(0xFF, 0x00); writeRegister(0xFF, 0x00);
writeRegister(0x09, 0x00); writeRegister(0x09, 0x00);
writeRegister(0x10, 0x00); writeRegister(0x10, 0x00);
writeRegister(0x11, 0x00); writeRegister(0x11, 0x00);
writeRegister(0x24, 0x01); writeRegister(0x24, 0x01);
writeRegister(0x25, 0xFF); writeRegister(0x25, 0xFF);
writeRegister(0x75, 0x00); writeRegister(0x75, 0x00);
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x4E, 0x2C); writeRegister(0x4E, 0x2C);
writeRegister(0x48, 0x00); writeRegister(0x48, 0x00);
writeRegister(0x30, 0x20); writeRegister(0x30, 0x20);
writeRegister(0xFF, 0x00); writeRegister(0xFF, 0x00);
writeRegister(0x30, 0x09); writeRegister(0x30, 0x09);
writeRegister(0x54, 0x00); writeRegister(0x54, 0x00);
writeRegister(0x31, 0x04); writeRegister(0x31, 0x04);
writeRegister(0x32, 0x03); writeRegister(0x32, 0x03);
writeRegister(0x40, 0x83); writeRegister(0x40, 0x83);
writeRegister(0x46, 0x25); writeRegister(0x46, 0x25);
writeRegister(0x60, 0x00); writeRegister(0x60, 0x00);
writeRegister(0x27, 0x00); writeRegister(0x27, 0x00);
writeRegister(0x50, 0x06); writeRegister(0x50, 0x06);
writeRegister(0x51, 0x00); writeRegister(0x51, 0x00);
writeRegister(0x52, 0x96); writeRegister(0x52, 0x96);
writeRegister(0x56, 0x08); writeRegister(0x56, 0x08);
writeRegister(0x57, 0x30); writeRegister(0x57, 0x30);
writeRegister(0x61, 0x00); writeRegister(0x61, 0x00);
writeRegister(0x62, 0x00); writeRegister(0x62, 0x00);
writeRegister(0x64, 0x00); writeRegister(0x64, 0x00);
writeRegister(0x65, 0x00); writeRegister(0x65, 0x00);
writeRegister(0x66, 0xA0); writeRegister(0x66, 0xA0);
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x22, 0x32); writeRegister(0x22, 0x32);
writeRegister(0x47, 0x14); writeRegister(0x47, 0x14);
writeRegister(0x49, 0xFF); writeRegister(0x49, 0xFF);
writeRegister(0x4A, 0x00); writeRegister(0x4A, 0x00);
writeRegister(0xFF, 0x00); writeRegister(0xFF, 0x00);
writeRegister(0x7A, 0x0A); writeRegister(0x7A, 0x0A);
writeRegister(0x7B, 0x00); writeRegister(0x7B, 0x00);
writeRegister(0x78, 0x21); writeRegister(0x78, 0x21);
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x23, 0x34); writeRegister(0x23, 0x34);
writeRegister(0x42, 0x00); writeRegister(0x42, 0x00);
writeRegister(0x44, 0xFF); writeRegister(0x44, 0xFF);
writeRegister(0x45, 0x26); writeRegister(0x45, 0x26);
writeRegister(0x46, 0x05); writeRegister(0x46, 0x05);
writeRegister(0x40, 0x40); writeRegister(0x40, 0x40);
writeRegister(0x0E, 0x06); writeRegister(0x0E, 0x06);
writeRegister(0x20, 0x1A); writeRegister(0x20, 0x1A);
writeRegister(0x43, 0x40); writeRegister(0x43, 0x40);
writeRegister(0xFF, 0x00); writeRegister(0xFF, 0x00);
writeRegister(0x34, 0x03); writeRegister(0x34, 0x03);
writeRegister(0x35, 0x44); writeRegister(0x35, 0x44);
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x31, 0x04); writeRegister(0x31, 0x04);
writeRegister(0x4B, 0x09); writeRegister(0x4B, 0x09);
writeRegister(0x4C, 0x05); writeRegister(0x4C, 0x05);
writeRegister(0x4D, 0x04); writeRegister(0x4D, 0x04);
writeRegister(0xFF, 0x00); writeRegister(0xFF, 0x00);
writeRegister(0x44, 0x00); writeRegister(0x44, 0x00);
writeRegister(0x45, 0x20); writeRegister(0x45, 0x20);
writeRegister(0x47, 0x08); writeRegister(0x47, 0x08);
writeRegister(0x48, 0x28); writeRegister(0x48, 0x28);
writeRegister(0x67, 0x00); writeRegister(0x67, 0x00);
writeRegister(0x70, 0x04); writeRegister(0x70, 0x04);
writeRegister(0x71, 0x01); writeRegister(0x71, 0x01);
writeRegister(0x72, 0xFE); writeRegister(0x72, 0xFE);
writeRegister(0x76, 0x00); writeRegister(0x76, 0x00);
writeRegister(0x77, 0x00); writeRegister(0x77, 0x00);
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x0D, 0x01); writeRegister(0x0D, 0x01);
writeRegister(0xFF, 0x00); writeRegister(0xFF, 0x00);
writeRegister(0x80, 0x01); writeRegister(0x80, 0x01);
writeRegister(0x01, 0xF8); writeRegister(0x01, 0xF8);
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x8E, 0x01); writeRegister(0x8E, 0x01);
writeRegister(0x00, 0x01); writeRegister(0x00, 0x01);
writeRegister(0xFF, 0x00); writeRegister(0xFF, 0x00);
writeRegister(0x80, 0x00); writeRegister(0x80, 0x00);
return true; return true;
} }
bool bool
VL53LXX::singleRefCalibration(uint8_t byte) VL53LXX::singleRefCalibration(uint8_t byte)
{ {
uint8_t val; uint8_t val;
writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
do do {
{ readRegister(RESULT_INTERRUPT_STATUS_REG, val);
readRegister(RESULT_INTERRUPT_STATUS_REG, val); } while ((val & 0x07) == 0);
}
while ((val & 0x07) == 0);
writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01);
writeRegister(SYSRANGE_START_REG, 0x00); writeRegister(SYSRANGE_START_REG, 0x00);
return true; return true;
} }

282
src/drivers/pmw3901/pmw3901.cpp
Unescape View File

@ -166,10 +166,10 @@ private:
// int collect(); // int collect();
int readRegister(unsigned reg, uint8_t *data, unsigned count); int readRegister(unsigned reg, uint8_t *data, unsigned count);
int writeRegister(unsigned reg, uint8_t data); int writeRegister(unsigned reg, uint8_t data);
int sensorInit(); int sensorInit();
int readMotionCount(int16_t &deltaX, int16_t &deltaY); int readMotionCount(int16_t &deltaX, int16_t &deltaY);
/** /**
* Static trampoline from the workq context; because we don't have a * Static trampoline from the workq context; because we don't have a
@ -246,7 +246,7 @@ PMW3901::sensorInit()
usleep(5000); usleep(5000);
// Test the SPI communication, checking chipId and inverse chipId // Test the SPI communication, checking chipId and inverse chipId
if (data[0] != 0x49 && data[1] != 0xB8) return false; if (data[0] != 0x49 && data[1] != 0xB8) { return false; }
// Reading the motion registers one time // Reading the motion registers one time
readRegister(0x02, &data[0], 1); readRegister(0x02, &data[0], 1);
@ -259,85 +259,85 @@ PMW3901::sensorInit()
// set performance optimization registers // set performance optimization registers
writeRegister(0x7F, 0x00); writeRegister(0x7F, 0x00);
writeRegister(0x61, 0xAD); writeRegister(0x61, 0xAD);
writeRegister(0x7F, 0x03); writeRegister(0x7F, 0x03);
writeRegister(0x40, 0x00); writeRegister(0x40, 0x00);
writeRegister(0x7F, 0x05); writeRegister(0x7F, 0x05);
writeRegister(0x41, 0xB3); writeRegister(0x41, 0xB3);
writeRegister(0x43, 0xF1); writeRegister(0x43, 0xF1);
writeRegister(0x45, 0x14); writeRegister(0x45, 0x14);
writeRegister(0x5B, 0x32); writeRegister(0x5B, 0x32);
writeRegister(0x5F, 0x34); writeRegister(0x5F, 0x34);
writeRegister(0x7B, 0x08); writeRegister(0x7B, 0x08);
writeRegister(0x7F, 0x06); writeRegister(0x7F, 0x06);
writeRegister(0x44, 0x1B); writeRegister(0x44, 0x1B);
writeRegister(0x40, 0xBF); writeRegister(0x40, 0xBF);
writeRegister(0x4E, 0x3F); writeRegister(0x4E, 0x3F);
writeRegister(0x7F, 0x08); writeRegister(0x7F, 0x08);
writeRegister(0x65, 0x20); writeRegister(0x65, 0x20);
writeRegister(0x6A, 0x18); writeRegister(0x6A, 0x18);
writeRegister(0x7F, 0x09); writeRegister(0x7F, 0x09);
writeRegister(0x4F, 0xAF); writeRegister(0x4F, 0xAF);
writeRegister(0x5F, 0x40); writeRegister(0x5F, 0x40);
writeRegister(0x48, 0x80); writeRegister(0x48, 0x80);
writeRegister(0x49, 0x80); writeRegister(0x49, 0x80);
writeRegister(0x57, 0x77); writeRegister(0x57, 0x77);
writeRegister(0x60, 0x78); writeRegister(0x60, 0x78);
writeRegister(0x61, 0x78); writeRegister(0x61, 0x78);
writeRegister(0x62, 0x08); writeRegister(0x62, 0x08);
writeRegister(0x63, 0x50); writeRegister(0x63, 0x50);
writeRegister(0x7F, 0x0A); writeRegister(0x7F, 0x0A);
writeRegister(0x45, 0x60); writeRegister(0x45, 0x60);
writeRegister(0x7F, 0x00); writeRegister(0x7F, 0x00);
writeRegister(0x4D, 0x11); writeRegister(0x4D, 0x11);
writeRegister(0x55, 0x80); writeRegister(0x55, 0x80);
writeRegister(0x74, 0x1F); writeRegister(0x74, 0x1F);
writeRegister(0x75, 0x1F); writeRegister(0x75, 0x1F);
writeRegister(0x4A, 0x78); writeRegister(0x4A, 0x78);
writeRegister(0x4B, 0x78); writeRegister(0x4B, 0x78);
writeRegister(0x44, 0x08); writeRegister(0x44, 0x08);
writeRegister(0x45, 0x50); writeRegister(0x45, 0x50);
writeRegister(0x64, 0xFF); writeRegister(0x64, 0xFF);
writeRegister(0x65, 0x1F); writeRegister(0x65, 0x1F);
writeRegister(0x7F, 0x14); writeRegister(0x7F, 0x14);
writeRegister(0x65, 0x60); writeRegister(0x65, 0x60);
writeRegister(0x66, 0x08); writeRegister(0x66, 0x08);
writeRegister(0x63, 0x78); writeRegister(0x63, 0x78);
writeRegister(0x7F, 0x15); writeRegister(0x7F, 0x15);
writeRegister(0x48, 0x58); writeRegister(0x48, 0x58);
writeRegister(0x7F, 0x07); writeRegister(0x7F, 0x07);
writeRegister(0x41, 0x0D); writeRegister(0x41, 0x0D);
writeRegister(0x43, 0x14); writeRegister(0x43, 0x14);
writeRegister(0x4B, 0x0E); writeRegister(0x4B, 0x0E);
writeRegister(0x45, 0x0F); writeRegister(0x45, 0x0F);
writeRegister(0x44, 0x42); writeRegister(0x44, 0x42);
writeRegister(0x4C, 0x80); writeRegister(0x4C, 0x80);
writeRegister(0x7F, 0x10); writeRegister(0x7F, 0x10);
writeRegister(0x5B, 0x02); writeRegister(0x5B, 0x02);
writeRegister(0x7F, 0x07); writeRegister(0x7F, 0x07);
writeRegister(0x40, 0x41); writeRegister(0x40, 0x41);
writeRegister(0x70, 0x00); writeRegister(0x70, 0x00);
usleep(10000); usleep(10000);
writeRegister(0x32, 0x44); writeRegister(0x32, 0x44);
writeRegister(0x7F, 0x07); writeRegister(0x7F, 0x07);
writeRegister(0x40, 0x40); writeRegister(0x40, 0x40);
writeRegister(0x7F, 0x06); writeRegister(0x7F, 0x06);
writeRegister(0x62, 0xf0); writeRegister(0x62, 0xf0);
writeRegister(0x63, 0x00); writeRegister(0x63, 0x00);
writeRegister(0x7F, 0x0D); writeRegister(0x7F, 0x0D);
writeRegister(0x48, 0xC0); writeRegister(0x48, 0xC0);
writeRegister(0x6F, 0xd5); writeRegister(0x6F, 0xd5);
writeRegister(0x7F, 0x00); writeRegister(0x7F, 0x00);
writeRegister(0x5B, 0xa0); writeRegister(0x5B, 0xa0);
writeRegister(0x4E, 0xA8); writeRegister(0x4E, 0xA8);
writeRegister(0x5A, 0x50); writeRegister(0x5A, 0x50);
writeRegister(0x40, 0x80); writeRegister(0x40, 0x80);
ret = OK; ret = OK;
return ret; return ret;
} }
@ -351,7 +351,7 @@ PMW3901::init()
goto out; goto out;
} }
set_frequency(PMW3901_SPI_BUS_SPEED); set_frequency(PMW3901_SPI_BUS_SPEED);
sensorInit(); sensorInit();
@ -365,17 +365,17 @@ PMW3901::init()
//_class_instance = register_class_devname(PMW3901_DEVICE_PATH); //_class_instance = register_class_devname(PMW3901_DEVICE_PATH);
//if (_class_instance == CLASS_DEVICE_PRIMARY) { // change to optical flow topic //if (_class_instance == CLASS_DEVICE_PRIMARY) { // change to optical flow topic
/* get a publish handle on the range finder topic */ /* get a publish handle on the range finder topic */
// struct distance_sensor_s ds_report; // struct distance_sensor_s ds_report;
// _reports->get(&ds_report); // _reports->get(&ds_report);
// _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, // _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
// &_orb_class_instance, ORB_PRIO_LOW); // &_orb_class_instance, ORB_PRIO_LOW);
// if (_distance_sensor_topic == nullptr) { // if (_distance_sensor_topic == nullptr) {
// DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); // DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
// } // }
//} //}
@ -394,64 +394,64 @@ PMW3901::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{ {
switch (cmd) { switch (cmd) {
case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
switch (arg) { switch (arg) {
case 0:
return -EINVAL;
case SENSOR_POLLRATE_DEFAULT: { case 0:
return -EINVAL;
/* do we need to start internal polling? */ case SENSOR_POLLRATE_DEFAULT: {
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */ /* do we need to start internal polling? */
_measure_ticks = USEC2TICK(PMW3901_CONVERSION_INTERVAL); bool want_start = (_measure_ticks == 0);
/* if we need to start the poll state machine, do it */ /* set interval for next measurement to minimum legal value */
if (want_start) { _measure_ticks = USEC2TICK(PMW3901_CONVERSION_INTERVAL);
start();
}
return OK; /* if we need to start the poll state machine, do it */
if (want_start) {
start();
} }
case SENSOR_POLLRATE_MANUAL: { return OK;
}
stop();
_measure_ticks = 0;
return OK;
}
/* adjust to a legal polling interval in Hz */ case SENSOR_POLLRATE_MANUAL: {
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */ stop();
unsigned ticks = USEC2TICK(1000000 / arg); _measure_ticks = 0;
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* check against maximum rate */ /* convert hz to tick interval via microseconds */
if (ticks < USEC2TICK(PMW3901_CONVERSION_INTERVAL)) { unsigned ticks = USEC2TICK(1000000 / arg);
return -EINVAL;
}
/* update interval for next measurement */ /* check against maximum rate */
_measure_ticks = ticks; if (ticks < USEC2TICK(PMW3901_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* if we need to start the poll state machine, do it */ /* update interval for next measurement */
if (want_start) { _measure_ticks = ticks;
start();
}
return OK; /* if we need to start the poll state machine, do it */
if (want_start) {
start();
} }
return OK;
} }
}
} }
default: default:
/* give it to the superclass */ /* give it to the superclass */
return SPI::ioctl(filp, cmd, arg); return SPI::ioctl(filp, cmd, arg);
} }
} }
@ -515,7 +515,7 @@ PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count)
cmd[0] = DIR_READ(reg); cmd[0] = DIR_READ(reg);
ret = transfer(&cmd[0], &cmd[0], count+1); ret = transfer(&cmd[0], &cmd[0], count + 1);
if (OK != ret) { if (OK != ret) {
perf_count(_comms_errors); perf_count(_comms_errors);
@ -563,7 +563,7 @@ PMW3901::collect()
int16_t delta_x_raw, delta_y_raw; int16_t delta_x_raw, delta_y_raw;
float delta_x, delta_y; float delta_x, delta_y;
perf_begin(_sample_perf); perf_begin(_sample_perf);
uint64_t timestamp = hrt_absolute_time(); uint64_t timestamp = hrt_absolute_time();
uint64_t dt_flow = timestamp - _previous_collect_timestamp; uint64_t dt_flow = timestamp - _previous_collect_timestamp;
@ -590,6 +590,7 @@ PMW3901::collect()
if (_optical_flow_pub == nullptr) { if (_optical_flow_pub == nullptr) {
_optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &report); _optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &report);
} else { } else {
orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &report); orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &report);
@ -601,21 +602,22 @@ PMW3901::collect()
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);
ret = OK; ret = OK;
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return ret;
} }
int int
PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY) PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY)
{ {
int ret; int ret;
uint8_t data[10] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0, uint8_t data[10] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,
DIR_READ(0x05), 0, DIR_READ(0x06), 0 }; DIR_READ(0x05), 0, DIR_READ(0x06), 0
};
ret = transfer(&data[0], &data[0], 10); ret = transfer(&data[0], &data[0], 10);
@ -815,7 +817,7 @@ test()
// errx(1, "failed to set 2Hz poll rate"); // errx(1, "failed to set 2Hz poll rate");
// } // }
for(int i = 0; i < 10000; i++){ for (int i = 0; i < 10000; i++) {
g_dev->collect(); g_dev->collect();
usleep(10000); usleep(10000);
} }

Write Preview
Loading…
Cancel
Save