From 2406aa8b504c294a31913405a2665c75d47ff216 Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Fri, 5 Jul 2019 15:34:02 +0200 Subject: [PATCH] Further fixing up parameters --- .../airframes/50003_aion_robotics_r1_rover | 12 +++++- src/drivers/roboclaw/RoboClaw.cpp | 40 +++++++++++++++---- src/drivers/roboclaw/RoboClaw.hpp | 21 +++------- src/drivers/roboclaw/roboclaw_main.cpp | 11 ++--- src/drivers/roboclaw/roboclaw_params.c | 37 ++++++++--------- 5 files changed, 70 insertions(+), 51 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index 1636734fd6..4fedb5032a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -17,8 +17,6 @@ sh /etc/init.d/rc.rover_defaults -roboclaw start /dev/ttyS3 128 1200 - if [ $AUTOCNF = yes ] then param set BAT_N_CELLS 4 @@ -68,8 +66,18 @@ then # Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor param set CBRK_AIRSPD_CHK 162128 + + # Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians + param set GND_MAX_ANG 3.1415 + + param set RBCLW_BAUD 8 + param set RBCLW_COUNTS_REV 1200 + param set RBCLW_ADDRESS 128 fi +# Start this driver after setting parameters, because the driver uses some of those parameters. +roboclaw start /dev/ttyS3 + # Configure this as rover set MAV_TYPE 10 diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 2edb0dc565..98510a6c3f 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -71,6 +71,7 @@ RoboClaw::RoboClaw(const char *deviceName): _uart(0), _uart_set(), _uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US}, + _actuatorsSub(-1), _lastEncoderCount{0, 0}, _encoderCounts{0, 0}, _motorSpeeds{0, 0} @@ -80,7 +81,7 @@ RoboClaw::RoboClaw(const char *deviceName): _param_handles.actuator_write_period_ms = param_find("RBCLW_WRITE_PER"); _param_handles.encoder_read_period_ms = param_find("RBCLW_READ_PER"); _param_handles.counts_per_rev = param_find("RBCLW_COUNTS_REV"); - _param_handles.serial_baud_rate = param_find("RMCLW_BAUD"); + _param_handles.serial_baud_rate = param_find("RBCLW_BAUD"); _param_handles.serial_timeout_retries = param_find("RBCLW_RETRIES"); _param_handles.stop_retries = param_find("RBCLW_STOP_RETRY"); _param_handles.address = param_find("RBCLW_ADDRESS"); @@ -126,6 +127,14 @@ RoboClaw::~RoboClaw() void RoboClaw::taskMain() { + // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. + uint8_t rbuff[4]; + int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); + + if (err_code <= 0) { + PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver."); + return; + } // This main loop performs two different tasks, asynchronously: // - Send actuator_controls_0 to the Roboclaw as soon as they are available @@ -142,13 +151,15 @@ void RoboClaw::taskMain() orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); _armedSub = orb_subscribe(ORB_ID(actuator_armed)); + _paramSub = orb_subscribe(ORB_ID(parameter_update)); - //TODO: Add parameter listening - pollfd fds[2]; - fds[0].fd = _actuatorsSub; + pollfd fds[3]; + fds[0].fd = _paramSub; fds[0].events = POLLIN; - fds[1].fd = _armedSub; + fds[1].fd = _actuatorsSub; fds[1].events = POLLIN; + fds[2].fd = _armedSub; + fds[2].events = POLLIN; memset((void *) &_wheelEncoderMsg, 0, sizeof(wheel_encoders_s)); _wheelEncoderMsg.timestamp = hrt_absolute_time(); @@ -158,8 +169,13 @@ void RoboClaw::taskMain() int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000); + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate); + _parameters_update(); + } + // No timeout, update on either the actuator controls or the armed state - if (pret > 0 && (fds[0].revents & POLLIN || fds[1].revents & POLLIN)) { + if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN)) { orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls); orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed); @@ -170,8 +186,11 @@ void RoboClaw::taskMain() if (disarmed) { // If disarmed, I want to be certain that the stop command gets through. - while ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0) { - //PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret); + int tries = 0; + + while (tries < _parameters.stop_retries && ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0)) { + PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret); + tries++; px4_usleep(TIMEOUT_US); } @@ -209,6 +228,7 @@ void RoboClaw::taskMain() orb_unsubscribe(_actuatorsSub); orb_unsubscribe(_armedSub); + orb_unsubscribe(_paramSub); orb_unadvertise(_wheelEncodersAdv); } @@ -520,6 +540,10 @@ void RoboClaw::_parameters_update() param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms); param_get(_param_handles.address, &_parameters.address); + if (_actuatorsSub > 0) { + orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); + } + int baudRate; param_get(_param_handles.serial_baud_rate, &baudRate); diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 67838520c6..7a021cf188 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -129,12 +130,6 @@ public: */ int resetEncoders(); - /** - * main update loop that updates RoboClaw motor - * dutycycle based on actuator publication - */ - int update(); - /** * read data from serial */ @@ -145,17 +140,8 @@ public: */ void printStatus(char *string, size_t n); - void Run(); - private: - // Quadrature status flags - enum e_quadrature_status_flags { - STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ - STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ - STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ - }; - // commands // We just list the commands we want from the manual here. enum e_command { @@ -182,6 +168,8 @@ private: CMD_READ_SPEED_HIRES_2 = 31, CMD_SIGNED_DUTYCYCLE_1 = 32, CMD_SIGNED_DUTYCYCLE_2 = 33, + + CMD_READ_STATUS = 90 }; struct { @@ -215,6 +203,9 @@ private: int _armedSub; actuator_armed_s _actuatorArmed; + int _paramSub; + parameter_update_s _paramUpdate; + orb_advert_t _wheelEncodersAdv; wheel_encoders_s _wheelEncoderMsg; diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index aa720bccdb..0bbeb66d9e 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -106,7 +106,7 @@ int roboclaw_main(int argc, char *argv[]) deamon_task = px4_task_spawn_cmd("roboclaw", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, - 2500, + 1500, roboclaw_thread_main, (char *const *)argv); return 0; @@ -140,17 +140,12 @@ int roboclaw_thread_main(int argc, char *argv[]) argc -= 2; argv += 2; - if (argc < 3) { - printf("usage: roboclaw start device address\n"); + if (argc < 2) { + printf("usage: roboclaw start \n"); return -1; } const char *deviceName = argv[1]; - uint8_t address = strtoul(argv[2], nullptr, 0); - uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0); - - printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", - deviceName, address, pulsesPerRev); // start RoboClaw roboclaw(deviceName); diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c index 46eccfad8b..d33d19eb44 100644 --- a/src/drivers/roboclaw/roboclaw_params.c +++ b/src/drivers/roboclaw/roboclaw_params.c @@ -75,24 +75,6 @@ PARAM_DEFINE_INT32(RBCLW_READ_PER, 10); */ PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200); -/** - * Roboclaw serial baud rate - * - * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate. - * @min 1 - * @max 8 - * @value 1 2400 baud - * @value 2 9600 baud - * @value 3 19200 baud - * @value 4 38400 baud - * @value 5 57600 baud - * @value 6 115200 baud - * @value 7 230400 baud - * @value 8 460800 baud - * @group Roboclaw driver - */ -PARAM_DEFINE_INT32(RBCLW_BAUD, 8); - /** * Communication retries * @@ -132,3 +114,22 @@ PARAM_DEFINE_INT32(RBCLW_STOP_RETRY, 10); * @group Roboclaw driver */ PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128); + +/** + * Roboclaw serial baud rate + * + * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate. + * @min 1 + * @max 8 + * @value 1 2400 baud + * @value 2 9600 baud + * @value 3 19200 baud + * @value 4 38400 baud + * @value 5 57600 baud + * @value 6 115200 baud + * @value 7 230400 baud + * @value 8 460800 baud + * @group Roboclaw driver + * @reboot_required true + */ +PARAM_DEFINE_INT32(RBCLW_BAUD, 8);