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

add another set of uorb headers

Browse Source
This commit is contained in:
Thomas Gubler
2015-01-25 15:32:40 +01:00
parent 7634147c0f
commit cd35ab2661
3 changed files with 24 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/roboclaw/RoboClaw.cpp
+8 -7
View File
@@ -54,6 +54,7 @@
#include <mavlink/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_hrt.h>
@@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor)
uint8_t rbuf[50];
usleep(5000);
int nread = read(_uart, rbuf, 50);
if (nread < 6) {
if (nread < 6) {
printf("failed to read\n");
return -1;
}
@@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor)
countBytes[0] = rbuf[3];
uint8_t status = rbuf[4];
uint8_t checksum = rbuf[5];
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
checksum_mask;
// check if checksum is valid
if (checksum != checksum_computed) {
@@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor)
static int64_t overflowAmount = 0x100000000LL;
if (motor == MOTOR_1) {
_motor1Overflow += overFlow;
_motor1Position = float(int64_t(count) +
_motor1Position = float(int64_t(count) +
_motor1Overflow*overflowAmount)/_pulsesPerRev;
} else if (motor == MOTOR_2) {
_motor2Overflow += overFlow;
_motor2Position = float(int64_t(count) +
_motor2Position = float(int64_t(count) +
_motor2Overflow*overflowAmount)/_pulsesPerRev;
}
return 0;
@@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
return -1;
}
int RoboClaw::resetEncoders()
int RoboClaw::resetEncoders()
{
uint16_t sum = 0;
return _sendCommand(CMD_RESET_ENCODERS,
@@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
return sum;
}
int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
size_t n_data, uint16_t & prev_sum)
{
tcflush(_uart, TCIOFLUSH); // flush buffers
@@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
return write(_uart, buf, n_data + 3);
}
int roboclawTest(const char *deviceName, uint8_t address,
int roboclawTest(const char *deviceName, uint8_t address,
uint16_t pulsesPerRev)
{
printf("roboclaw test: starting\n");