From 01ee8c9b58ad2b691ca51c20cd9ca1c36564e5d0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 19 Oct 2015 13:14:40 +0200
Subject: [PATCH] roboclaw: Fixed code style

---
 src/drivers/roboclaw/RoboClaw.cpp      | 121 +++++++++++++++++--------
 src/drivers/roboclaw/roboclaw_main.cpp |  25 ++---
 2 files changed, 97 insertions(+), 49 deletions(-)

diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index 5b945e452d..a3d2f96a20 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -61,7 +61,7 @@
 uint8_t RoboClaw::checksum_mask = 0x7f;
 
 RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
-		uint16_t pulsesPerRev):
+		   uint16_t pulsesPerRev):
 	_address(address),
 	_pulsesPerRev(pulsesPerRev),
 	_uart(0),
@@ -80,18 +80,27 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
 
 	// start serial port
 	_uart = open(deviceName, O_RDWR | O_NOCTTY);
-	if (_uart < 0) err(1, "could not open %s", deviceName);
+
+	if (_uart < 0) { err(1, "could not open %s", deviceName); }
+
 	int ret = 0;
 	struct termios uart_config;
 	ret = tcgetattr(_uart, &uart_config);
-	if (ret < 0) err (1, "failed to get attr");
+
+	if (ret < 0) { err(1, "failed to get attr"); }
+
 	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
 	ret = cfsetispeed(&uart_config, B38400);
-	if (ret < 0) err (1, "failed to set input speed");
+
+	if (ret < 0) { err(1, "failed to set input speed"); }
+
 	ret = cfsetospeed(&uart_config, B38400);
-	if (ret < 0) err (1, "failed to set output speed");
+
+	if (ret < 0) { err(1, "failed to set output speed"); }
+
 	ret = tcsetattr(_uart, TCSANOW, &uart_config);
-	if (ret < 0) err (1, "failed to set attr");
+
+	if (ret < 0) { err(1, "failed to set attr"); }
 
 	// setup default settings, reset encoders
 	resetEncoders();
@@ -107,25 +116,30 @@ RoboClaw::~RoboClaw()
 int RoboClaw::readEncoder(e_motor motor)
 {
 	uint16_t sum = 0;
+
 	if (motor == MOTOR_1) {
 		_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
+
 	} else if (motor == MOTOR_2) {
 		_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
 	}
+
 	uint8_t rbuf[50];
 	usleep(5000);
 	int nread = read(_uart, rbuf, 50);
+
 	if (nread < 6) {
 		printf("failed to read\n");
 		return -1;
 	}
+
 	//printf("received: \n");
 	//for (int i=0;i<nread;i++) {
-		//printf("%d\t", rbuf[i]);
+	//printf("%d\t", rbuf[i]);
 	//}
 	//printf("\n");
 	uint32_t count = 0;
-	uint8_t * countBytes = (uint8_t *)(&count);
+	uint8_t *countBytes = (uint8_t *)(&count);
 	countBytes[3] = rbuf[0];
 	countBytes[2] = rbuf[1];
 	countBytes[1] = rbuf[2];
@@ -133,13 +147,15 @@ int RoboClaw::readEncoder(e_motor motor)
 	uint8_t status = rbuf[4];
 	uint8_t checksum = rbuf[5];
 	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
-		checksum_mask;
+				    checksum_mask;
+
 	// check if checksum is valid
 	if (checksum != checksum_computed) {
 		printf("checksum failed: expected %d got %d\n",
-				checksum, checksum_computed);
+		       checksum, checksum_computed);
 		return -1;
 	}
+
 	int overFlow = 0;
 
 	if (status & STATUS_REVERSE) {
@@ -149,21 +165,25 @@ int RoboClaw::readEncoder(e_motor motor)
 	if (status & STATUS_UNDERFLOW) {
 		//printf("roboclaw: underflow\n");
 		overFlow = -1;
+
 	} else if (status & STATUS_OVERFLOW) {
 		//printf("roboclaw: overflow\n");
 		overFlow = +1;
 	}
 
 	static int64_t overflowAmount = 0x100000000LL;
+
 	if (motor == MOTOR_1) {
 		_motor1Overflow += overFlow;
 		_motor1Position = float(int64_t(count) +
-			_motor1Overflow*overflowAmount)/_pulsesPerRev;
+					_motor1Overflow * overflowAmount) / _pulsesPerRev;
+
 	} else if (motor == MOTOR_2) {
 		_motor2Overflow += overFlow;
 		_motor2Position = float(int64_t(count) +
-			_motor2Overflow*overflowAmount)/_pulsesPerRev;
+					_motor2Overflow * overflowAmount) / _pulsesPerRev;
 	}
+
 	return 0;
 }
 
@@ -181,65 +201,83 @@ float RoboClaw::getMotorPosition(e_motor motor)
 {
 	if (motor == MOTOR_1) {
 		return _motor1Position;
+
 	} else if (motor == MOTOR_2) {
 		return _motor2Position;
+
 	} else {
 		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
 		return NAN;
-    }
+	}
 }
 
 float RoboClaw::getMotorSpeed(e_motor motor)
 {
 	if (motor == MOTOR_1) {
 		return _motor1Speed;
+
 	} else if (motor == MOTOR_2) {
 		return _motor2Speed;
+
 	} else {
 		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
 		return NAN;
-    }
+	}
 }
 
 int RoboClaw::setMotorSpeed(e_motor motor, float value)
 {
 	uint16_t sum = 0;
+
 	// bound
-	if (value > 1) value = 1;
-	if (value < -1) value = -1;
-	uint8_t speed = fabs(value)*127;
+	if (value > 1) { value = 1; }
+
+	if (value < -1) { value = -1; }
+
+	uint8_t speed = fabs(value) * 127;
+
 	// send command
 	if (motor == MOTOR_1) {
 		if (value > 0) {
 			return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
+
 		} else {
 			return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
 		}
+
 	} else if (motor == MOTOR_2) {
 		if (value > 0) {
 			return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
+
 		} else {
 			return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
 		}
 	}
+
 	return -1;
 }
 
 int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
 {
 	uint16_t sum = 0;
+
 	// bound
-	if (value > 1) value = 1;
-	if (value < -1) value = -1;
-	int16_t duty = 1500*value;
+	if (value > 1) { value = 1; }
+
+	if (value < -1) { value = -1; }
+
+	int16_t duty = 1500 * value;
+
 	// send command
 	if (motor == MOTOR_1) {
 		return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
-				(uint8_t *)(&duty), 2, sum);
+				    (uint8_t *)(&duty), 2, sum);
+
 	} else if (motor == MOTOR_2) {
 		return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
-				(uint8_t *)(&duty), 2, sum);
+				    (uint8_t *)(&duty), 2, sum);
 	}
+
 	return -1;
 }
 
@@ -247,7 +285,7 @@ int RoboClaw::resetEncoders()
 {
 	uint16_t sum = 0;
 	return _sendCommand(CMD_RESET_ENCODERS,
-			nullptr, 0, sum);
+			    nullptr, 0, sum);
 }
 
 int RoboClaw::update()
@@ -256,52 +294,57 @@ int RoboClaw::update()
 	// check for exit condition every second
 	// note "::poll" is required to distinguish global
 	// poll from member function for driver
-	if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error
+	if (::poll(&_controlPoll, 1, 1000) < 0) { return -1; } // poll error
 
 	// if new data, send to motors
 	if (_actuators.updated()) {
 		_actuators.update();
-		setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
-		setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
+		setMotorDutyCycle(MOTOR_1, _actuators.control[CH_VOLTAGE_LEFT]);
+		setMotorDutyCycle(MOTOR_2, _actuators.control[CH_VOLTAGE_RIGHT]);
 	}
+
 	return 0;
 }
 
-uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
+uint16_t RoboClaw::_sumBytes(uint8_t *buf, size_t n)
 {
 	uint16_t sum = 0;
+
 	//printf("sum\n");
-	for (size_t i=0;i<n;i++) {
+	for (size_t i = 0; i < n; i++) {
 		sum += buf[i];
 		//printf("%d\t", buf[i]);
 	}
+
 	//printf("total sum %d\n", sum);
 	return sum;
 }
 
-int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
-		size_t n_data, uint16_t & prev_sum)
+int RoboClaw::_sendCommand(e_command cmd, uint8_t *data,
+			   size_t n_data, uint16_t &prev_sum)
 {
 	tcflush(_uart, TCIOFLUSH); // flush  buffers
 	uint8_t buf[n_data + 3];
 	buf[0] = _address;
 	buf[1] = cmd;
-	for (size_t i=0;i<n_data;i++) {
-		buf[i+2] = data[n_data - i - 1]; // MSB
+
+	for (size_t i = 0; i < n_data; i++) {
+		buf[i + 2] = data[n_data - i - 1]; // MSB
 	}
+
 	uint16_t sum = _sumBytes(buf, n_data + 2);
 	prev_sum += sum;
 	buf[n_data + 2] = sum & checksum_mask;
 	//printf("\nmessage:\n");
 	//for (size_t i=0;i<n_data+3;i++) {
-		//printf("%d\t", buf[i]);
+	//printf("%d\t", buf[i]);
 	//}
 	//printf("\n");
 	return write(_uart, buf, n_data + 3);
 }
 
 int roboclawTest(const char *deviceName, uint8_t address,
-		uint16_t pulsesPerRev)
+		 uint16_t pulsesPerRev)
 {
 	printf("roboclaw test: starting\n");
 
@@ -310,21 +353,23 @@ int roboclawTest(const char *deviceName, uint8_t address,
 	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
 	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
 	char buf[200];
-	for (int i=0; i<10; i++) {
+
+	for (int i = 0; i < 10; i++) {
 		usleep(100000);
 		roboclaw.readEncoder(RoboClaw::MOTOR_1);
 		roboclaw.readEncoder(RoboClaw::MOTOR_2);
-		roboclaw.printStatus(buf,200);
+		roboclaw.printStatus(buf, 200);
 		printf("%s", buf);
 	}
 
 	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
 	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
-	for (int i=0; i<10; i++) {
+
+	for (int i = 0; i < 10; i++) {
 		usleep(100000);
 		roboclaw.readEncoder(RoboClaw::MOTOR_1);
 		roboclaw.readEncoder(RoboClaw::MOTOR_2);
-		roboclaw.printStatus(buf,200);
+		roboclaw.printStatus(buf, 200);
 		printf("%s", buf);
 	}
 
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
index aa5e6dcc0b..0110f3b21c 100644
--- a/src/drivers/roboclaw/roboclaw_main.cpp
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -78,7 +78,7 @@ static void usage();
 static void usage()
 {
 	fprintf(stderr, "usage: roboclaw "
-			"{start|stop|status|test}\n\n");
+		"{start|stop|status|test}\n\n");
 }
 
 /**
@@ -106,24 +106,26 @@ int roboclaw_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		deamon_task = px4_task_spawn_cmd("roboclaw",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 10,
-					 2048,
-					 roboclaw_thread_main,
-					 (char * const *)argv);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 10,
+						 2048,
+						 roboclaw_thread_main,
+						 (char *const *)argv);
 		exit(0);
 
 	} else if (!strcmp(argv[1], "test")) {
 
-		const char * deviceName = "/dev/ttyS2";
+		const char *deviceName = "/dev/ttyS2";
 		uint8_t address = 128;
 		uint16_t pulsesPerRev = 1200;
 
 		if (argc == 2) {
 			printf("testing with default settings\n");
+
 		} else if (argc != 4) {
 			printf("usage: roboclaw test device address pulses_per_rev\n");
 			exit(-1);
+
 		} else {
 			deviceName = argv[2];
 			address = strtoul(argv[3], nullptr, 0);
@@ -131,7 +133,7 @@ int roboclaw_main(int argc, char *argv[])
 		}
 
 		printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
-			deviceName, address, pulsesPerRev);
+		       deviceName, address, pulsesPerRev);
 
 		roboclawTest(deviceName, address, pulsesPerRev);
 		thread_should_exit = true;
@@ -150,6 +152,7 @@ int roboclaw_main(int argc, char *argv[])
 		} else {
 			printf("\troboclaw app not started\n");
 		}
+
 		exit(0);
 	}
 
@@ -162,8 +165,8 @@ int roboclaw_thread_main(int argc, char *argv[])
 	printf("[roboclaw] starting\n");
 
 	// skip parent process args
-	argc -=2;
-	argv +=2;
+	argc -= 2;
+	argv += 2;
 
 	if (argc < 3) {
 		printf("usage: roboclaw start device address\n");
@@ -175,7 +178,7 @@ int roboclaw_thread_main(int argc, char *argv[])
 	uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0);
 
 	printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
-			deviceName, address, pulsesPerRev);
+	       deviceName, address, pulsesPerRev);
 
 	// start
 	RoboClaw roboclaw(deviceName, address, pulsesPerRev);