void buffer_refill() { // {{{
if (preparing) {
//debug("no refill because prepare");
return;
}
if (moving_to_current == 2)
move_to_current();
if (!computing_move || refilling || stopping || discard_pending || discarding) {
//debug("refill block %d %d %d %d %d", computing_move, refilling, stopping, discard_pending, discarding);
return;
}
refilling = true;
// send_fragment in the previous refill may have failed; try it again.
if (current_fragment_pos > 0)
send_fragment();
//debug("refill start %d %d %d", running_fragment, current_fragment, sending_fragment);
// Keep one free fragment, because we want to be able to rewind and use the buffer before the one currently active.
while (computing_move && !stopping && !discard_pending && !discarding && (running_fragment - 1 - current_fragment + FRAGMENTS_PER_BUFFER) % FRAGMENTS_PER_BUFFER > 4 && !sending_fragment) {
//debug("refill %d %d %f", current_fragment, current_fragment_pos, spaces[0].motor[0]->settings.current_pos);
// fill fragment until full.
apply_tick();
//debug("refill2 %d %f", current_fragment, spaces[0].motor[0]->settings.current_pos);
if (current_fragment_pos >= SAMPLES_PER_FRAGMENT) {
//debug("fragment full %d %d %d", computing_move, current_fragment_pos, BYTES_PER_FRAGMENT);
send_fragment();
}
// Check for commands from host; in case of many short buffers, this loop may not end in a reasonable time.
//serial(0);
}
if (stopping || discard_pending) {
//debug("aborting refill for stopping");
refilling = false;
return;
}
if (!computing_move && current_fragment_pos > 0) {
//debug("finalize");
send_fragment();
}
refilling = false;
arch_start_move(0);
} // }}}
void buffer_refill() { // {{{
// Try to fill the buffer. This is called at any time that the buffer may be refillable.
//debug("refill");
if (aborting || preparing || FRAGMENTS_PER_BUFFER == 0) {
//debug("no refill because prepare or no buffer yet");
return;
}
if (!computing_move || refilling || stopping || discard_pending || discarding) {
//debug("no refill due to block: %d %d %d %d %d", !computing_move, refilling, stopping, discard_pending, discarding);
return;
}
refilling = true;
// send_fragment in the previous refill may have failed; try it again.
/*if (current_fragment_pos > 0) {
debug("sending because data pending frag=%d pos=%d", current_fragment, current_fragment_pos);
send_fragment();
}*/
//debug("refill start %d %d %d", running_fragment, current_fragment, sending_fragment);
// Keep one free fragment, because we want to be able to rewind and use the buffer before the one currently active.
while (computing_move && !aborting && !stopping && !discard_pending && !discarding && (running_fragment - 1 - current_fragment + FRAGMENTS_PER_BUFFER) % FRAGMENTS_PER_BUFFER > (FRAGMENTS_PER_BUFFER > 4 ? 4 : FRAGMENTS_PER_BUFFER - 2) && !sending_fragment) {
//debug("refill %d %d %f", current_fragment, current_fragment_pos, spaces[0].motor[0]->settings.current_pos);
// fill fragment until full.
apply_tick();
//debug("refill2 %d %f", current_fragment, spaces[0].motor[0]->settings.current_pos);
if (current_fragment_pos >= SAMPLES_PER_FRAGMENT) {
//debug("sending because fragment full (weird) %d %d %d", computing_move, current_fragment_pos, BYTES_PER_FRAGMENT);
send_fragment();
}
}
if (aborting || stopping || discard_pending) {
//debug("aborting refill for stopping");
refilling = false;
return;
}
if (!computing_move && current_fragment_pos > 0) {
//debug("sending because move ended");
send_fragment();
}
refilling = false;
arch_start_move(0);
} // }}}
static void discover_prga(struct wstate *ws)
{
// create packet...
if (!ws->ws_fs.fs_data) {
int pad = 0;
if (ws->ws_pi.pi_len >= 20)
pad = ws->ws_pi.pi_len*3;
prepare_fragstate(ws, &ws->ws_fs, pad);
}
if (!ws->ws_fs.fs_waiting_relay) {
send_fragment(ws, &ws->ws_fs, &ws->ws_pi);
if (ws->ws_fs.fs_waiting_relay) {
if (gettimeofday(&ws->ws_fs.fs_last, NULL) == -1)
err(1, "gettimeofday()");
}
}
}
/**
* Called in user context.
* return 0 if data was added to the buffer and
* -ENODATA if none was available. This should add some number of bits
* evenly divisible by code_length to the buffer
*/
static int igorplugusb_remote_poll(void *data, struct lirc_buffer *buf)
{
int ret;
struct igorplug *ir = (struct igorplug *)data;
if (!ir || !ir->usbdev) /* Has the device been removed? */
return -ENODEV;
memset(ir->buf_in, 0, ir->len_in);
ret = usb_control_msg(ir->usbdev, usb_rcvctrlpipe(ir->usbdev, 0),
GET_INFRACODE, USB_TYPE_VENDOR | USB_DIR_IN,
0/* offset */, /*unused*/0,
ir->buf_in, ir->len_in,
/*timeout*/HZ * USB_CTRL_GET_TIMEOUT);
if (ret > 0) {
int code, timediff;
struct timeval now;
/* ACK packet has 1 byte --> ignore */
if (ret < DEVICE_HEADERLEN)
return -ENODATA;
dprintk(DRIVER_NAME ": Got %d bytes. Header: %*ph\n",
ret, 3, ir->buf_in);
do_gettimeofday(&now);
timediff = now.tv_sec - ir->last_time.tv_sec;
if (timediff + 1 > PULSE_MASK / 1000000)
timediff = PULSE_MASK;
else {
timediff *= 1000000;
timediff += now.tv_usec - ir->last_time.tv_usec;
}
ir->last_time.tv_sec = now.tv_sec;
ir->last_time.tv_usec = now.tv_usec;
/* create leading gap */
code = timediff;
lirc_buffer_write(buf, (unsigned char *)&code);
ir->in_space = 1; /* next comes a pulse */
if (ir->buf_in[2] == 0)
send_fragment(ir, buf, DEVICE_HEADERLEN, ret);
else {
dev_warn(&ir->usbdev->dev,
"[%d]: Device buffer overrun.\n", ir->devnum);
/* HHHNNNNNNNNNNNOOOOOOOO H = header
<---[2]---> N = newer
<---------ret--------> O = older */
ir->buf_in[2] %= ret - DEVICE_HEADERLEN; /* sanitize */
/* keep even-ness to not desync pulse/pause */
send_fragment(ir, buf, DEVICE_HEADERLEN +
ir->buf_in[2] - (ir->buf_in[2] & 1), ret);
send_fragment(ir, buf, DEVICE_HEADERLEN,
DEVICE_HEADERLEN + ir->buf_in[2]);
}
ret = usb_control_msg(
ir->usbdev, usb_rcvctrlpipe(ir->usbdev, 0),
SET_INFRABUFFER_EMPTY, USB_TYPE_VENDOR|USB_DIR_IN,
/*unused*/0, /*unused*/0,
/*dummy*/ir->buf_in, /*dummy*/ir->len_in,
/*timeout*/HZ * USB_CTRL_GET_TIMEOUT);
if (ret < 0)
printk(DRIVER_NAME "[%d]: SET_INFRABUFFER_EMPTY: "
"error %d\n", ir->devnum, ret);
return 0;
} else if (ret < 0)
printk(DRIVER_NAME "[%d]: GET_INFRACODE: error %d\n",
ir->devnum, ret);
return -ENODATA;
}
static int8_t fetcher_handler(void *state, Message *msg)
{
switch (msg->type) {
case MSG_FETCHER_FRAGMENT:
{
fetcher_fragment_t *f;
f = (fetcher_fragment_t*)msg->data;
f->key = entohs( f->key );
f->frag_id = entohs(f->frag_id);
DEBUG_PID(KER_FETCHER_PID,"MSG_FETCHER_FRAGMENT:\n");
handle_overheard_fragment(msg);
if(fst == NULL) {
DEBUG_PID(KER_FETCHER_PID, "NO Request!!!\n");
return SOS_OK; //!< no request
}
//DEBUG_PID(KER_FETCHER_PID,"calling restart_request_timer()\n");
restart_request_timer();
fst->retx = 0;
//DEBUG_PID(KER_FETCHER_PID,"calling handle_data()\n");
return handle_data(msg);
}
case MSG_FETCHER_REQUEST:
{
fetcher_bitmap_t *bmap =
(fetcher_bitmap_t *) msg->data;
bmap->key = entohs( bmap->key );
//! received request from neighbors
DEBUG("handling request to %d from %d\n", msg->daddr, msg->saddr);
if(msg->daddr == ker_id()) {
return handle_request(msg);
}
if(fst == NULL) return SOS_OK; //!< no request
restart_request_timer();
fst->retx = 0;
return SOS_OK;
}
case MSG_TIMER_TIMEOUT:
{
MsgParam *params = (MsgParam*)(msg->data);
if(params->byte == FETCHER_REQUEST_TID) {
//DEBUG("request timeout\n");
if( no_mem_retry ) {
send_fetcher_done();
return SOS_OK;
}
handle_request_timeout();
} else if(params->byte == FETCHER_TRANSMIT_TID) {
//DEBUG("send fragment timeout\n");
if( send_state.num_msg_in_queue < FETCHER_MAX_MSG_IN_QUEUE ) {
send_fragment();
}
}
return SOS_OK;
}
case MSG_PKT_SENDDONE:
{
if( send_state.num_msg_in_queue > 0 ) {
send_state.num_msg_in_queue--;
}
return SOS_OK;
}
#ifdef SOS_HAS_EXFLASH
case MSG_EXFLASH_WRITEDONE:
{
ker_free(send_state.fragr);
send_state.fragr = NULL;
check_map_and_post();
return SOS_OK;
}
case MSG_EXFLASH_READDONE:
{
post_auto(KER_FETCHER_PID,
KER_FETCHER_PID,
MSG_FETCHER_FRAGMENT,
sizeof(fetcher_fragment_t),
send_state.frag,
SOS_MSG_RELEASE,
send_state.dest);
send_state.frag = NULL;
return SOS_OK;
}
#endif
case MSG_INIT:
{
send_state.map = NULL;
send_state.frag = NULL;
send_state.fragr = NULL;
send_state.num_msg_in_queue = 0;
ker_msg_change_rules(KER_FETCHER_PID, SOS_MSG_RULES_PROMISCUOUS);
ker_permanent_timer_init(&(send_state.timer), KER_FETCHER_PID, FETCHER_TRANSMIT_TID, TIMER_REPEAT);
ker_timer_init(KER_FETCHER_PID, FETCHER_REQUEST_TID, TIMER_ONE_SHOT);
return SOS_OK;
}
}
return -EINVAL;
}
static void send_udp_frags(int fd_raw, struct sockaddr *addr,
socklen_t alen, bool ipv6)
{
struct ip *iphdr = (struct ip *)ip_frame;
struct ip6_hdr *ip6hdr = (struct ip6_hdr *)ip_frame;
const bool ipv4 = !ipv6;
int res;
int offset;
int frag_len;
/* Send the UDP datagram using raw IP fragments: the 0th fragment
* has the UDP header; other fragments are pieces of udp_payload
* split in chunks of frag_len size.
*
* Odd fragments (1st, 3rd, 5th, etc.) are sent out first, then
* even fragments (0th, 2nd, etc.) are sent out.
*/
if (ipv6) {
struct ip6_frag *fraghdr = (struct ip6_frag *)(ip_frame + IP6_HLEN);
((struct sockaddr_in6 *)addr)->sin6_port = 0;
memset(ip6hdr, 0, sizeof(*ip6hdr));
ip6hdr->ip6_flow = htonl(6<<28); /* Version. */
ip6hdr->ip6_nxt = IPPROTO_FRAGMENT;
ip6hdr->ip6_hops = 255;
ip6hdr->ip6_src = addr6;
ip6hdr->ip6_dst = addr6;
fraghdr->ip6f_nxt = IPPROTO_UDP;
fraghdr->ip6f_reserved = 0;
fraghdr->ip6f_ident = htonl(ip_id++);
} else {
memset(iphdr, 0, sizeof(*iphdr));
iphdr->ip_hl = 5;
iphdr->ip_v = 4;
iphdr->ip_tos = 0;
iphdr->ip_id = htons(ip_id++);
iphdr->ip_ttl = 0x40;
iphdr->ip_p = IPPROTO_UDP;
iphdr->ip_src.s_addr = htonl(INADDR_LOOPBACK);
iphdr->ip_dst = addr4;
iphdr->ip_sum = 0;
}
/* Occasionally test in-order fragments. */
if (!cfg_overlap && (rand() % 100 < 15)) {
offset = 0;
while (offset < (UDP_HLEN + payload_len)) {
send_fragment(fd_raw, addr, alen, offset, ipv6);
offset += max_frag_len;
}
return;
}
/* Occasionally test IPv4 "runs" (see net/ipv4/ip_fragment.c) */
if (ipv4 && !cfg_overlap && (rand() % 100 < 20) &&
(payload_len > 9 * max_frag_len)) {
offset = 6 * max_frag_len;
while (offset < (UDP_HLEN + payload_len)) {
send_fragment(fd_raw, addr, alen, offset, ipv6);
offset += max_frag_len;
}
offset = 3 * max_frag_len;
while (offset < 6 * max_frag_len) {
send_fragment(fd_raw, addr, alen, offset, ipv6);
offset += max_frag_len;
}
offset = 0;
while (offset < 3 * max_frag_len) {
send_fragment(fd_raw, addr, alen, offset, ipv6);
offset += max_frag_len;
}
return;
}
/* Odd fragments. */
offset = max_frag_len;
while (offset < (UDP_HLEN + payload_len)) {
send_fragment(fd_raw, addr, alen, offset, ipv6);
/* IPv4 ignores duplicates, so randomly send a duplicate. */
if (ipv4 && (1 == rand() % 100))
send_fragment(fd_raw, addr, alen, offset, ipv6);
offset += 2 * max_frag_len;
}
if (cfg_overlap) {
/* Send an extra random fragment. */
if (ipv6) {
struct ip6_frag *fraghdr = (struct ip6_frag *)(ip_frame + IP6_HLEN);
/* sendto() returns EINVAL if offset + frag_len is too small. */
offset = rand() % (UDP_HLEN + payload_len - 1);
frag_len = max_frag_len + rand() % 256;
/* In IPv6 if !!(frag_len % 8), the fragment is dropped. */
frag_len &= ~0x7;
fraghdr->ip6f_offlg = htons(offset / 8 | IP6_MF);
ip6hdr->ip6_plen = htons(frag_len);
frag_len += IP6_HLEN;
} else {
/* In IPv4, duplicates and some fragments completely inside
* previously sent fragments are dropped/ignored. So
* random offset and frag_len can result in a dropped
* fragment instead of a dropped queue/packet. So we
* hard-code offset and frag_len.
*
* See ade446403bfb ("net: ipv4: do not handle duplicate
* fragments as overlapping").
*/
if (max_frag_len * 4 < payload_len || max_frag_len < 16) {
/* not enough payload to play with random offset and frag_len. */
offset = 8;
frag_len = IP4_HLEN + UDP_HLEN + max_frag_len;
} else {
offset = rand() % (payload_len / 2);
frag_len = 2 * max_frag_len + 1 + rand() % 256;
}
iphdr->ip_off = htons(offset / 8 | IP4_MF);
iphdr->ip_len = htons(frag_len);
}
res = sendto(fd_raw, ip_frame, frag_len, 0, addr, alen);
if (res < 0)
error(1, errno, "sendto overlap: %d", frag_len);
if (res != frag_len)
error(1, 0, "sendto overlap: %d vs %d", (int)res, frag_len);
frag_counter++;
}
/* Event fragments. */
offset = 0;
while (offset < (UDP_HLEN + payload_len)) {
send_fragment(fd_raw, addr, alen, offset, ipv6);
/* IPv4 ignores duplicates, so randomly send a duplicate. */
if (ipv4 && (1 == rand() % 100))
send_fragment(fd_raw, addr, alen, offset, ipv6);
offset += 2 * max_frag_len;
}
}
// For documentation about variables used here, see struct History in cdriver.h
int next_move(int32_t start_time) { // {{{
// Clean up state before starting the move.
if (current_fragment_pos > 0) {
//debug("sending because new move is started");
send_fragment();
}
settings.probing = false;
settings.factor = 0;
int num_cbs = 0;
run_file_fill_queue();
if (settings.queue_start == settings.queue_end && !settings.queue_full) {
//debug("no next move");
computing_move = false;
return num_cbs;
}
mdebug("Next move; queue start = %d, end = %d", settings.queue_start, settings.queue_end);
// Set everything up for running queue[settings.queue_start].
int q = settings.queue_start;
int n = (settings.queue_start + 1) % QUEUE_LENGTH;
settings.single = queue[q].single;
if (queue[q].cb)
++num_cbs;
// Make sure machine state is good. {{{
// If the source is unknown, determine it from current_pos.
for (int s = 0; s < NUM_SPACES; ++s) {
if (s == 1) {
// Extruders are handled later.
continue;
}
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (std::isnan(sp.axis[a]->settings.source)) {
if (!std::isnan(sp.axis[a]->settings.current)) {
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
continue;
}
reset_pos(&sp);
for (int aa = 0; aa < sp.num_axes; ++aa)
sp.axis[aa]->settings.current = sp.axis[aa]->settings.source;
break;
}
else
mdebug("non-nan: %d %d %f %f", s, a, sp.axis[a]->settings.source, sp.motor[a]->settings.current_pos);
}
// Followers don't have good motor data, so initialize them here.
for (int a = 0; a < sp.num_axes; ++a) {
if (s == 2)
sp.motor[a]->settings.current_pos = sp.axis[a]->settings.source;
}
}
// }}}
change0(q);
// Fill unspecified coordinates with previous values. {{{
Space &sp0 = spaces[0];
for (int a = 0; a < sp0.num_axes; ++a) {
if (n != settings.queue_end) {
// If only one of them is set, set the other one as well to make the rounded corner work.
if (!std::isnan(queue[q].X[a]) && std::isnan(queue[n].X[a])) {
queue[n].X[a] = queue[q].X[a];
mdebug("filling next %d with %f", a, queue[n].X[a]);
}
if (std::isnan(queue[q].X[a]) && !std::isnan(queue[n].X[a])) {
queue[q].X[a] = sp0.axis[a]->settings.source;
mdebug("filling %d with %f", a, queue[q].X[a]);
}
}
if ((!std::isnan(queue[q].X[a]) || (n != settings.queue_end && !std::isnan(queue[n].X[a]))) && std::isnan(sp0.axis[a]->settings.source)) {
debug("Motor position for axis %d is not known, so move cannot take place; aborting move and removing it from the queue: q1=%f q2=%f src=%f", a, queue[q].X[a], queue[n].X[a], sp0.axis[a]->settings.source);
// This possibly removes one move too many, but it shouldn't happen anyway.
settings.queue_start = n;
settings.queue_full = false;
abort_move(current_fragment_pos);
return num_cbs;
}
}
// }}}
// Reset time. {{{
if (computing_move) {
settings.hwtime -= start_time;
//debug("time -= %d, now %d", start_time, settings.hwtime);
}
else {
settings.hwtime = 0;
}
// }}}
if (!computing_move) { // Set up source if this is a new move. {{{
mdebug("starting new move");
//debug("current %d running %d", current_fragment, running_fragment);
for (int s = 0; s < NUM_SPACES; ++s) {
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (!std::isnan(sp.axis[a]->settings.current)) {
mdebug("setting source for %d %d to current %f (was %f)", s, a, sp.axis[a]->settings.current, sp.axis[a]->settings.source);
sp.axis[a]->settings.source = sp.axis[a]->settings.current;
}
}
}
} // }}}
settings.v0 = queue[q].v0 * feedrate;
settings.v1 = queue[q].v1 * feedrate;
double dot = 0, norma = 0, normb = 0, normab = 0;
for (int i = 0; i < 3; ++i) {
bool use = i < spaces[0].num_axes;
double p = (use ? spaces[0].axis[i]->settings.source : 0);
settings.P[i] = (use ? (std::isnan(queue[q].X[i]) ? p : (queue[q].X[i] + (i == 2 ? zoffset : 0) + p) / 2) : 0);
settings.A[i] = settings.P[i] - p;
settings.B[i] = (use ? queue[q].B[i] : 0);
double ab = settings.A[i] + settings.B[i];
dot += settings.A[i] * ab;
norma += settings.A[i] * settings.A[i];
normb += settings.B[i] * settings.B[i];
normab += ab * ab;
}
norma = sqrt(norma);
normb = sqrt(normb);
normab = sqrt(normab);
settings.alpha_max = acos(dot / (norma * normab));
if (std::isnan(settings.alpha_max))
settings.alpha_max = 0;
settings.dist = (normb > 1e-5 ? norma * (normab / normb) * settings.alpha_max : norma) * 2;
if (std::isnan(settings.dist) || std::fabs(settings.dist) < 1e-10) {
//debug("no space dist, using other system. dist=%f a=%f ab=%f b=%f", settings.dist, norma, normab, normb);
if (queue[q].tool >= 0 && queue[q].tool < spaces[1].num_axes)
settings.dist = std::fabs(queue[q].e - spaces[1].axis[queue[q].tool]->settings.source);
else if (queue[q].single && queue[q].tool < 0 && ~queue[q].tool < spaces[2].num_axes)
settings.dist = std::fabs(queue[q].e - spaces[2].axis[~queue[q].tool]->settings.source);
}
double dt = settings.dist / ((settings.v0 + settings.v1) / 2);
settings.end_time = (std::isnan(dt) ? 0 : 1e6 * dt);
if (queue[q].tool >= 0 && queue[q].tool < spaces[1].num_axes && !std::isnan(queue[q].e)) {
spaces[1].axis[queue[q].tool]->settings.endpos = queue[q].e;
mdebug("move extruder to %f", queue[q].e);
}
else if (queue[q].single && queue[q].tool < 0 && ~queue[q].tool < spaces[2].num_axes && !std::isnan(queue[q].e)) {
spaces[2].axis[~queue[q].tool]->settings.endpos = queue[q].e;
mdebug("move follower to %f, current=%f source=%f current_pos=%f", queue[q].e, spaces[2].axis[~queue[q].tool]->settings.current, spaces[2].axis[~queue[q].tool]->settings.source, spaces[2].motor[~queue[q].tool]->settings.current_pos);
}
auto last_hwtime_step = settings.hwtime_step;
if (queue[q].pattern_size > 0) {
memcpy(settings.pattern, queue[q].pattern, queue[q].pattern_size);
settings.hwtime_step = dt * 1e6 / (queue[q].pattern_size * 8);
if (settings.hwtime_step < min_hwtime_step)
settings.hwtime_step = min_hwtime_step;
}
settings.pattern_size = queue[q].pattern_size;
if (settings.hwtime_step != last_hwtime_step)
arch_globals_change();
/*
if (spaces[0].num_axes > 2) {
debug("move prepared, from=(%f,%f,%f) Q=(%f,%f,%f) P=(%f,%f,%f), A=(%f,%f,%f), B=(%f,%f,%f), max alpha=%f, dist=%f, e=%f, v0=%f, v1=%f, end time=%f, single=%d, UVW=(%f,%f,%f)", spaces[0].axis[0]->settings.source, spaces[0].axis[1]->settings.source, spaces[0].axis[2]->settings.source, queue[q].X[0], queue[q].X[1], queue[q].X[2], settings.P[0], settings.P[1], settings.P[2], settings.A[0], settings.A[1], settings.A[2], settings.B[0], settings.B[1], settings.B[2], settings.alpha_max, settings.dist, queue[q].e, settings.v0, settings.v1, settings.end_time / 1e6, queue[q].single, spaces[0].motor[0]->settings.current_pos, spaces[0].motor[1]->settings.current_pos, spaces[0].motor[2]->settings.current_pos);
}
else if (spaces[0].num_axes > 1) {
debug("move prepared, from=(%f,%f) Q=(%f,%f,%f) P=(%f,%f,%f), A=(%f,%f,%f), B=(%f,%f,%f), max alpha=%f, dist=%f, e=%f, v0=%f, v1=%f, end time=%f, single=%d, UV=(%f,%f)", spaces[0].axis[0]->settings.source, spaces[0].axis[1]->settings.source, queue[q].X[0], queue[q].X[1], queue[q].X[2], settings.P[0], settings.P[1], settings.P[2], settings.A[0], settings.A[1], settings.A[2], settings.B[0], settings.B[1], settings.B[2], settings.alpha_max, settings.dist, queue[q].e, settings.v0, settings.v1, settings.end_time / 1e6, queue[q].single, spaces[0].motor[0]->settings.current_pos, spaces[0].motor[1]->settings.current_pos);
}
// */
//debug("times end %d current %d dist %f v0 %f v1 %f", settings.end_time, settings.hwtime, settings.dist, settings.v0, settings.v1);
settings.queue_start = n;
first_fragment = current_fragment; // Do this every time, because otherwise the queue must be regenerated. TODO: send partial fragment to make sure this hack actually works, or fix it properly.
if (!computing_move)
store_settings();
computing_move = true;
return num_cbs;
} // }}}
static void do_steps() { // {{{
// Do the steps to arrive at the correct position. Update axis and motor current positions.
// Set new current position.
for (int s = 0; s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
for (int a = 0; a < sp.num_axes; ++a) {
if (!std::isnan(sp.axis[a]->settings.target))
sp.axis[a]->settings.current = sp.axis[a]->settings.target;
}
}
// Move the motors.
//debug("start move");
for (int s = 0; s < NUM_SPACES; ++s) {
if (!settings.single && s == 2)
continue;
Space &sp = spaces[s];
for (int m = 0; m < sp.num_motors; ++m) {
Motor &mtr = *sp.motor[m];
double target = mtr.settings.target_pos;
if (std::isnan(target))
continue;
cpdebug(s, m, "ccp3 stopping %d target %f frag %d", stopping, target, current_fragment);
double rounded_cp = arch_round_pos(s, m, mtr.settings.current_pos);
double rounded_new_cp = arch_round_pos(s, m, target);
if (rounded_cp != rounded_new_cp) {
if (!mtr.active) {
mtr.active = true;
num_active_motors += 1;
//debug("activating motor %d %d", s, m);
}
int diff = round((rounded_new_cp - rounded_cp) * mtr.steps_per_unit);
if (diff > 0x7f) {
debug("Error: trying to send more than 127 steps: %d", diff);
int adjust = diff - 0x7f;
settings.hwtime -= settings.hwtime_step * (adjust / diff);
diff = 0x7f;
target -= adjust;
mtr.settings.target_pos = target;
}
if (diff < -0x80) {
debug("Error: trying to send more than 128 steps: %d", -diff);
int adjust = diff + 0x80;
settings.hwtime -= settings.hwtime_step * (adjust / diff);
diff = -0x80;
target -= adjust;
mtr.settings.target_pos = target;
}
//debug("sending %d %d steps %d", s, m, diff);
DATA_SET(s, m, diff);
}
//debug("new cp: %d %d %f %d", s, m, target, current_fragment_pos);
if (!settings.single) {
for (int mm = 0; mm < spaces[2].num_motors; ++mm) {
int fm = space_types[spaces[2].type].follow(&spaces[2], mm);
if (fm < 0)
continue;
int fs = fm >> 8;
fm &= 0x7f;
if (fs != s || fm != m || (fs == 2 && fm >= mm))
continue;
//debug("follow %d %d %d %d %d %f %f", s, m, fs, fm, mm, target, mtr.settings.current_pos);
spaces[2].motor[mm]->settings.current_pos += target - mtr.settings.current_pos;
}
}
mtr.settings.current_pos = target;
cpdebug(s, m, "cp three %f", target);
mtr.settings.last_v = mtr.settings.target_v;
}
}
int pattern_pos = (settings.hwtime - settings.hwtime_step / 2) / settings.hwtime_step;
//debug("time %d step %d pattern pos %d size %d", settings.hwtime, settings.hwtime_step, pattern_pos, settings.pattern_size);
if (pattern_pos < settings.pattern_size * 8) {
if (!pattern.active) {
pattern.active = true;
num_active_motors += 1;
//debug("activating pattern");
}
//debug("set pattern for %d at %d to %d", current_fragment, current_fragment_pos, settings.pattern[pattern_pos]);
PATTERN_SET(settings.pattern[pattern_pos]);
}
current_fragment_pos += 1;
if (current_fragment_pos >= SAMPLES_PER_FRAGMENT) {
//debug("sending because fragment full (normal) %d %d %d", computing_move, current_fragment_pos, BYTES_PER_FRAGMENT);
send_fragment();
}
} // }}}
