/*
* Encoder publisher node
*/
msg_t encoder_node(void *arg) {
encoder_node_conf * conf = (encoder_node_conf *) arg;
r2p::Node node(conf->name);
r2p::Publisher<r2p::EncoderMsg> enc_pub;
systime_t time;
qeidelta_t delta;
r2p::EncoderMsg *msgp;
(void) arg;
chRegSetThreadName(conf->name);
/* Enable the QEI driver. */
qeiInit();
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
node.advertise(enc_pub, conf->topic);
for (;;) {
time = chTimeNow();
delta = qeiUpdate(&QEI_DRIVER);
if (enc_pub.alloc(msgp)) {
msgp->delta = delta * conf->t2r;
enc_pub.publish(*msgp);
}
time += MS2ST(20);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
msg_t encoder_node(void *arg) {
Node node("encoder");
Publisher<tEncoderMsg> enc_pub;
systime_t time;
tEncoderMsg *msgp;
(void) arg;
chRegSetThreadName("encoder");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
node.advertise(enc_pub, "encoder"MOTOR_ID_STRING);
for (;;) {
time = chTimeNow();
if (enc_pub.alloc(msgp)) {
msgp->timestamp.sec = chTimeNow();
msgp->timestamp.nsec = chTimeNow();
msgp->delta = T2R(qeiUpdate(&QEI_DRIVER) * 100);
enc_pub.publish(*msgp);
}
time += MS2ST(10);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
msg_t qeipub_node(void *arg) {
Node node("qeipub");
Publisher<tQEIMsg> qei_pub;
systime_t time;
tQEIMsg *msgp;
(void) arg;
chRegSetThreadName("qeipub");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
node.advertise(qei_pub, "qei"MOTOR_ID_STRING);
for (;;) {
time = chTimeNow();
int16_t delta = qeiUpdate(&QEI_DRIVER);
if (qei_pub.alloc(msgp)) {
msgp->timestamp.sec = 0;
msgp->timestamp.nsec = 0;
msgp->delta = delta;
qei_pub.publish(*msgp);
}
time += MS2ST(50);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
msg_t encoder_node(void *arg) {
Node node("encoder");
Publisher<tEncoderMsg> enc_pub;
systime_t time;
tEncoderMsg *msgp;
(void) arg;
chRegSetThreadName("encoder");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
switch (stm32_id8()) {
case M1:
node.advertise(enc_pub, "encoder1");
break;
case M2:
node.advertise(enc_pub, "encoder2");
break;
case M3:
node.advertise(enc_pub, "encoder3");
break;
default:
node.advertise(enc_pub, "encoder");
break;
}
for (;;) {
time = chTimeNow();
if (enc_pub.alloc(msgp)) {
msgp->timestamp.sec = chTimeNow();
msgp->timestamp.nsec = chTimeNow();
msgp->delta = T2R(qeiUpdate(&QEI_DRIVER) * 100);
enc_pub.publish(*msgp);
}
time += MS2ST(10);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
msg_t qeipub_node(void *arg) {
Node node("qeipub");
Publisher<tQEIMsg> qei_pub;
systime_t time;
tQEIMsg *msgp;
(void) arg;
chRegSetThreadName("qeipub");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
switch (stm32_id8()) {
case M1:
node.advertise(qei_pub, "qei1");
break;
case M2:
node.advertise(qei_pub, "qei2");
break;
case M3:
node.advertise(qei_pub, "qei3");
break;
default:
node.advertise(qei_pub, "qei");
break;
}
for (;;) {
time = chTimeNow();
int16_t delta = qeiUpdate(&QEI_DRIVER);
if (qei_pub.alloc(msgp)) {
msgp->timestamp.sec = 0;
msgp->timestamp.nsec = 0;
msgp->delta = delta;
qei_pub.publish(*msgp);
}
time += MS2ST(50);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}