diff --git a/msg/wheel_encoders.msg b/msg/wheel_encoders.msg index c0a47a761d..63a231cbab 100644 --- a/msg/wheel_encoders.msg +++ b/msg/wheel_encoders.msg @@ -9,7 +9,7 @@ uint64 timestamp # time since system start (microseconds) # TODO: How large should the arrays be? What if we have a 6-wheeled rover? bool[4] has_encoder # True for each wheel that has an encoder int64[4] encoder_position # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation -float32[4] speed # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse +int32[4] speed # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse # TODO: Should this be just one uint32, assuming each wheel has the same encoder? uint32[4] pulses_per_rev # Number of pulses per revolution for each wheel diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 98510a6c3f..9f5164badf 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -209,13 +209,19 @@ void RoboClaw::taskMain() if (readEncoder() > 0) { _wheelEncoderMsg.timestamp = encoderTaskLastRun; + _wheelEncoderMsg.encoder_position[0] = _encoderCounts[0]; _wheelEncoderMsg.encoder_position[1] = _encoderCounts[1]; - //PX4_INFO("[%llu] PUBLISHING", _wheelEncoderMsg.timestamp); - orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv, &_wheelEncoderMsg); + _wheelEncoderMsg.speed[0] = _motorSpeeds[0]; + _wheelEncoderMsg.speed[1] = _motorSpeeds[1]; - //PX4_INFO("[%llu] Reading encoders", hrt_absolute_time()); + if (_wheelEncodersAdv == nullptr) { + _wheelEncodersAdv = orb_advertise(ORB_ID(wheel_encoders), &_wheelEncoderMsg); + + } else { + orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv, &_wheelEncoderMsg); + } } else { PX4_ERR("Error reading encoders"); @@ -229,35 +235,45 @@ void RoboClaw::taskMain() orb_unsubscribe(_actuatorsSub); orb_unsubscribe(_armedSub); orb_unsubscribe(_paramSub); - orb_unadvertise(_wheelEncodersAdv); } int RoboClaw::readEncoder() { - uint8_t rbuff[ENCODER_MESSAGE_SIZE]; - int nread = 0; + uint8_t rbuff_pos[ENCODER_MESSAGE_SIZE]; + uint8_t rbuff_speed[11]; - for (int retry = 0; retry < _parameters.serial_timeout_retries && nread == 0; retry++) { - nread = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff[0], ENCODER_MESSAGE_SIZE, false, true); + bool success = false; + + for (int retry = 0; retry < _parameters.serial_timeout_retries && !success; retry++) { + success = true; + success &= _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, + true) == ENCODER_MESSAGE_SIZE; + success &= _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], 7, false, true) == 7; + success &= _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], 7, false, true) == 7; } - if (nread < ENCODER_MESSAGE_SIZE) { - PX4_ERR("Error reading encoders: %d", nread); + if (!success) { + PX4_ERR("Error reading encoders"); return -1; } uint32_t count; + uint32_t speed; uint8_t *count_bytes; + uint8_t *speed_bytes; for (int motor = 0; motor <= 1; motor++) { count = 0; - count_bytes = &rbuff[motor * 4]; + speed = 0; + count_bytes = &rbuff_pos[motor * 4]; + speed_bytes = &rbuff_speed[motor * 4]; // Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the // endianness of the system this code is running on. for (int byte = 0; byte < 4; byte++) { count = (count << 8) + count_bytes[byte]; + speed = (speed << 8) + speed_bytes[byte]; } // The Roboclaw stores encoder counts as unsigned 32-bit ints. This can overflow, especially when starting @@ -275,9 +291,9 @@ int RoboClaw::readEncoder() // At this point, abs(diff) is always <= 2^31, so this cast from unsigned to signed is safe. int32_t diff = fwd_diff <= rev_diff ? fwd_diff : -int32_t(rev_diff); _encoderCounts[motor] += diff; -// PX4_INFO("Motor %d - LastCount: %7u, Count: %7u, Diff1: %8u, Diff2: %8u, diff: %4d, End counts: %4lld", -// motor, _lastEncoderCount[motor], count, fwd_diff, rev_diff, diff, _encoderCounts[motor]); _lastEncoderCount[motor] = count; + + _motorSpeeds[motor] = speed; } return 1; @@ -517,7 +533,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, return bytes_read; } else { - //PX4_ERR("Invalid checksum. Expected 0x%04X, got 0x%04X", checksum_calc, checksum_recv); return -10; } diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 7a021cf188..7a28dd45e2 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -160,8 +160,11 @@ private: // encoder commands CMD_READ_ENCODER_1 = 16, CMD_READ_ENCODER_2 = 17, + CMD_READ_SPEED_1 = 18, + CMD_READ_SPEED_2 = 19, CMD_RESET_ENCODERS = 20, CMD_READ_BOTH_ENCODERS = 78, + CMD_READ_BOTH_SPEEDS = 79, // advanced motor control CMD_READ_SPEED_HIRES_1 = 30, diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 0bbeb66d9e..2976d5cb65 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -45,6 +45,7 @@ #include #include +#include #include #include #include @@ -57,7 +58,7 @@ #include "RoboClaw.hpp" static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ +px4_task_t deamon_task; /** * Deamon management function. @@ -76,7 +77,48 @@ static void usage(); static void usage() { - PX4_INFO("usage: roboclaw {start|stop|status|test}"); + PRINT_MODULE_USAGE_NAME("roboclaw", "driver"); + + PRINT_MODULE_DESCRIPTION(R"DESCR_STR( +### Description + +This driver communicates over UART with the [Roboclaw motor driver](http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf). +It performs two tasks: + + - Control the motors based on the `actuator_controls_0` UOrb topic. + - Read the wheel encoders and publish the raw data in the `wheel_encoders` UOrb topic + +In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and +your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. For Pixhawk 4, +use the `UART & I2C B` port, which corresponds to `/dev/ttyS3`. + +### Implementation + +The main loop of this module (Located in `RoboClaw.cpp::task_main()`) performs 2 tasks: + + 1. Write `actuator_controls_0` messages to the Roboclaw as they become available + 2. Read encoder data from the Roboclaw at a constant, fixed rate. + +Because of the latency of UART, this driver does not write every single `actuator_controls_0` message to the Roboclaw +immediately. Instead, it is rate limited based on the parameter `RBCLW_WRITE_PER`. + +On startup, this driver will attempt to read the status of the Roboclaw to verify that it is connected. If this fails, +the driver terminates immediately. + +### Examples + +The command to start this driver is: + + $ roboclaw start + +`` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`. + +All available commands are: + + - `$ roboclaw start ` + - `$ roboclaw status` + - `$ roboclaw stop` + )DESCR_STR"); } /**