Fixed formatting of flow driver

1 files changed, 198 insertions, 125 deletions

diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index f74db1b52..24592a301 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -76,7 +76,7 @@
 #define PX4FLOW_BUS 			PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION
 #define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
 //range 0x42 - 0x49
-
+ 
 /* PX4FLOW Registers addresses */
 //#define PX4FLOW_REG		0x00	/* Measure Register 0*/
 #define PX4FLOW_REG 	0x16	/* Measure Register 22*/
@@ -119,11 +119,12 @@ typedef struct i2c_integral_frame {
 	uint32_t time_since_last_sonar_update;
 	uint16_t ground_distance;
 	uint8_t qual;
-}__attribute__((packed));
+} __attribute__((packed));
 struct i2c_integral_frame f_integral;
 
 
-class PX4FLOW: public device::I2C {
+class PX4FLOW: public device::I2C
+{
 public:
 	PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
 	virtual ~PX4FLOW();
@@ -144,8 +145,8 @@ protected:
 private:
 
 	work_s _work;
-	RingBuffer *_reports;bool _sensor_ok;
-	int _measure_ticks;bool _collect_phase;
+	RingBuffer *_reports; bool _sensor_ok;
+	int _measure_ticks; bool _collect_phase;
 
 	orb_advert_t _px4flow_topic;
 
@@ -198,12 +199,13 @@ private:
 extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
 
 PX4FLOW::PX4FLOW(int bus, int address) :
-		I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
-		_reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase(
-				false), _px4flow_topic(-1), _sample_perf(
-				perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(
+	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
+	_reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase(
+		false), _px4flow_topic(-1), _sample_perf(
+			perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(
 				perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows(
-				perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) {
+					perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
+{
 	// enable debug() calls
 	_debug_enabled = true;
 
@@ -211,117 +213,131 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 	memset(&_work, 0, sizeof(_work));
 }
 
-PX4FLOW::~PX4FLOW() {
+PX4FLOW::~PX4FLOW()
+{
 	/* make sure we are truly inactive */
 	stop();
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 }
 
-int PX4FLOW::init() {
+int PX4FLOW::init()
+{
 	int ret = ERROR;
 
 	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	if (I2C::init() != OK) {
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(optical_flow_s));
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
 		goto out;
+	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
-	out: return ret;
+out: return ret;
 }
 
-int PX4FLOW::probe() {
+int PX4FLOW::probe()
+{
 	return measure();
 }
 
-int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) {
+int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
 	switch (cmd) {
 
 	case SENSORIOCSPOLLRATE: {
-		switch (arg) {
+			switch (arg) {
 
-		/* switching to manual polling */
-		case SENSOR_POLLRATE_MANUAL:
-			stop();
-			_measure_ticks = 0;
-			return OK;
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
 
 			/* external signalling (DRDY) not supported */
-		case SENSOR_POLLRATE_EXTERNAL:
+			case SENSOR_POLLRATE_EXTERNAL:
 
 			/* zero would be bad */
-		case 0:
-			return -EINVAL;
+			case 0:
+				return -EINVAL;
 
 			/* set default/max polling rate */
-		case SENSOR_POLLRATE_MAX:
-		case SENSOR_POLLRATE_DEFAULT: {
-			/* do we need to start internal polling? */
-			bool want_start = (_measure_ticks == 0);
+			case SENSOR_POLLRATE_MAX:
+			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(PX4FLOW_CONVERSION_INTERVAL);
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
 
-			/* if we need to start the poll state machine, do it */
-			if (want_start)
-				start();
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
 
-			return OK;
-		}
+					return OK;
+				}
 
 			/* adjust to a legal polling interval in Hz */
-		default: {
-			/* do we need to start internal polling? */
-			bool want_start = (_measure_ticks == 0);
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
 
-			/* convert hz to tick interval via microseconds */
-			unsigned ticks = USEC2TICK(1000000 / arg);
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
 
-			/* check against maximum rate */
-			if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
-				return -EINVAL;
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
+						return -EINVAL;
+					}
 
-			/* update interval for next measurement */
-			_measure_ticks = ticks;
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
 
-			/* if we need to start the poll state machine, do it */
-			if (want_start)
-				start();
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
 
-			return OK;
-		}
+					return OK;
+				}
+			}
 		}
-	}
 
 	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
+		if (_measure_ticks == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 1) || (arg > 100))
-			return -EINVAL;
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
 
-		irqstate_t flags = irqsave();
-		if (!_reports->resize(arg)) {
 			irqrestore(flags);
-			return -ENOMEM;
-		}
-		irqrestore(flags);
 
-		return OK;
-	}
+			return OK;
+		}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
@@ -336,15 +352,17 @@ int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) {
 	}
 }
 
-ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) {
+ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
+{
 	unsigned count = buflen / sizeof(struct optical_flow_s);
 	struct optical_flow_s *rbuf =
-			reinterpret_cast<struct optical_flow_s *>(buffer);
+		reinterpret_cast<struct optical_flow_s *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
@@ -397,7 +415,8 @@ ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) {
 	return ret;
 }
 
-int PX4FLOW::measure() {
+int PX4FLOW::measure()
+{
 	int ret;
 
 	/*
@@ -412,12 +431,14 @@ int PX4FLOW::measure() {
 		printf("i2c::transfer flow returned %d");
 		return ret;
 	}
+
 	ret = OK;
 
 	return ret;
 }
 
-int PX4FLOW::collect() {
+int PX4FLOW::collect()
+{
 	int ret = -EIO;
 
 	/* read from the sensor */
@@ -426,10 +447,11 @@ int PX4FLOW::collect() {
 
 	perf_begin(_sample_perf);
 
-	if(PX4FLOW_REG==0x00){
+	if (PX4FLOW_REG == 0x00) {
 		ret = transfer(nullptr, 0, &val[0], 45); //read 45 bytes (22+23 : frame1 + frame2)
 	}
-	if(PX4FLOW_REG==0x16){
+
+	if (PX4FLOW_REG == 0x16) {
 		ret = transfer(nullptr, 0, &val[0], 23); //read 23 bytes (only frame2)
 	}
 
@@ -461,42 +483,56 @@ int PX4FLOW::collect() {
 		f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
 		f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
 		f_integral.integration_timespan = val[37] << 24 | val[36] << 16
-				| val[35] << 8 | val[34];
+						  | val[35] << 8 | val[34];
 		f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
 				| val[39] << 8 | val[38];
 		f_integral.ground_distance = val[43] << 8 | val[42];
 		f_integral.qual = val[44];
 	}
-	if(PX4FLOW_REG==0x16){
+
+	if (PX4FLOW_REG == 0x16) {
 		f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
-		f_integral.pixel_flow_x_integral =val[3] << 8 | val[2];
-		f_integral.pixel_flow_y_integral =val[5] << 8 | val[4];
-		f_integral.gyro_x_rate_integral =val[7] << 8 | val[6];
-		f_integral.gyro_y_rate_integral =val[9] << 8 | val[8];
-		f_integral.gyro_z_rate_integral =val[11] << 8 | val[10];
-		f_integral.integration_timespan = val[15] <<24 |val[14] << 16 |val[13] << 8 |val[12];
-		f_integral.time_since_last_sonar_update = val[19] <<24 |val[18] << 16 |val[17] << 8 |val[16];
-		f_integral.ground_distance =val[21] <<8 |val[20];
-		f_integral.qual =val[22];
+		f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
+		f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
+		f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
+		f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
+		f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
+		f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
+		f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
+		f_integral.ground_distance = val[21] << 8 | val[20];
+		f_integral.qual = val[22];
 	}
 
 
 	struct optical_flow_s report;
+
 	report.timestamp = hrt_absolute_time();
+
 	report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
+
 	report.pixel_flow_y_integral = float(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
+
 	report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
+
 	report.ground_distance_m = float(f_integral.ground_distance) / 1000.0f;//convert to meters
+
 	report.quality = f_integral.qual;//0:bad ; 255 max quality
-	report.gyro_x_rate_integral= float(f_integral.gyro_x_rate_integral)/10000.0f;//convert to radians
-	report.gyro_y_rate_integral= float(f_integral.gyro_y_rate_integral)/10000.0f;//convert to radians
-	report.gyro_z_rate_integral= float(f_integral.gyro_z_rate_integral)/10000.0f;//convert to radians
-	report.integration_timespan= f_integral.integration_timespan;//microseconds
+
+	report.gyro_x_rate_integral = float(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
+
+	report.gyro_y_rate_integral = float(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
+
+	report.gyro_z_rate_integral = float(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
+
+	report.integration_timespan = f_integral.integration_timespan; //microseconds
+
 	report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
+
 	report.sensor_id = 0;
 
 	if (_px4flow_topic < 0) {
 		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
+
 	} else {
 		/* publish it */
 		orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
@@ -516,7 +552,8 @@ int PX4FLOW::collect() {
 	return ret;
 }
 
-void PX4FLOW::start() {
+void PX4FLOW::start()
+{
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_reports->flush();
@@ -526,33 +563,39 @@ void PX4FLOW::start() {
 
 	/* notify about state change */
 	struct subsystem_info_s info = { true, true, true,
-			SUBSYSTEM_TYPE_OPTICALFLOW };
+		       SUBSYSTEM_TYPE_OPTICALFLOW
+	};
 	static orb_advert_t pub = -1;
 
 	if (pub > 0) {
 		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
 	} else {
 		pub = orb_advertise(ORB_ID(subsystem_info), &info);
 	}
 }
 
-void PX4FLOW::stop() {
+void PX4FLOW::stop()
+{
 	work_cancel(HPWORK, &_work);
 }
 
-void PX4FLOW::cycle_trampoline(void *arg) {
+void PX4FLOW::cycle_trampoline(void *arg)
+{
 	PX4FLOW *dev = (PX4FLOW *) arg;
 
 	dev->cycle();
 }
 
-void PX4FLOW::cycle() {
+void PX4FLOW::cycle()
+{
 //	/* collection phase? */
 
 //	static uint64_t deltatime = hrt_absolute_time();
 
-	if (OK != measure())
+	if (OK != measure()) {
 		log("measure error");
+	}
 
 	//usleep(PX4FLOW_CONVERSION_INTERVAL/40);
 
@@ -577,13 +620,14 @@ void PX4FLOW::cycle() {
 //			_measure_ticks-USEC2TICK(deltatime));
 
 	work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this,
-			_measure_ticks);
+		   _measure_ticks);
 
 //	deltatime = hrt_absolute_time();
 
 }
 
-void PX4FLOW::print_info() {
+void PX4FLOW::print_info()
+{
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
@@ -594,7 +638,8 @@ void PX4FLOW::print_info() {
 /**
  * Local functions in support of the shell command.
  */
-namespace px4flow {
+namespace px4flow
+{
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -613,33 +658,39 @@ void info();
 /**
  * Start the driver.
  */
-void start() {
+void start()
+{
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver */
 	g_dev = new PX4FLOW(PX4FLOW_BUS);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
-	if (OK != g_dev->init())
+	if (OK != g_dev->init()) {
 		goto fail;
+	}
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
 		goto fail;
+	}
 
 	exit(0);
 
-	fail:
+fail:
 
 	if (g_dev != nullptr) {
 		delete g_dev;
@@ -652,13 +703,16 @@ void start() {
 /**
  * Stop the driver
  */
-void stop() {
+void stop()
+{
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
+
 	} else {
 		errx(1, "driver not running");
 	}
+
 	exit(0);
 }
 
@@ -667,7 +721,8 @@ void stop() {
  * make sure we can collect data from the sensor in polled
  * and automatic modes.
  */
-void test() {
+void test()
+{
 	struct optical_flow_s report;
 	ssize_t sz;
 	int ret;
@@ -676,8 +731,8 @@ void test() {
 
 	if (fd < 0)
 		err(1,
-				"%s open failed (try 'px4flow start' if the driver is not running",
-				PX4FLOW_DEVICE_PATH);
+		    "%s open failed (try 'px4flow start' if the driver is not running",
+		    PX4FLOW_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -685,14 +740,18 @@ void test() {
 	if (sz != sizeof(report))
 		//	err(1, "immediate read failed");
 
+	{
 		warnx("single read");
+	}
+
 	warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum);
 	warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum);
 	warnx("time:        %lld", report.timestamp);
 
 	/* start the sensor polling at 10Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10))	// 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {	// 2))
 		errx(1, "failed to set 10Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 10; i++) {
@@ -703,20 +762,22 @@ void test() {
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
 
 		warnx("framecount_total: %u", f.frame_count);
 		warnx("framecount_integral: %u",
-				f_integral.frame_count_since_last_readout);
+		      f_integral.frame_count_since_last_readout);
 		warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
 		warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
 		warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
@@ -724,9 +785,9 @@ void test() {
 		warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
 		warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
 		warnx("ground_distance: %0.2f m",
-				(double) f_integral.ground_distance / 1000);
+		      (double) f_integral.ground_distance / 1000);
 		warnx("time since last sonar update [us]: %i",
-				f_integral.time_since_last_sonar_update);
+		      f_integral.time_since_last_sonar_update);
 		warnx("quality integration average : %i", f_integral.qual);
 		warnx("quality : %i", f.qual);
 
@@ -739,17 +800,21 @@ void test() {
 /**
  * Reset the driver.
  */
-void reset() {
+void reset()
+{
 	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -757,9 +822,11 @@ void reset() {
 /**
  * Print a little info about the driver.
  */
-void info() {
-	if (g_dev == nullptr)
+void info()
+{
+	if (g_dev == nullptr) {
 		errx(1, "driver not running");
+	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
@@ -769,36 +836,42 @@ void info() {
 
 } // namespace
 
-int px4flow_main(int argc, char *argv[]) {
+int px4flow_main(int argc, char *argv[])
+{
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		px4flow::start();
+	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop"))
+	if (!strcmp(argv[1], "stop")) {
 		px4flow::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		px4flow::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		px4flow::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		px4flow::info();
+	}
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }