crazyflie: clean up

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 :

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(&reg_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(&reg_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 (_subsystem_pub != nullptr) {
-
+		orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), 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;
 }
 
 

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
 	)

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 OK;
-
-	return ret;
 
 }
 
@@ -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 (_subsystem_pub != nullptr) {
-
+		orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
 
 	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+		_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
 	}
 }