diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 986865d45ba..eef98cd8938 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -142,6 +142,7 @@ set(msg_files vehicle_status_flags.msg vehicle_trajectory_waypoint.msg vtol_vehicle_status.msg + wheel_encoders.msg wind_estimate.msg ) diff --git a/msg/wheel_encoders.msg b/msg/wheel_encoders.msg index eaf7e66edd3..c0a47a761d3 100644 --- a/msg/wheel_encoders.msg +++ b/msg/wheel_encoders.msg @@ -1,15 +1,15 @@ # TODO: How should this mapping be done? What if there's a 6-wheeled (or more) rover? -uint8_t FRONT_RIGHT = 0 -uint8_t FRONT_LEFT = 1 -uint8_t REAR_RIGHT = 2 -uint8_t REAR_LEFT = 3 +uint8 FRONT_RIGHT = 0 +uint8 FRONT_LEFT = 1 +uint8 REAR_RIGHT = 2 +uint8 REAR_LEFT = 3 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 -float[4] speed # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse +float32[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 362f01733fd..0feb9b64ba7 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -61,11 +61,10 @@ // The RoboClaw has a serial communication timeout of 10ms. #define TIMEOUT_US 10000 -// TODO: Make this a parameter +// TODO: Make these all parameters #define FAILED_TRANSACTION_RETRIES 1 - -// The RoboClaw determines the change in the wheel encoder value when it overflows -#define OVERFLOW_AMOUNT 0x100000000LL +#define ENCODER_READ_PERIOD_MS 10 +#define ACTUATOR_WRITE_PERIOD_MS 10 // TODO: Delete this //void printbytes(const char *msg, uint8_t *bytes, int numbytes) @@ -86,24 +85,21 @@ // PX4_INFO("%s", buff); //} +bool RoboClaw::taskShouldExit = false; + RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerRev): - ScheduledWorkItem(px4::wq_configurations::hp_default), _address(address), _pulsesPerRev(pulsesPerRev), _uart(0), + _uart_set(), _uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US}, - _uart_mutex(PTHREAD_MUTEX_INITIALIZER), - _controlPoll(), - _actuators(ORB_ID(actuator_controls_0), 20), + _actuatorsOrbID(ORB_ID(actuator_controls_0)), + _wheelEncodersOrbID(ORB_ID(wheel_encoders)), _lastEncoderCount{0, 0}, _encoderCounts{0, 0}, _motorSpeeds{0, 0} { - // setup control polling - _controlPoll.fd = _actuators.getHandle(); - _controlPoll.events = POLLIN; - // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); @@ -130,8 +126,6 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerR FD_ZERO(&_uart_set); - pthread_mutex_init(&_uart_mutex, nullptr); - // setup default settings, reset encoders resetEncoders(); } @@ -141,17 +135,62 @@ RoboClaw::~RoboClaw() setMotorDutyCycle(MOTOR_1, 0.0); setMotorDutyCycle(MOTOR_2, 0.0); close(_uart); - - pthread_mutex_destroy(&_uart_mutex); } -void RoboClaw::Run() +void RoboClaw::taskMain() { - readEncoder(); - //readEncoder(MOTOR_2); + uint64_t encoderTaskLastRun = 0; + int waitTime = 0; - //PX4_INFO("Motor1: (%d, %d), Motor2: (%d, %d)", _motor1EncoderCounts, _motor1Revolutions, _motor2EncoderCounts, - // _motor2Revolutions); + _actuatorsSub = orb_subscribe(_actuatorsOrbID); + orb_set_interval(_actuatorsSub, ACTUATOR_WRITE_PERIOD_MS); + _actuatorsPoll.fd = _actuatorsSub; + _actuatorsPoll.events = POLLIN; + + memset((void *) &_wheelEncoderMsg, 0, sizeof(wheel_encoders_s)); + _wheelEncoderMsg.timestamp = hrt_absolute_time(); + _wheelEncodersAdv = orb_advertise(_wheelEncodersOrbID, &_wheelEncoderMsg); + + while (!taskShouldExit) { + + int pret = poll(&_actuatorsPoll, 1, waitTime / 1000); + + if (pret > 0 && _actuatorsPoll.revents & POLLIN) { + orb_copy(_actuatorsOrbID, _actuatorsSub, &_actuatorControls); + int drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]); + int turn_ret = turn(_actuatorControls.control[actuator_controls_s::INDEX_YAW]); + + if (drive_ret <= 0 || turn_ret <= 0) { + PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret); + } + + //PX4_INFO("[%llu] Writing actuators", hrt_absolute_time()); + + } else { + encoderTaskLastRun = hrt_absolute_time(); + + 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); + + //PX4_INFO("[%llu] Reading encoders", hrt_absolute_time()); + + } else { + PX4_ERR("Error reading encoders"); + } + } + + waitTime = ENCODER_READ_PERIOD_MS * 1000 - (hrt_absolute_time() - encoderTaskLastRun); + waitTime = waitTime < 0 ? 0 : waitTime; + //PX4_INFO("ROBOCLAW WAIT TIME: %d", waitTime); + } + + orb_unsubscribe(_actuatorsSub); + orb_unadvertise(_wheelEncodersAdv); } int RoboClaw::readEncoder() @@ -306,33 +345,31 @@ int RoboClaw::resetEncoders() return _sendNothing(CMD_RESET_ENCODERS); } -int RoboClaw::update() -{ - //TODO: Also update motor locations and speeds here - - // wait for an actuator publication, - // 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 new data, send to motors - if (_actuators.updated()) { - _actuators.update(); - // setMotorDutyCycle(MOTOR_1, _actuators.get().control[actuator_controls_s::INDEX_]); - // setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]); - int drive_ret = drive(_actuators.get().control[actuator_controls_s::INDEX_THROTTLE]); - int turn_ret = turn(_actuators.get().control[actuator_controls_s::INDEX_YAW]); - - if (drive_ret <= 0 || turn_ret <= 0) { - PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret); - } - } - - Run(); - - return 0; -} +//int RoboClaw::update() +//{ +// //TODO: Also update motor locations and speeds here +// +// // wait for an actuator publication, +// // 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 new data, send to motors +// if (_actuators.updated()) { +// _actuators.update(); +// int drive_ret = drive(_actuators.get().control[actuator_controls_s::INDEX_THROTTLE]); +// int turn_ret = turn(_actuators.get().control[actuator_controls_s::INDEX_YAW]); +// +// if (drive_ret <= 0 || turn_ret <= 0) { +// PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret); +// } +// } +// +// Run(); +// +// return 0; +//} int RoboClaw::_sendUnsigned7Bit(e_command command, float data) { @@ -390,9 +427,9 @@ uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum) { - // WRITE + int err_code = 0; - pthread_mutex_lock(&_uart_mutex); + // WRITE tcflush(_uart, TCIOFLUSH); // flush buffers uint8_t buf[wbytes + 4]; @@ -415,7 +452,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, if (count < (int) wbytes) { // Did not successfully send all bytes. PX4_ERR("Only wrote %d out of %d bytes", count, (int) wbytes); - pthread_mutex_unlock(&_uart_mutex); return -1; } @@ -424,52 +460,57 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, FD_ZERO(&_uart_set); FD_SET(_uart, &_uart_set); - int rv = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); + uint8_t *rbuff_curr = rbuff; + size_t bytes_read = 0; + + // select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many + // bytes are available. I need to keep reading until I get the number of bytes I expect. + while (bytes_read < rbytes) { + err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); + + if (err_code < 0) { + return err_code; + } + + err_code = read(_uart, rbuff_curr, rbytes - bytes_read); + + if (err_code < 0) { + return err_code; + + } else { + bytes_read += err_code; + rbuff_curr += err_code; + } + } //TODO: Clean up this mess of IFs and returns - if (rv > 0) { - // select() returns as soon as ANY bytes are available. I need to wait until ALL of the bytes are available. - // TODO: Make sure this is not a busy wait. - usleep(2000); - int bytes_read = read(_uart, rbuff, rbytes); + if (recv_checksum) { + if (bytes_read < 2) { + return -1; + } - if (recv_checksum) { - if (bytes_read < 2) { - pthread_mutex_unlock(&_uart_mutex); - return -1; - } + // The checksum sent back by the roboclaw is calculated based on the address and command bytes as well + // as the data returned. + uint16_t checksum_calc = _calcCRC(buf, 2); + checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); + uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; - // The checksum sent back by the roboclaw is calculated based on the address and command bytes as well - // as the data returned. - uint16_t checksum_calc = _calcCRC(buf, 2); - checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); - uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; - - if (checksum_calc == checksum_recv) { - pthread_mutex_unlock(&_uart_mutex); - return bytes_read; - - } else { - //PX4_ERR("Invalid checksum. Expected 0x%04X, got 0x%04X", checksum_calc, checksum_recv); - pthread_mutex_unlock(&_uart_mutex); - return -10; - } + if (checksum_calc == checksum_recv) { + return bytes_read; } else { - if (bytes_read == 1 && rbuff[0] == 0xFF) { - pthread_mutex_unlock(&_uart_mutex); - return 1; - - } else { - pthread_mutex_unlock(&_uart_mutex); - return -11; - } + //PX4_ERR("Invalid checksum. Expected 0x%04X, got 0x%04X", checksum_calc, checksum_recv); + return -10; } } else { - pthread_mutex_unlock(&_uart_mutex); - return rv; + if (bytes_read == 1 && rbuff[0] == 0xFF) { + return 1; + + } else { + return -11; + } } } diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 78f180d6aca..2a037fc89b3 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -47,21 +47,22 @@ #include #include #include +#include #include #include #include #include -//#include -#include /** * This is a driver for the RoboClaw motor controller */ -class RoboClaw : public px4::ScheduledWorkItem +class RoboClaw { public: static int roboclawTest(int argc, char *argv[]); + void taskMain(); + static bool taskShouldExit; /** control channels */ enum e_channel { @@ -190,19 +191,22 @@ private: fd_set _uart_set; struct timeval _uart_timeout; - pthread_mutex_t _uart_mutex; - /** poll structure for control packets */ - struct pollfd _controlPoll; + struct pollfd _actuatorsPoll; /** actuator controls subscription */ - uORB::SubscriptionPollable _actuators; + int _actuatorsSub; + const struct orb_metadata *_actuatorsOrbID; + actuator_controls_s _actuatorControls; + + orb_advert_t _wheelEncodersAdv; + const struct orb_metadata *_wheelEncodersOrbID; + wheel_encoders_s _wheelEncoderMsg; uint32_t _lastEncoderCount[2]; int64_t _encoderCounts[2]; int32_t _motorSpeeds[2]; - static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0); int _sendUnsigned7Bit(e_command command, float data); int _sendSigned16Bit(e_command command, float data); @@ -211,8 +215,7 @@ private: /** * Perform a round-trip write and read. * - * NOTE: This function uses a mutex contained in this class. This makes it thread-safe, but also a potential - * source of deadlock. + * NOTE: This function is not thread-safe. * * @param cmd Command to send to the Roboclaw * @param wbuff Write buffer. Must not contain command, address, or checksum. For most commands, this will be diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index bf8ef91f064..46d6171fe1d 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -56,7 +56,6 @@ #include #include "RoboClaw.hpp" -static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -100,17 +99,17 @@ int roboclaw_main(int argc, char *argv[]) if (thread_running) { printf("roboclaw already running\n"); /* this is not an error */ - exit(0); + return 0; } - thread_should_exit = false; + RoboClaw::taskShouldExit = false; deamon_task = px4_task_spawn_cmd("roboclaw", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2500, roboclaw_thread_main, (char *const *)argv); - exit(0); + return 0; } else if (!strcmp(argv[1], "test")) { @@ -123,7 +122,7 @@ int roboclaw_main(int argc, char *argv[]) } else if (argc != 4) { printf("usage: roboclaw test device address pulses_per_rev\n"); - exit(-1); + return -1; } else { deviceName = argv[2]; @@ -137,13 +136,13 @@ int roboclaw_main(int argc, char *argv[]) //RoboClaw::roboclawTest(deviceName, address, pulsesPerRev); px4_task_spawn_cmd("robclwtst", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2500, RoboClaw::roboclawTest, (char *const *)argv); - thread_should_exit = true; - exit(0); + RoboClaw::taskShouldExit = true; + return 0; } else if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); + RoboClaw::taskShouldExit = true; + return 0; } else if (!strcmp(argv[1], "status")) { @@ -154,11 +153,11 @@ int roboclaw_main(int argc, char *argv[]) printf("\troboclaw app not started\n"); } - exit(0); + return 0; } usage(); - exit(1); + return 1; } int roboclaw_thread_main(int argc, char *argv[]) @@ -186,21 +185,10 @@ int roboclaw_thread_main(int argc, char *argv[]) thread_running = true; - //TODO: Make constants - //roboclaw.ScheduleOnInterval(1000000, 1000000); - - // TODO: Move the main loop into the class - // loop - while (!thread_should_exit) { - roboclaw.update(); - } - - //roboclaw.ScheduleClear(); + roboclaw.taskMain(); // exit printf("[roboclaw] exiting.\n"); thread_running = false; return 0; } - -// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78