static void read_sample_from_ring(void *arg, long period)
{
ringbuffer_t *rb = (ringbuffer_t *) arg;
hal_delayline_t *hd = rb->scratchpad;
const sample_t *s;
ringsize_t size;
// detect rising edge on abort pin, and flush rb if so
if (*(hd->abort) && (*(hd->abort) ^ hd->last_abort)) {
int dropped = record_flush_reader(rb);
rtapi_print_msg(RTAPI_MSG_INFO,
"%s: %s aborted - dropped %d samples\n",
cname, hd->name, dropped);
}
// if pin_delay < 0 then use 0, otherwise
// use delay pin value
// if time has decreased, then take action by setting the output_ts to the
// result of input_ts - delay. Otherwise do nothing. This will result in
// loss of some records. Make sure you're in a safe situation!
// The situation of increasing the time will be acted upon in
// write_sample_to_ring()
if ( *(hd->pin_delay) < (hal_u32_t)(hd->input_ts - hd->output_ts)) {
if ( *(hd->pin_delay) < 0) {
hd->delay = 0;
}
else {
hd->delay = *(hd->pin_delay);
}
// set the new timestamp
hd->input_ts = hd->input_ts - (__u64)(hd->delay);
}
// peek at the head of the queue
while (record_read(rb, (const void **)&s, &size) == 0) {
// do nothing if timestamp is in the future
if (s->timestamp > hd->output_ts)
goto NOTYET;
if (s->timestamp == hd->output_ts)
// the time is right
apply(s, hd);
else
// skip old samples and bump an error counter
*(hd->too_old) += 1;
// sanity: if (size != hd->sample_size).. terribly wrong.
record_shift(rb); // consume record
}
NOTYET:
hd->output_ts++; // always bump the timestamp
hd->last_abort = *(hd->abort);
}
// thread function, per-instance
// interpolates all joints of this instance
static int update(void *arg, const hal_funct_args_t *fa)
{
struct inst_data *ip = (struct inst_data *) arg;
double period = ((double) fa_period(fa)) * 1e-9;
int i;
if (segment_completed(ip, period)) {
// check for a new JointTrajectoryPoint
void *data;
ringsize_t size;
if (record_read(&ip->traj, (const void**)&data, &size) == 0) {
// protobuf-decode it
pb_istream_t stream = pb_istream_from_buffer(data, size);
pb_JointTrajectoryPoint rx = pb_JointTrajectoryPoint_init_zero;
if (!pb_decode(&stream, pb_JointTrajectoryPoint_fields, &rx)) {
rtapi_print_msg(RTAPI_MSG_ERR, "%s: pb_decode(JointTrajectoryPoint) failed: '%s'",
compname, PB_GET_ERROR(&stream));
} else {
// decode ok - start a new segment
double duration = *(ip->duration) = rx.time_from_start - ip->time_from_start;
// the very first point in the ringbuffer is not a segment.
// therefore we need to "jump" to these initial settings for the
// interpolator to calculate the correct path.
// for example, a path can start at position, velocity and acceleration
// who are non-zero. In a typical ROS message the first point has a
// duration of "0.0"
if (duration == 0.0) {
// set the start positions
// or try out to drop this point later on
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = true;
*(jp->curr_pos) = *(jp->end_pos) = rx.positions[i];
*(jp->curr_vel) = *(jp->end_vel) = rx.velocities[i];
*(jp->curr_acc) = *(jp->end_acc) = rx.accelerations[i];
jp->coeff[0] = *(jp->end_pos);
jp->coeff[1] = 0.0;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
}
// so when we have read the first point, we need to discard everythin
// else and make sure we will read the second point, as to complete the
// first segment
} else {
generatePowers(*(ip->degree), duration, ip->powers);
ip->time_from_start = rx.time_from_start;
*(ip->progress) = 0.0;
for (i = 0; i < rx.positions_count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = true;
double pos2 = *(jp->end_pos) = rx.positions[i];
double vel2 = *(jp->end_vel) = rx.velocities[i];
double acc2 = *(jp->end_acc) = rx.accelerations[i];
double pos1 = *(jp->curr_pos);
double vel1 = *(jp->curr_vel);
double acc1 = *(jp->curr_acc);
switch (*(ip->degree)) {
case 1:
jp->coeff[0] = pos1;
jp->coeff[1] = (pos2 - pos1) / duration;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
break;
case 3:
jp->coeff[0] = pos1;
jp->coeff[1] = vel1;
jp->coeff[2] = (-3.0*pos1 + 3.0*pos2 - 2.0*vel1*ip->powers[1] - vel2*ip->powers[1]) / ip->powers[2];
jp->coeff[3] = (2.0*pos1 - 2.0*pos2 + vel1*ip->powers[1] + vel2*ip->powers[1]) / ip->powers[3];
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
break;
case 5:
jp->coeff[0] = pos1;
jp->coeff[1] = vel1;
jp->coeff[2] = 0.5 * acc1;
jp->coeff[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*ip->powers[2] + acc2*ip->powers[2] -
12.0*vel1*ip->powers[1] - 8.0*vel2*ip->powers[1]) / (2.0*ip->powers[3]);
jp->coeff[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*ip->powers[2] - 2.0*acc2*ip->powers[2] +
16.0*vel1*ip->powers[1] + 14.0*vel2*ip->powers[1]) / (2.0*ip->powers[4]);
jp->coeff[5] = (-12.0*pos1 + 12.0*pos2 - acc1*ip->powers[2] + acc2*ip->powers[2] -
6.0*vel1*ip->powers[1] - 6.0*vel2*ip->powers[1]) / (2.0*ip->powers[5]);
break;
}
}
}
}
record_shift(&ip->traj); // consume record
} else {
// segment completed and no new point in ringbuffer
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = false;
jp->coeff[0] = *(jp->end_pos);
jp->coeff[1] = 0.0;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
}
}
}
*(ip->progress) += period;
generatePowers(*(ip->degree), *(ip->progress), ip->pnow);
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
interpolate_joint(ip, jp, *(ip->progress), 0);
}
return 0;
}
static int message_thread()
{
rtapi_msgheader_t *msg;
size_t msg_size;
size_t payload_length;
int retval;
char *cp;
int sigfd;
sigset_t sigset;
// sigset of all the signals that we're interested in
retval = sigemptyset(&sigset); assert(retval == 0);
retval = sigaddset(&sigset, SIGINT); assert(retval == 0);
retval = sigaddset(&sigset, SIGKILL); assert(retval == 0);
retval = sigaddset(&sigset, SIGTERM); assert(retval == 0);
retval = sigaddset(&sigset, SIGSEGV); assert(retval == 0);
retval = sigaddset(&sigset, SIGFPE); assert(retval == 0);
// block the signals in order for signalfd to receive them
retval = sigprocmask(SIG_BLOCK, &sigset, NULL); assert(retval == 0);
sigfd = signalfd(-1, &sigset, 0);
assert(sigfd != -1);
struct pollfd pfd[1];
int ret;
pfd[0].fd = sigfd;
pfd[0].events = POLLIN | POLLERR | POLLHUP;
global_data->magic = GLOBAL_READY;
do {
if (global_data->rtapi_app_pid == 0) {
syslog_async(LOG_ERR,
"msgd:%d: rtapi_app exit detected - shutting down",
rtapi_instance);
msgd_exit++;
}
while ((retval = record_read(&rtapi_msg_buffer,
(const void **) &msg, &msg_size)) == 0) {
payload_length = msg_size - sizeof(rtapi_msgheader_t);
switch (msg->encoding) {
case MSG_ASCII:
// strip trailing newlines
while ((cp = strrchr(msg->buf,'\n')))
*cp = '\0';
syslog_async(rtapi2syslog(msg->level), "%s:%d:%s %.*s",
msg->tag, msg->pid, origins[msg->origin],
(int) payload_length, msg->buf);
break;
case MSG_STASHF:
break;
case MSG_PROTOBUF:
break;
default: ;
// whine
}
record_shift(&rtapi_msg_buffer);
msg_poll = msg_poll_min;
}
ret = poll(pfd, 1, msg_poll);
if (ret < 0) {
syslog_async(LOG_ERR, "msgd:%d: poll(): %s - shutting down\n",
rtapi_instance, strerror(errno));
msgd_exit++;
} else if (pfd[0].revents & POLLIN) { // signal received
struct signalfd_siginfo info;
size_t bytes = read(sigfd, &info, sizeof(info));
assert(bytes == sizeof(info));
signal_handler(info.ssi_signo);
}
msg_poll += msg_poll_inc;
if (msg_poll > msg_poll_max)
msg_poll = msg_poll_max;
} while (!msgd_exit);
return 0;
}
