fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 04:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

roboclaw: Fixed code style

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-19 13:14:40 +02:00
parent c88c2c7276
commit 01ee8c9b58
2 changed files with 97 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/roboclaw/RoboClaw.cpp
+83 -38
View File
@@ -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);
}
src/drivers/roboclaw/roboclaw_main.cpp
+14 -11
View File
@@ -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);