mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
Fixed disarming
This commit is contained in:
Timothy Scott
Julian Oes
parent
b5cf8416b6
commit
f780191c5a
@ -93,8 +93,6 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerR
|
||||
_uart(0),
|
||||
_uart_set(),
|
||||
_uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US},
|
||||
_actuatorsOrbID(ORB_ID(actuator_controls_0)),
|
||||
_wheelEncodersOrbID(ORB_ID(wheel_encoders)),
|
||||
_lastEncoderCount{0, 0},
|
||||
_encoderCounts{0, 0},
|
||||
_motorSpeeds{0, 0}
|
||||
@ -139,32 +137,63 @@ RoboClaw::~RoboClaw()
|
||||
|
||||
void RoboClaw::taskMain()
|
||||
{
|
||||
|
||||
// This main loop performs two different tasks, asynchronously:
|
||||
// - Send actuator_controls_0 to the Roboclaw as soon as they are available
|
||||
// - Read the encoder values at a constant rate
|
||||
// To do this, the timeout on the poll() function is used.
|
||||
// waitTime is the amount of time left (int microseconds) until the next time I should read from the encoders.
|
||||
// It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before
|
||||
// I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return
|
||||
// immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment)
|
||||
uint64_t encoderTaskLastRun = 0;
|
||||
int waitTime = 0;
|
||||
|
||||
_actuatorsSub = orb_subscribe(_actuatorsOrbID);
|
||||
_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
|
||||
orb_set_interval(_actuatorsSub, ACTUATOR_WRITE_PERIOD_MS);
|
||||
_actuatorsPoll.fd = _actuatorsSub;
|
||||
_actuatorsPoll.events = POLLIN;
|
||||
|
||||
_armedSub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
|
||||
//TODO: Add parameter listening
|
||||
pollfd fds[2];
|
||||
fds[0].fd = _actuatorsSub;
|
||||
fds[0].events = POLLIN;
|
||||
fds[1].fd = _armedSub;
|
||||
fds[1].events = POLLIN;
|
||||
|
||||
memset((void *) &_wheelEncoderMsg, 0, sizeof(wheel_encoders_s));
|
||||
_wheelEncoderMsg.timestamp = hrt_absolute_time();
|
||||
_wheelEncodersAdv = orb_advertise(_wheelEncodersOrbID, &_wheelEncoderMsg);
|
||||
_wheelEncodersAdv = orb_advertise(ORB_ID(wheel_encoders), &_wheelEncoderMsg);
|
||||
|
||||
while (!taskShouldExit) {
|
||||
|
||||
int pret = poll(&_actuatorsPoll, 1, waitTime / 1000);
|
||||
int pret = poll(fds, sizeof(fds) / sizeof(pollfd), 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]);
|
||||
// No timeout, update on either the actuator controls or the armed state
|
||||
if (pret > 0 && (fds[0].revents & POLLIN || fds[1].revents & POLLIN)) {
|
||||
orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls);
|
||||
orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed);
|
||||
|
||||
if (drive_ret <= 0 || turn_ret <= 0) {
|
||||
PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret);
|
||||
int drive_ret = 0, turn_ret = 0;
|
||||
|
||||
// Disarmed
|
||||
if (!_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown
|
||||
|| _actuatorArmed.force_failsafe) {
|
||||
// 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);
|
||||
}
|
||||
|
||||
} else {
|
||||
drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]);
|
||||
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());
|
||||
// A timeout occurred, which means that it's time to update the encoders
|
||||
|
||||
} else {
|
||||
encoderTaskLastRun = hrt_absolute_time();
|
||||
@ -186,10 +215,10 @@ void RoboClaw::taskMain()
|
||||
|
||||
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_unsubscribe(_armedSub);
|
||||
orb_unadvertise(_wheelEncodersAdv);
|
||||
}
|
||||
|
||||
@ -345,32 +374,6 @@ 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();
|
||||
// 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)
|
||||
{
|
||||
data = fabs(data);
|
||||
@ -513,40 +516,3 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int RoboClaw::roboclawTest(int argc, char *argv[])
|
||||
{
|
||||
printf("roboclaw test: starting\n");
|
||||
|
||||
// setup
|
||||
RoboClaw roboclaw("/dev/ttyS3", 128, 1200);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
|
||||
char buf[200];
|
||||
|
||||
for (int i = 0; i < 10; i++) {
|
||||
usleep(100000);
|
||||
roboclaw.readEncoder();
|
||||
//roboclaw.readEncoder(RoboClaw::MOTOR_2);
|
||||
roboclaw.printStatus(buf, 200);
|
||||
printf("%s", buf);
|
||||
}
|
||||
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
|
||||
|
||||
for (int i = 0; i < 10; i++) {
|
||||
usleep(100000);
|
||||
roboclaw.readEncoder();
|
||||
//roboclaw.readEncoder(RoboClaw::MOTOR_2);
|
||||
roboclaw.printStatus(buf, 200);
|
||||
printf("%s", buf);
|
||||
}
|
||||
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.0);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.0);
|
||||
|
||||
printf("Test complete\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -48,6 +48,7 @@
|
||||
#include <uORB/SubscriptionPollable.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/wheel_encoders.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/time.h>
|
||||
@ -191,16 +192,14 @@ private:
|
||||
fd_set _uart_set;
|
||||
struct timeval _uart_timeout;
|
||||
|
||||
/** poll structure for control packets */
|
||||
struct pollfd _actuatorsPoll;
|
||||
|
||||
/** actuator controls subscription */
|
||||
int _actuatorsSub;
|
||||
const struct orb_metadata *_actuatorsOrbID;
|
||||
actuator_controls_s _actuatorControls;
|
||||
|
||||
int _armedSub;
|
||||
actuator_armed_s _actuatorArmed;
|
||||
|
||||
orb_advert_t _wheelEncodersAdv;
|
||||
const struct orb_metadata *_wheelEncodersOrbID;
|
||||
wheel_encoders_s _wheelEncoderMsg;
|
||||
|
||||
uint32_t _lastEncoderCount[2];
|
||||
@ -236,7 +235,3 @@ private:
|
||||
int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
|
||||
uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false);
|
||||
};
|
||||
|
||||
// unit testing
|
||||
int roboclawTest(const char *deviceName, uint8_t address,
|
||||
uint16_t pulsesPerRev);
|
||||
|
||||
@ -34,12 +34,12 @@
|
||||
|
||||
|
||||
/**
|
||||
* @ file roboclaw_main.cpp
|
||||
* @file roboclaw_main.cpp
|
||||
*
|
||||
* RoboClaw Motor Driver
|
||||
*
|
||||
* references:
|
||||
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
|
||||
* http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf
|
||||
*
|
||||
*/
|
||||
|
||||
@ -111,34 +111,6 @@ int roboclaw_main(int argc, char *argv[])
|
||||
(char *const *)argv);
|
||||
return 0;
|
||||
|
||||
} else if (!strcmp(argv[1], "test")) {
|
||||
|
||||
const char *deviceName = "/dev/ttyS3";
|
||||
uint8_t address = 128;
|
||||
uint16_t pulsesPerRev = 1200;
|
||||
|
||||
if (argc == 2) {
|
||||
printf("testing with default settings\n");
|
||||
|
||||
} else if (argc != 4) {
|
||||
printf("usage: roboclaw test device address pulses_per_rev\n");
|
||||
return -1;
|
||||
|
||||
} else {
|
||||
deviceName = argv[2];
|
||||
address = strtoul(argv[3], nullptr, 0);
|
||||
pulsesPerRev = strtoul(argv[4], nullptr, 0);
|
||||
}
|
||||
|
||||
printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
|
||||
deviceName, address, pulsesPerRev);
|
||||
|
||||
//RoboClaw::roboclawTest(deviceName, address, pulsesPerRev);
|
||||
px4_task_spawn_cmd("robclwtst", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2500, RoboClaw::roboclawTest,
|
||||
(char *const *)argv);
|
||||
RoboClaw::taskShouldExit = true;
|
||||
return 0;
|
||||
|
||||
} else if (!strcmp(argv[1], "stop")) {
|
||||
|
||||
RoboClaw::taskShouldExit = true;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user