dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Roboclaw: Initial cleanup, next commit will be refactor removing the duplicated write and read functions

This commit is contained in:
PerFrivik 2023-10-11 15:40:41 +02:00 committed by Matthias Grob
parent fe4d319ba9
commit dde7bbb4f6
1 changed files with 3 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
src/drivers/roboclaw/RoboClaw.cpp
View File

@ -161,7 +161,7 @@ void RoboClaw::taskMain()
PX4_ERR("Shutting down Roboclaw driver.");
return;
} else {
PX4_ERR("Successfully connected");
PX4_INFO("Successfully connected");
}
// This main loop performs two different tasks, asynchronously:
@ -177,7 +177,7 @@ void RoboClaw::taskMain()
int waitTime = 10_ms;
// uint64_t encoderTaskLastRun = 0;
uint64_t encoderTaskLastRun = 0;
uint64_t actuatorsLastWritten = 0;
@ -208,12 +208,6 @@ void RoboClaw::taskMain()
bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms;
// vehicle_control_poll();
// readEncoder();
// printStatus();
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate);
_parameters_update();
@ -229,12 +223,10 @@ void RoboClaw::taskMain()
if (disarmed) {
// printf("i am disarmed \n");
PX4_ERR("Not armed so im not driving");
setMotorSpeed(MOTOR_1, 0.f);
setMotorSpeed(MOTOR_2, 0.f);
} else {
PX4_ERR("Armed so I can be driving");
vehicle_control_poll();
const float yaw = (double)vehicle_thrust_setpoint.xyz[0]; //temporary change
// printf("thrust %f\n", (double)throttle);
@ -247,7 +239,7 @@ void RoboClaw::taskMain()
actuatorsLastWritten = hrt_absolute_time();
} else {
PX4_ERR("im am going to try to readEncoder");
if (readEncoder() > 0) {
for (int i = 0; i < 2; i++) {
@ -287,17 +279,13 @@ int RoboClaw::readEncoder()
for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) {
success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false,
true) == ENCODER_MESSAGE_SIZE;
PX4_ERR("success 1 %f", (double)success);
success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false,
true) == ENCODER_SPEED_MESSAGE_SIZE;
PX4_ERR("success 2 %f", (double)success);
success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false,
true) == ENCODER_SPEED_MESSAGE_SIZE;
PX4_ERR("success 3 %f", (double)success);
}
if (!success) {
PX4_ERR("Error reading encoders");
return -1;
}
@ -523,11 +511,6 @@ RoboClaw::RoboClawError RoboClaw::writeToDevice(e_command cmd, uint8_t *wbuff, s
tcflush(_uart, TCIOFLUSH); // flush the buffers
// // Construct the packet to be sent.
// uint8_t buf[wbytes + 4];
// buf[0] = (uint8_t) _parameters.address;
// buf[1] = cmd;
if (wbuff) {
memcpy(&buf[2], wbuff, wbytes);
}
@ -606,11 +589,7 @@ void RoboClaw::printError(RoboClawError error)
switch (error) {
case RoboClawError::Success:
// Optionally print success message or do nothing
break;
// case RoboClawError::InvalidUartFileDescriptor:
// PX4_ERR("Invalid UART file descriptor");
// break;
case RoboClawError::WriteError:
PX4_ERR("Failed to write all bytes to the device");
break;
@ -620,7 +599,6 @@ void RoboClaw::printError(RoboClawError error)
case RoboClawError::ReadTimeout:
PX4_ERR("Timeout while reading from the device");
break;
// Add more cases as needed for other error codes
default:
PX4_ERR("Unknown error code");
}
@ -656,13 +634,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
// int count = write(_uart, buf, wbytes);
int count = write(_uart, buf, wbytes);
PX4_INFO("wrote this many bytes: %d", count);
PX4_INFO("Written data: ");
for (int i = 0; i < count; ++i) {
PX4_INFO("0x%08X (%d)", buf[i], buf[i]);
}
if (count < (int) wbytes) { // Did not successfully send all bytes.
PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes);
@ -690,17 +661,10 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
// An error code of 0 means that select timed out, which is how the Roboclaw indicates an error.
if (err_code <= 0) {
PX4_INFO("I timed out lol");
return err_code;
}
PX4_INFO("Attempting to read data...");
err_code = read(_uart, rbuff_curr, rbytes - bytes_read);
PX4_INFO("Read call returned: %d", err_code);
for (int i = 0; i < err_code; i++) {
PX4_ERR("Received byte printt %d: 0x%08X (%d)", i, rbuff_curr[i], rbuff_curr[i]);
}
if (err_code <= 0) {
return err_code;