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

Changed wheel encoder publication to multi-instance

This commit is contained in:
Timothy Scott 2019-07-19 16:36:16 +02:00 committed by Julian Oes
parent 3850322046
commit 830d576f45
3 changed files with 25 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
msg/wheel_encoders.msg
View File

@ -1,15 +1,5 @@
# TODO: How should this mapping be done? What if there's a 6-wheeled (or more) rover?
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
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
int64 encoder_position # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation
int32 speed # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse
uint32 pulses_per_rev # Number of pulses per revolution for each wheel

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

@ -170,9 +170,9 @@ void RoboClaw::taskMain()
fds[2].fd = _armedSub;
fds[2].events = POLLIN;
memset((void *) &_wheelEncoderMsg, 0, sizeof(wheel_encoders_s));
_wheelEncoderMsg.timestamp = hrt_absolute_time();
_wheelEncodersAdv = orb_advertise(ORB_ID(wheel_encoders), &_wheelEncoderMsg);
memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg));
_wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev;
_wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev;
while (!taskShouldExit) {
@ -221,19 +221,21 @@ void RoboClaw::taskMain()
encoderTaskLastRun = hrt_absolute_time();
if (readEncoder() > 0) {
_wheelEncoderMsg.timestamp = encoderTaskLastRun;
_wheelEncoderMsg.encoder_position[0] = _encoderCounts[0];
_wheelEncoderMsg.encoder_position[1] = _encoderCounts[1];
for (int i = 0; i < 2; i++) {
_wheelEncoderMsg[i].timestamp = encoderTaskLastRun;
_wheelEncoderMsg.speed[0] = _motorSpeeds[0];
_wheelEncoderMsg.speed[1] = _motorSpeeds[1];
_wheelEncoderMsg[i].encoder_position = _encoderCounts[i];
_wheelEncoderMsg[i].speed = _motorSpeeds[i];
if (_wheelEncodersAdv == nullptr) {
_wheelEncodersAdv = orb_advertise(ORB_ID(wheel_encoders), &_wheelEncoderMsg);
if (_wheelEncodersAdv[i] == nullptr) {
int instance;
_wheelEncodersAdv[i] = orb_advertise_multi(ORB_ID(wheel_encoders), &_wheelEncoderMsg[i],
&instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv, &_wheelEncoderMsg);
} else {
orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv[i], &_wheelEncoderMsg[i]);
}
}
} else {

16
src/drivers/roboclaw/RoboClaw.hpp
View File

@ -195,21 +195,21 @@ private:
struct timeval _uart_timeout;
/** actuator controls subscription */
int _actuatorsSub;
int _actuatorsSub{-1};
actuator_controls_s _actuatorControls;
int _armedSub;
int _armedSub{-1};
actuator_armed_s _actuatorArmed;
int _paramSub;
int _paramSub{-1};
parameter_update_s _paramUpdate;
orb_advert_t _wheelEncodersAdv;
wheel_encoders_s _wheelEncoderMsg;
orb_advert_t _wheelEncodersAdv[2] {nullptr, nullptr};
wheel_encoders_s _wheelEncoderMsg[2];
uint32_t _lastEncoderCount[2];
int64_t _encoderCounts[2];
int32_t _motorSpeeds[2];
uint32_t _lastEncoderCount[2] {0, 0};
int64_t _encoderCounts[2] {0, 0};
int32_t _motorSpeeds[2] {0, 0};
void _parameters_update();