__checkReturn PackedIP PackIP(
__in char const ip1,
__in char const ip2,
__in char const ip3,
__in char const ip4 )
{
return MakeLong(
MakeWord( ip1, ip2 ),
MakeWord( ip3, ip4 ) );
}
// Caller is resonsible for the received_data buffer's appropriate size.
// Returns the total numberSet bytes received for packet.
// If receive timed out, value of num_bytes_received is undefined.
bool RXPacket(uint8_t* received_data, int packet_length, int* num_bytes_received) {
dxl_hal_set_timeout(packet_length);
// Repeatedly attempt to read until either we have all of the bytes
// requested or the timeout expires.
// TODO replace this logic with something that isn't so CPU intensive.
// like interrupt driven...
int bytes_read = 0;
while (bytes_read < packet_length) {
if (dxl_hal_timeout() == 1) {
return false;
}
assert(packet_length < MAX_PACKET_BYTES);
bytes_read += dxl_hal_rx(&(received_data[bytes_read]), packet_length);
}
// PrintPacket(data, num_bytes);
// Now attempt to match the header to the data received.
int header_match_count = 0;
int buffer_index = 0;
while (header_match_count < HEADER_SIZE) {
if (buffer_index >= bytes_read) {
return false;
}
if (received_data[buffer_index++] == HEADER[header_match_count]) {
header_match_count++;
} else {
header_match_count = 0;
}
}
// Now, the header is matched
// Move bytes down to the beginning of the data buffer.
int packet_start_index = buffer_index - HEADER_SIZE;
bytes_read -= packet_start_index;
memmove(received_data, &(received_data[packet_start_index]), bytes_read);
// Now read the remaining bytes if necessary.
while (bytes_read < packet_length) {
assert((packet_length - bytes_read) < MAX_PACKET_BYTES);
bytes_read += dxl_hal_rx(&(received_data[bytes_read]), packet_length - bytes_read);
if (dxl_hal_timeout() == 1) {
return false;
}
}
uint16_t expected_crc = UpdateCRC(0, received_data, bytes_read - CHECKSUM_SIZE);
uint16_t received_crc = MakeWord(received_data[bytes_read - 2], received_data[bytes_read - 1]);
if (expected_crc != received_crc) {
// CRC check failed.
return false;
}
*num_bytes_received = bytes_read;
return true;
}
void FinalPacketConsistencyCheck(uint8_t* data, uint16_t num_bytes) {
assert(num_bytes >= BASE_PACKET_SIZE);
// Check header
int i;
for (i = 0; i < HEADER_SIZE; i++) {
assert(data[i] == HEADER[i]);
}
// Check packet length setting
assert(MakeWord(data[LENGTH_L_ADDR], data[LENGTH_H_ADDR]) == num_bytes - PREFIX_SIZE + 1);
// Check Instruction
assert(data[INST_ADDR] > 0 && data[INST_ADDR] <= 11);
// CRC Check
uint16_t expected_crc = UpdateCRC(0, data, num_bytes - CHECKSUM_SIZE);
assert(MakeWord(data[num_bytes - 2], data[num_bytes - 1]) == expected_crc);
}
/* ReadJointAngle - sends out a single read to a joint motor
* Return - joint angle in ticks of prompted joint
* - for failed read, return -99999
*/
int DarwinController::ReadJointAngle(int id){
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
unsigned char txpacketread[] = {0, 0, id, 0x04, 0x02, 0x24, 0x02, 0};
FinishPacket(txpacketread);
int result = ReadWrite(txpacketread, rxpacket);
if(result == 0){
printf("Failed read! \n");
return -9999999;
} else {
return MakeWord((int)rxpacket[PARAMETER], (int)rxpacket[PARAMETER + 1]);
}
}
bool ProcessFrame(const string& frame, Eigen::VectorXd& motorAngle)
{
std::vector<int> tmpPos;
tmpPos.resize(NUM_MOTORS);
for (int i = 0; i < NUM_MOTORS; ++i)
{
//int mPos = MakeWord(Decipher(frame[NUM_BYTES_PER_MOTOR * i]), Decipher(frame[NUM_BYTES_PER_MOTOR * i + 1]), Decipher(frame[NUM_BYTES_PER_MOTOR * i + 2]), Decipher(frame[NUM_BYTES_PER_MOTOR * i + 3]));
int mPos = MakeWord(frame[NUM_BYTES_PER_MOTOR * i], frame[NUM_BYTES_PER_MOTOR * i + 1]);
tmpPos[i] = mPos;
if (mPos < 0 || mPos >= 1024)
{
std::cout << "Warning! Joint angle abnormal." << std::endl;
return false;
}
}
for (int i = 0; i < NUM_MOTORS; ++i)
{
motorAngle[i] = tmpPos[i];
}
return true;
}
/* ReadWord - Read word from the specified id and address
* Return - length of successfully read packet. Return bad number if fails.
*/
int DarwinController::ReadWord(int id, int address, int *word){
unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
txpacket[ID] = (unsigned char)id;
txpacket[INSTRUCTION] = READ;
txpacket[PARAMETER] = (unsigned char)address;
txpacket[PARAMETER+1] = 2;
txpacket[LENGTH] = 4;
int txlength = txpacket[LENGTH] + 4;
FinishPacket(txpacket); // don't know why i had this commented out.
int result = ReadWrite(txpacket, rxpacket);
if(result == txlength){
*word = MakeWord((int)rxpacket[PARAMETER], (int)rxpacket[PARAMETER + 1]);
}
return result;
}
int AttachGalley(OBJECT hd, OBJECT *inners, OBJECT *suspend_pt)
{ OBJECT hd_index; /* the index of hd in the enclosing galley */
OBJECT hd_inners; /* inner galleys of hd, if unsized */
OBJECT dest; /* the target @Galley hd empties into */
OBJECT dest_index; /* the index of dest */
OBJECT target; /* the target indefinite containing dest */
OBJECT target_index; /* the index of target */
OBJECT target_galley; /* the body of target, made into a galley */
OBJECT tg_inners; /* inner galleys of target_galley */
BOOLEAN need_precedes = FALSE;/* true if destination lies before galley */
OBJECT recs; /* list of recursive definite objects */
OBJECT link, y = nilobj; /* for scanning through the components of hd */
CONSTRAINT c; /* temporary variable holding a constraint */
OBJECT env, n1, tmp, zlink, z, sym; /* placeholders and temporaries */
BOOLEAN was_sized; /* true if sized(hd) initially */
int dim; /* the galley direction */
FULL_LENGTH perp_back, perp_fwd;
OBJECT why, junk;
debug2(DGA, D, "[ AttachGalley(Galley %s into %s)",
SymName(actual(hd)), SymName(whereto(hd)));
ifdebug(DGA, DD, DebugGalley(hd, nilobj, 4));
assert( Up(hd) != hd, "AttachGalley: no index!" );
Parent(hd_index, Up(hd));
assert( type(hd_index) == UNATTACHED, "AttachGalley: not UNATTACHED!" );
hd_inners = tg_inners = nilobj;
was_sized = sized(hd);
dim = gall_dir(hd);
for(;;)
{
/*************************************************************************/
/* */
/* Search for a destination for hd. If hd is unsized, search for */
/* inner galleys preceding it first of all, then for receptive objects */
/* following it, possibly in inner galleys. If no luck, exit. */
/* If hd is sized, search only for receptive objects in the current */
/* galley below the current spot, and fail if cannot find any. */
/* */
/*************************************************************************/
sym = whereto(hd);
if( sized(hd) )
{
/* sized galley case: search on from current spot */
target_index = SearchGalley(Up(hd_index), sym, TRUE, FALSE, TRUE, TRUE);
if( target_index == nilobj )
{
/* search failed to find any new target, so kill the galley */
for( link = Down(hd); link != hd; link = NextDown(link) )
{ Child(y, link);
if( type(y) == SPLIT ) Child(y, DownDim(y, dim));
if( is_definite(type(y)) ) break;
}
if( link != hd )
Error(19, 1, "galley %s deleted from here (no target)",
WARN, &fpos(y), SymName(actual(hd)));
if( hd_inners != nilobj ) DisposeObject(hd_inners), hd_inners=nilobj;
if( tg_inners != nilobj ) DisposeObject(tg_inners), tg_inners=nilobj;
KillGalley(hd, FALSE);
*inners = nilobj;
debug0(DGA, D, "] AttachGalley returning ATTACH_KILLED");
return ATTACH_KILLED;
}
else if( actual(actual(target_index)) == InputSym )
{
/* search found input object, so suspend on that */
DeleteNode(hd_index);
Link(target_index, hd);
*inners = nilobj;
debug0(DGA, D, "] AttachGalley returning ATTACH_INPUT");
return ATTACH_INPUT;
}
}
else /* unsized galley, either backwards or normal */
{
if( foll_or_prec(hd) == GALL_PREC )
{ target_index= SearchGalley(Up(hd_index), sym, FALSE, TRUE,TRUE,FALSE);
need_precedes = FALSE;
}
else
{ target_index = SearchGalley(Up(hd_index), sym, FALSE,TRUE,FALSE,FALSE);
need_precedes = (target_index != nilobj);
if( target_index == nilobj )
target_index = SearchGalley(Up(hd_index), sym, TRUE,TRUE,TRUE,FALSE);
}
/* if no luck, exit without error */
if( target_index == nilobj )
{ *inners = nilobj;
debug0(DGA, D, "] AttachGalley returning ATTACH_NOTARGET");
return ATTACH_NOTARGET;
}
}
assert( type(target_index) == RECEPTIVE, "AttachGalley: target_index!" );
target = actual(target_index);
assert( type(target) == CLOSURE, "AttachGalley: target!" );
/* set target_galley to the expanded value of target */
debug1(DYY, D, "[ EnterErrorBlock(FALSE) (expanding target %s)",
SymName(actual(target)));
EnterErrorBlock(FALSE);
New(target_galley, HEAD);
force_gall(target_galley) = FALSE;
enclose_obj(target_galley) = limiter(target_galley) = nilobj;
ClearHeaders(target_galley);
opt_components(target_galley) = opt_constraints(target_galley) = nilobj;
gall_dir(target_galley) = external_hor(target) ? COLM : ROWM;
FposCopy(fpos(target_galley), fpos(target));
actual(target_galley) = actual(target);
whereto(target_galley) = ready_galls(target_galley) = nilobj;
foll_or_prec(target_galley) = GALL_FOLL;
must_expand(target_galley) = FALSE;
sized(target_galley) = FALSE;
/* get perpendicular constraint (none if horizontal galley) */
if( dim == ROWM )
{
Constrained(target, &c, 1-dim, &junk);
if( !constrained(c) )
Error(19, 2, "receptive symbol %s has unconstrained width",
FATAL, &fpos(target), SymName(actual(target)));
debug2(DSC, DD, "Constrained( %s, 1-dim ) = %s",
EchoObject(target), EchoConstraint(&c));
if( !FitsConstraint(0, 0, c) )
{ debug0(DGA, D, " reject: target_galley horizontal constraint is -1");
y = nilobj;
goto REJECT;
}
}
else /* actually unused */
SetConstraint(c, MAX_FULL_LENGTH, MAX_FULL_LENGTH, MAX_FULL_LENGTH);
debug1(DGA, DDD, " expanding %s", EchoObject(target));
tmp = CopyObject(target, no_fpos);
Link(target_galley, tmp);
env = DetachEnv(tmp);
debug4(DGM, D, " external_ver(%s) = %s, external_hor(%s) = %s",
SymName(actual(target)), bool(external_ver(target)),
SymName(actual(target)), bool(external_hor(target)));
SizeGalley(target_galley, env,
external_ver(target) || external_hor(target),
threaded(target), non_blocking(target_index),
trigger_externs(target_index), &save_style(target),
&c, whereto(hd), &dest_index, &recs, &tg_inners,
enclose_obj(hd) != nilobj ? CopyObject(enclose_obj(hd), no_fpos):nilobj);
debug1(DGA, DD, " SizeGalley tg_inners: %s", DebugInnersNames(tg_inners));
if( recs != nilobj ) ExpandRecursives(recs);
dest = actual(dest_index);
if( underline(dest) == UNDER_UNDEF ) underline(dest) = UNDER_OFF;
/* verify that hd satisfies any horizontal constraint on dest */
if( dim == ROWM )
{
debug1(DGA, DDD, " checking hor fit of hd in %s",SymName(actual(dest)));
Constrained(dest, &c, 1-dim, &junk);
debug3(DSC, DD, "Constrained( %s, %s ) = %s",
EchoObject(dest), dimen(1-dim), EchoConstraint(&c));
assert( constrained(c), "AttachGalley: dest unconstrained!" );
if( !FitsConstraint(0, 0, c) )
{ debug0(DGA, D, " reject: hd horizontal constraint is -1");
y = nilobj;
goto REJECT;
}
}
/* manifest and size the galley if not done yet */
if( !sized(hd) )
{
debug2(DYY, D, "[ EnterErrorBlock(TRUE) (sizing galley %s into %s)",
SymName(actual(hd)), SymName(whereto(hd)));
EnterErrorBlock(TRUE);
n1 = nilobj;
Child(y, Down(hd));
env = DetachEnv(y);
/*** threaded() only defined in ROWM case
SizeGalley(hd, env, TRUE, threaded(dest), non_blocking(target_index),
TRUE, &save_style(dest), &c, nilobj, &n1, &recs, &hd_inners);
*** */
SizeGalley(hd, env, TRUE, dim == ROWM ? threaded(dest) : FALSE,
non_blocking(target_index), TRUE, &save_style(dest), &c, nilobj,
&n1, &recs, &hd_inners, nilobj);
debug1(DGA,DD," SizeGalley hd_inners: %s", DebugInnersNames(hd_inners));
if( recs != nilobj ) ExpandRecursives(recs);
if( need_precedes ) /* need an ordering constraint */
{ OBJECT index1, index2;
New(index1, PRECEDES);
New(index2, FOLLOWS);
blocked(index2) = FALSE;
tmp = MakeWord(WORD, STR_EMPTY, no_fpos);
Link(index1, tmp); Link(index2, tmp);
Link(Up(hd_index), index1);
Link(Down(hd), index2);
debug0(DGA, D, " inserting PRECEDES and FOLLOWS");
}
LeaveErrorBlock(TRUE);
debug0(DYY, D, "] LeaveErrorBlock(TRUE) (finished sizing galley)");
}
if( dim == ROWM )
{ if( !FitsConstraint(back(hd, 1-dim), fwd(hd, 1-dim), c) )
{ debug3(DGA, D, " reject: hd %s,%s does not fit target_galley %s",
EchoLength(back(hd, 1-dim)), EchoLength(fwd(hd, 1-dim)),
EchoConstraint(&c));
Error(19, 3, "too little horizontal space for galley %s at %s",
WARN, &fpos(hd), SymName(actual(hd)), SymName(actual(dest)));
goto REJECT;
}
}
/* check status of first component of hd */
debug0(DGA, DDD, " now ready to attach; hd =");
ifdebug(DGA, DDD, DebugObject(hd));
for( link = Down(hd); link != hd; link = NextDown(link) )
{
Child(y, link);
debug1(DGA, DDD, " examining %s", EchoIndex(y));
if( type(y) == SPLIT ) Child(y, DownDim(y, dim));
switch( type(y) )
{
case EXPAND_IND:
case SCALE_IND:
case COVER_IND:
case GALL_PREC:
case GALL_FOLL:
case GALL_FOLL_OR_PREC:
case GALL_TARG:
case CROSS_PREC:
case CROSS_FOLL:
case CROSS_FOLL_OR_PREC:
case CROSS_TARG:
case PAGE_LABEL_IND:
break;
case PRECEDES:
case UNATTACHED:
if( was_sized )
{ /* SizeGalley was not called, so hd_inners was not set by it */
if( hd_inners == nilobj ) New(hd_inners, ACAT);
Link(hd_inners, y);
}
break;
case RECEPTIVE:
goto SUSPEND;
case RECEIVING:
goto SUSPEND;
case FOLLOWS:
Child(tmp, Down(y));
if( Up(tmp) == LastUp(tmp) )
{ link = pred(link, CHILD);
debug0(DGA, DD, " disposing FOLLOWS");
DisposeChild(NextDown(link));
break;
}
Parent(tmp, Up(tmp));
assert(type(tmp) == PRECEDES, "Attach: PRECEDES!");
switch( CheckComponentOrder(tmp, target_index) )
{
case CLEAR: DeleteNode(tmp);
link = pred(link, CHILD);
DisposeChild(NextDown(link));
break;
case PROMOTE: break;
case BLOCK: debug0(DGA, DD, "CheckContraint: BLOCK");
goto SUSPEND;
case CLOSE: debug0(DGA, D, " reject: CheckContraint");
goto REJECT;
}
break;
case GAP_OBJ:
underline(y) = underline(dest);
if( !join(gap(y)) ) seen_nojoin(hd) = TRUE;
break;
case BEGIN_HEADER:
case END_HEADER:
case SET_HEADER:
case CLEAR_HEADER:
/* do nothing until actually promoted out of here */
underline(y) = underline(dest);
break;
case CLOSURE:
case CROSS:
case FORCE_CROSS:
case NULL_CLOS:
case PAGE_LABEL:
underline(y) = underline(dest);
break;
case WORD:
case QWORD:
case ONE_COL:
case ONE_ROW:
case WIDE:
case HIGH:
case HSHIFT:
case VSHIFT:
case HMIRROR:
case VMIRROR:
case HSCALE:
case VSCALE:
case HCOVER:
case VCOVER:
case HCONTRACT:
case VCONTRACT:
case HLIMITED:
case VLIMITED:
case HEXPAND:
case VEXPAND:
case START_HVSPAN:
case START_HSPAN:
case START_VSPAN:
case HSPAN:
case VSPAN:
case ROTATE:
case BACKGROUND:
case SCALE:
case KERN_SHRINK:
case INCGRAPHIC:
case SINCGRAPHIC:
case PLAIN_GRAPHIC:
case GRAPHIC:
case LINK_SOURCE:
case LINK_DEST:
case LINK_DEST_NULL:
case LINK_URL:
case ACAT:
case HCAT:
case VCAT:
case ROW_THR:
case COL_THR:
underline(y) = underline(dest);
if( dim == ROWM )
{
/* make sure y is not joined to a target below (vertical only) */
for( zlink = NextDown(link); zlink != hd; zlink = NextDown(zlink) )
{ Child(z, zlink);
switch( type(z) )
{
case RECEPTIVE:
if( non_blocking(z) )
{ zlink = PrevDown(zlink);
DeleteNode(z);
}
else
{ y = z;
goto SUSPEND;
}
break;
case RECEIVING:
if( non_blocking(z) )
{ zlink = PrevDown(zlink);
while( Down(z) != z )
{ Child(tmp, Down(y));
if( opt_components(tmp) != nilobj )
{ DisposeObject(opt_components(tmp));
opt_components(tmp) = nilobj;
debug3(DOG, D, "AttachGalley(%s) de-optimizing %s %s",
SymName(actual(hd)), SymName(actual(tmp)), "(join)");
}
DetachGalley(tmp);
KillGalley(tmp, FALSE);
}
DeleteNode(z);
}
else
{ y = z;
goto SUSPEND;
}
break;
case GAP_OBJ:
if( !join(gap(z)) ) zlink = PrevDown(hd);
break;
default: break;
}
}
/* if HCAT, try vertical hyphenation (vertical galleys only) */
if( type(y) == HCAT ) VerticalHyphenate(y);
}
/* check availability of parallel space for the first component */
why = nilobj;
Constrained(dest, &c, dim, &why);
debug3(DGF, DD, " dest parallel Constrained(%s, %s) = %s",
EchoObject(dest), dimen(dim), EchoConstraint(&c));
if( !FitsConstraint(back(y, dim), fwd(y, dim), c) )
{ BOOLEAN scaled;
/* if forcing galley doesn't fit, try scaling first component */
scaled = FALSE;
if( force_gall(hd) && size(y, dim) > 0 )
{ int scale_factor;
scale_factor = ScaleToConstraint(back(y,dim), fwd(y,dim), &c);
if( scale_factor > 0.5 * SF )
{ char num1[20], num2[20];
sprintf(num1, "%.1fc", (float) size(y, dim) / CM);
sprintf(num2, "%.1fc", (float) bfc(c) / CM);
if( dim == ROWM )
Error(19, 4, "%s object too high for %s space; %s inserted",
WARN, &fpos(y), num1, num2, KW_SCALE);
else
Error(19, 5, "%s object too wide for %s space; %s inserted",
WARN, &fpos(y), num1, num2, KW_SCALE);
y = InterposeScale(y, scale_factor, dim);
scaled = TRUE;
}
}
/* otherwise we must reject, and warn the user */
if( !scaled )
{ char num1[20], num2[20];
debug3(DGA, D, " reject: vsize %s,%s in %s; y=",
EchoLength(back(y, dim)), EchoLength(fwd(y, dim)),
EchoConstraint(&c));
ifdebug(DGA, D, DebugObject(y));
if( size(y, dim) > 0 )
{ sprintf(num1, "%.1fc", (float) size(y, dim) / CM);
sprintf(num2, "%.1fc", (float) bfc(c) / CM);
if( dim == ROWM )
Error(19, 12, "%s object too high for %s space; will try elsewhere",
WARN, &fpos(y), num1, num2);
else
Error(19, 13, "%s object too wide for %s space; will try elsewhere",
WARN, &fpos(y), num1, num2);
}
goto REJECT;
}
}
/* check availability of perpendicular space for first component */
if( dim == ROWM )
{ perp_back = back(hd, 1-dim); perp_fwd = fwd(hd, 1-dim);
}
else
{ perp_back = back(y, 1-dim); perp_fwd = fwd(y, 1-dim);
}
Constrained(dest, &c, 1-dim, &junk);
debug3(DGF, DD, " dest perpendicular Constrained(%s, %s) = %s",
EchoObject(dest), dimen(1-dim), EchoConstraint(&c));
if( !FitsConstraint(perp_back, perp_fwd, c) )
{ BOOLEAN scaled;
/* if forcing galley doesn't fit, try scaling first component */
scaled = FALSE;
if( force_gall(hd) && perp_back + perp_fwd > 0 )
{ int scale_factor;
scale_factor = ScaleToConstraint(perp_back, perp_fwd, &c);
if( scale_factor > 0.5 * SF )
{ char num1[20], num2[20];
sprintf(num1, "%.1fc", (float) (perp_back + perp_fwd) / CM);
sprintf(num2, "%.1fc", (float) bfc(c) / CM);
if( 1-dim == ROWM )
Error(19, 6, "%s object too high for %s space; %s inserted",
WARN, &fpos(y), num1, num2, KW_SCALE);
else
Error(19, 7, "%s object too wide for %s space; %s inserted",
WARN, &fpos(y), num1, num2, KW_SCALE);
y = InterposeScale(y, scale_factor, 1-dim);
scaled = TRUE;
}
}
/* otherwise we must reject, and warn the user */
if( !scaled )
{
debug3(DGA, D, " reject: vsize %s,%s in %s; y=",
EchoLength(perp_back), EchoLength(perp_fwd),
EchoConstraint(&c));
ifdebug(DGA, D, DebugObject(y));
goto REJECT;
}
}
/* dest seems OK, so perform its size adjustments */
debug0(DSA, D, "calling AdjustSize from AttachGalley (a)");
AdjustSize(dest, back(y, dim), fwd(y, dim), dim);
debug0(DSA, D, "calling AdjustSize from AttachGalley (b)");
AdjustSize(dest, perp_back, perp_fwd, 1-dim);
/* now check parallel space for target_galley in target */
Constrained(target, &c, dim, &why);
debug3(DGF, DD, " target parallel Constrained(%s, %s) = %s",
EchoObject(target), dimen(dim), EchoConstraint(&c));
Child(z, LastDown(target_galley)); /* works in all cases? */
assert( !is_index(type(z)), "AttachGalley: is_index(z)!" );
assert( back(z, dim)>=0 && fwd(z, dim)>=0, "AttachGalley: z size!" );
if( !FitsConstraint(back(z, dim), fwd(z, dim), c) )
{ BOOLEAN scaled;
debug2(DGA, D, " why = %d %s", (int) why, EchoObject(why));
debug2(DGA, D, " limiter = %d %s", (int) limiter(hd),
EchoObject(limiter(hd)));
/* if forcing galley doesn't fit, try scaling z */
scaled = FALSE;
if( force_gall(hd) && size(z, dim) > 0 && limiter(hd) != why )
{ int scale_factor;
scale_factor = ScaleToConstraint(back(z,dim), fwd(z,dim), &c);
if( scale_factor > 0.5 * SF )
{ char num1[20], num2[20];
sprintf(num1, "%.1fc", (float) size(z, dim) / CM);
sprintf(num2, "%.1fc", (float) bfc(c) / CM);
if( dim == ROWM )
Error(19, 8, "%s object too high for %s space; %s inserted",
WARN, &fpos(y), num1, num2, KW_SCALE);
else
Error(19, 9, "%s object too wide for %s space; %s inserted",
WARN, &fpos(y), num1, num2, KW_SCALE);
z = InterposeWideOrHigh(z, dim);
z = InterposeScale(z, scale_factor, dim);
scaled = TRUE;
}
}
if( !scaled )
{ char num1[20], num2[20];
limiter(hd) = why;
debug3(DGA, D, " set limiter(%s) = %d %s", SymName(actual(hd)),
(int) limiter(hd), EchoObject(limiter(hd)));
debug3(DGA, D, " reject: size was %s,%s in %s; y =",
EchoLength(back(z, dim)), EchoLength(fwd(z, dim)),
EchoConstraint(&c));
ifdebug(DGA, D, DebugObject(y));
if( size(z, dim) > 0 )
{ sprintf(num1, "%.1fc", (float) size(z, dim) / CM);
sprintf(num2, "%.1fc", (float) bfc(c) / CM);
if( dim == ROWM )
Error(19, 14, "%s object too high for %s space; will try elsewhere",
WARN, &fpos(y), num1, num2);
else
Error(19, 15, "%s object too wide for %s space; will try elsewhere",
WARN, &fpos(y), num1, num2);
}
goto REJECT;
}
}
limiter(hd) = why;
debug3(DGA, D, " set limiter(%s) = %d %s", SymName(actual(hd)),
(int) limiter(hd), EchoObject(limiter(hd)));
/* now check perpendicular space for target_galley in target */
Constrained(target, &c, 1-dim, &junk);
debug3(DGF, DD, " target perpendicular Constrained(%s, %s) = %s",
EchoObject(target), dimen(1-dim), EchoConstraint(&c));
Child(z, LastDown(target_galley)); /* works in all cases? */
assert( !is_index(type(z)), "AttachGalley: is_index(z)!" );
assert( back(z, 1-dim)>=0 && fwd(z, 1-dim)>=0,
"AttachGalley: z size (perpendicular)!" );
if( !FitsConstraint(back(z, 1-dim), fwd(z, 1-dim), c) )
{ BOOLEAN scaled;
/* if forcing galley doesn't fit, try scaling z */
scaled = FALSE;
if( force_gall(hd) && size(z, 1-dim) > 0 )
{ int scale_factor;
scale_factor = ScaleToConstraint(back(z,1-dim), fwd(z,1-dim), &c);
if( scale_factor > 0.5 * SF )
{ char num1[20], num2[20];
sprintf(num1, "%.1fc", (float) size(z, 1-dim) / CM);
sprintf(num2, "%.1fc", (float) bfc(c) / CM);
if( 1-dim == ROWM )
Error(19, 10, "%s object too high for %s space; %s inserted",
WARN, &fpos(y), num1, num2, KW_SCALE);
else
Error(19, 11, "%s object too wide for %s space; %s inserted",
WARN, &fpos(y), num1, num2, KW_SCALE);
z = InterposeWideOrHigh(z, 1-dim);
z = InterposeScale(z, scale_factor, 1-dim);
scaled = TRUE;
}
}
if( !scaled )
{
debug3(DGA, D, " reject: size was %s,%s in %s; y =",
EchoLength(back(z, 1-dim)), EchoLength(fwd(z, 1-dim)),
EchoConstraint(&c));
ifdebug(DGA, D, DebugObject(y));
goto REJECT;
}
}
/* target seems OK, so adjust sizes and accept */
if( external_hor(target) )
{
/* don't adjust any sizes, none to adjust */
debug0(DSA, D, "not calling AdjustSize from AttachGalley (c)");
}
else if( external_ver(target) )
{
/* adjust perp size only, to galley size */
debug0(DSA, D, "calling AdjustSize from AttachGalley (d)");
AdjustSize(target, back(target_galley, 1-dim),
fwd(target_galley, 1-dim), 1-dim);
}
else
{
/* adjust both directions, using z (last component) */
Child(z, LastDown(target_galley));
debug0(DSA, D, "AttachGalley AdjustSize using z =");
ifdebug(DSA, D, DebugObject(z));
debug0(DSA, D, "calling AdjustSize from AttachGalley (e)");
AdjustSize(target, back(z, dim), fwd(z, dim), dim);
debug0(DSA, D, "calling AdjustSize from AttachGalley (f)");
AdjustSize(target, back(z, 1-dim), fwd(z, 1-dim), 1-dim);
}
goto ACCEPT;
default:
assert1(FALSE, "AttachGalley:", Image(type(y)));
break;
} /* end switch */
} /* end for */
/* null galley: promote whole galley without expanding the target */
debug0(DGA, D, " null galley");
if( tg_inners != nilobj ) DisposeObject(tg_inners), tg_inners = nilobj;
DisposeObject(target_galley);
LeaveErrorBlock(FALSE);
debug0(DYY, D, "] LeaveErrorBlock(FALSE) (null galley)");
/* kill off any null objects within the galley, then transfer it */
/* don't use Promote() since it does extra unwanted things here */
for( link = Down(hd); link != hd; link = NextDown(link) )
{ Child(y, link);
switch( type(y) )
{
case GAP_OBJ:
case CLOSURE:
case CROSS:
case FORCE_CROSS:
case NULL_CLOS:
case PAGE_LABEL:
link = PrevDown(link);
debug1(DGA, D, " null galley, disposing %s", Image(type(y)));
DisposeChild(NextDown(link));
break;
default:
break;
}
}
TransferLinks(NextDown(hd), hd, Up(target_index));
/* attach hd temporarily to target_index */
MoveLink(Up(hd), target_index, PARENT);
assert( type(hd_index) == UNATTACHED, "AttachGalley: type(hd_index)!" );
DeleteNode(hd_index);
/* return; only hd_inners needs to be flushed now */
*inners = hd_inners;
debug0(DGA, D, "] AttachGalley returning ATTACH_NULL");
return ATTACH_NULL;
REJECT:
/* reject first component */
/* debug1(DGA, D, " reject %s", EchoObject(y)); */
debug0(DGA, D, " reject first component");
LeaveErrorBlock(TRUE);
debug0(DYY, D, "] LeaveErrorBlock(TRUE) (REJECT)");
if( tg_inners != nilobj ) DisposeObject(tg_inners), tg_inners = nilobj;
DisposeObject(target_galley);
if( foll_or_prec(hd) == GALL_PREC && !sized(hd) )
{
/* move to just before the failed target */
MoveLink(Up(hd_index), Up(target_index), PARENT);
}
else
{
/* move to just after the failed target */
MoveLink(Up(hd_index), NextDown(Up(target_index)), PARENT);
}
continue;
SUSPEND:
/* suspend at first component */
debug1(DGA, D, " suspend %s", EchoIndex(y));
blocked(y) = TRUE;
LeaveErrorBlock(FALSE);
debug0(DYY, D, "] LeaveErrorBlock(FALSE) (SUSPEND)");
if( tg_inners != nilobj ) DisposeObject(tg_inners), tg_inners = nilobj;
DisposeObject(target_galley);
MoveLink(Up(hd_index), Up(target_index), PARENT);
if( was_sized )
{ /* nothing new to flush if suspending and already sized */
if( hd_inners != nilobj ) DisposeObject(hd_inners), hd_inners=nilobj;
*inners = nilobj;
}
else
{ /* flush newly discovered inners if not sized before */
*inners = hd_inners;
}
debug0(DGA, D, "] AttachGalley returning ATTACH_SUSPEND");
*suspend_pt = y;
return ATTACH_SUSPEND;
ACCEPT:
/* accept first component; now committed to the attach */
debug3(DGA, D, " accept %s %s %s", Image(type(y)), EchoObject(y),
EchoFilePos(&fpos(y)));
LeaveErrorBlock(TRUE);
debug0(DYY, D, "] LeaveErrorBlock(TRUE) (ACCEPT)");
/* attach hd to dest */
MoveLink(Up(hd), dest_index, PARENT);
assert( type(hd_index) == UNATTACHED, "AttachGalley: type(hd_index)!" );
DeleteNode(hd_index);
/* move first component of hd into dest */
/* nb Interpose must be done after all AdjustSize calls */
if( dim == ROWM && !external_ver(dest) )
Interpose(dest, VCAT, hd, y);
else if( dim == COLM && !external_hor(dest) )
{ Interpose(dest, ACAT, y, y);
Parent(junk, Up(dest));
assert( type(junk) == ACAT, "AttachGalley: type(junk) != ACAT!" );
StyleCopy(save_style(junk), save_style(dest));
adjust_cat(junk) = padjust(save_style(junk));
}
debug1(DGS, D, "calling Promote(hd, %s) from AttachGalley/ACCEPT",
link == hd ? "hd" : "NextDown(link)");
Promote(hd, link == hd ? hd : NextDown(link), dest_index, TRUE);
/* move target_galley into target */
/* nb Interpose must be done after all AdjustSize calls */
if( !(external_ver(target) || external_hor(target)) )
{ Child(z, LastDown(target_galley));
Interpose(target, VCAT, z, z);
}
debug0(DGS, D, "calling Promote(target_galley) from AttachGalley/ACCEPT");
Promote(target_galley, target_galley, target_index, TRUE);
DeleteNode(target_galley);
assert(Down(target_index)==target_index, "AttachGalley: target_ind");
if( blocked(target_index) ) blocked(dest_index) = TRUE;
DeleteNode(target_index);
/* return; both tg_inners and hd_inners need to be flushed now; */
/* if was_sized, hd_inners contains the inners of the first component; */
/* otherwise it contains the inners of all components, from SizeGalley */
if( tg_inners == nilobj ) *inners = hd_inners;
else if( hd_inners == nilobj ) *inners = tg_inners;
else
{ TransferLinks(Down(hd_inners), hd_inners, tg_inners);
DeleteNode(hd_inners);
*inners = tg_inners;
}
debug0(DGA, D, "] AttachGalley returning ATTACH_ACCEPT");
ifdebug(DGA, D,
if( dim == COLM && !external_hor(dest) )
{ OBJECT z;
Parent(z, Up(dest));
debug2(DGA, D, " COLM dest_encl on exit = %s %s",
Image(type(z)), EchoObject(z));
}
)
return ATTACH_ACCEPT;
} /* end for */
ALWAYS_INLINE static bool Detect(uint32_t last_word, Window<Subtype>& window, OffsetBlock* out) {
if (last_word != MakeWord('R', 'I', 'F', 'F')) {
return false;
}
return false;
}
int startup(int argc, char *argv[])
{ int i, len; FULL_CHAR *arg;
OBJECT t, y, res, s; /* current token, parser output */
BOOLEAN stdin_seen; /* TRUE when stdin file seen */
int source_file_count; /* number of source files in command */
FULL_CHAR *cross_db; /* name of cross reference database */
FULL_CHAR *outfile; /* name of output file */
FULL_CHAR *lib; /* name of library directory */
FILE *out_fp;
long MemCheckLong;
FULL_CHAR oname[MAX_BUFF], oval[MAX_BUFF], buff[MAX_BUFF], *p;
int bp; OBJECT z;
BOOLEAN seen_wordcount;
#if LOCALE_ON
char catname[MAX_BUFF], *loc;
#endif
/* find the name of the library directory, from envt or else from -D */
lib = AsciiToFull(getenv("LOUTLIB"));
if( lib == (FULL_CHAR *) NULL )
lib = AsciiToFull(LIB_DIR);
/* set locale if that's what we are doing */
#if LOCALE_ON
loc = setlocale(LC_MESSAGES, "");
if( loc == (char *) NULL )
{ Error(1, 6, "unable to initialize locale", WARN, no_fpos);
loc = "C";
}
sprintf(catname, "%s/%s/%s/LC_MESSAGES/errors.%s",
lib, LOCALE_DIR, loc, loc);
MsgCat = catopen(catname, 0);
#endif
/* initialise various modules, add current directory to search paths */
TotalWordCount = 0;
seen_wordcount = FALSE;
BackEnd = PS_BackEnd;
PlainCharWidth = PLAIN_WIDTH;
PlainCharHeight = PLAIN_HEIGHT;
PlainFormFeed = FALSE;
InitializeAll = FALSE;
UseCollate = COLLATE;
AllowCrossDb = TRUE;
InMemoryDbIndexes = TRUE;
Encapsulated = FALSE;
SafeExecution = SAFE_DFT ? TRUE : FALSE;
Kern = TRUE;
MemInit();
InitSym();
LexInit();
InitFiles();
AddToPath(SOURCE_PATH, MakeWord(WORD, STR_EMPTY, no_fpos));
AddToPath(DATABASE_PATH, MakeWord(WORD, STR_EMPTY, no_fpos));
AddToPath(INCLUDE_PATH, MakeWord(WORD, STR_EMPTY, no_fpos));
/* read command line */
stdin_seen = FALSE;
AltErrorFormat = FALSE;
cross_db = CROSS_DB;
outfile = STR_STDOUT;
source_file_count = 0;
New(CommandOptions, ACAT);
for( i = 1; i < argc; i++ )
{
if( *argv[i] == CH_HYPHEN ) switch( *(argv[i]+1) )
{
case CH_FLAG_OUTFILE:
/* read name of output file */
if( (outfile = GetArg(argv, argc, &i)) == NULL )
Error(1, 7, "usage: -o <filename>", FATAL, no_fpos);
if( StringEndsWith(outfile, SOURCE_SUFFIX) )
Error(1, 28, "-o: output file name %s ends with %s",
FATAL, no_fpos, outfile, SOURCE_SUFFIX);
break;
case CH_FLAG_SUPPRESS:
/* suppress references to OldCrossDb and NewCrossDb */
AllowCrossDb = FALSE;
break;
case CH_FLAG_MEMCR:
/* don't use in-memory database indexes */
InMemoryDbIndexes = FALSE;
break;
case CH_FLAG_NOKERN:
/* suppress kerning */
Kern = FALSE;
break;
case CH_FLAG_NOCOLLATE:
/* suppress local collation */
UseCollate = FALSE;
break;
case CH_FLAG_COLLATE:
/* invoke local collation */
UseCollate = TRUE;
break;
case CH_FLAG_CROSS:
/* read name of cross reference database */
if( (cross_db = GetArg(argv, argc, &i)) == NULL )
Error(1, 8, "usage: -c <filename>", FATAL, no_fpos);
break;
case CH_FLAG_ERRFILE:
/* read log file name */
if( (arg = GetArg(argv, argc, &i)) == NULL )
Error(1, 9, "usage: -e <filename>", FATAL, no_fpos);
ErrorInit(arg);
break;
case CH_FLAG_ALTERR:
/* alternative error message format */
AltErrorFormat = TRUE;
break;
case CH_FLAG_EPSFIRST:
/* -EPS produces encapsulated PostScript output */
if( !StringEqual(AsciiToFull(argv[i]+1), STR_EPS) )
Error(1, 10, "usage: -EPS", FATAL, no_fpos);
Encapsulated = TRUE;
break;
case CH_FLAG_DIRPATH:
/* add directory to database and sysdatabase paths */
if( (arg = GetArg(argv, argc, &i)) == NULL )
Error(1, 11, "usage: -D <directoryname>", FATAL, no_fpos);
AddToPath(DATABASE_PATH, MakeWord(WORD, arg, no_fpos));
AddToPath(SYSDATABASE_PATH, MakeWord(WORD, arg, no_fpos));
break;
case CH_FLAG_ENCPATH:
/* add directory to character mapping path */
if( (arg = GetArg(argv, argc, &i)) == NULL )
Error(1, 12, "usage: -C <directoryname>", FATAL, no_fpos);
AddToPath(MAPPING_PATH, MakeWord(WORD, arg, no_fpos));
break;
case CH_FLAG_FNTPATH:
/* add directory to font path */
if( (arg = GetArg(argv, argc, &i)) == NULL )
Error(1, 13, "usage: -F <directoryname>", FATAL, no_fpos);
AddToPath(FONT_PATH, MakeWord(WORD, arg, no_fpos));
break;
case CH_FLAG_HYPPATH:
/* add directory to hyph path */
if( (arg = GetArg(argv, argc, &i)) == NULL )
Error(1, 14, "usage: -H <directoryname>", FATAL, no_fpos);
AddToPath(HYPH_PATH, MakeWord(WORD, arg, no_fpos));
break;
case CH_FLAG_INCPATH:
/* add directory to include and sysinclude paths */
if( (arg = GetArg(argv, argc, &i)) == NULL )
Error(1, 15, "usage: -I <directoryname>", FATAL, no_fpos);
AddToPath(INCLUDE_PATH, MakeWord(WORD, arg, no_fpos));
AddToPath(SYSINCLUDE_PATH, MakeWord(WORD, arg, no_fpos));
break;
case CH_FLAG_INCLUDE:
/* read sysinclude file and strip any .lt suffix */
if( (arg = GetArg(argv, argc, &i)) == NULL )
Error(1, 16, "usage: -i <filename>", FATAL, no_fpos);
len = StringLength(arg) - StringLength(SOURCE_SUFFIX);
if( len >= 0 && StringEqual(&arg[len], SOURCE_SUFFIX) )
StringCopy(&arg[len], STR_EMPTY);
debug0(DFS, D, " calling DefineFile from main (1)");
DefineFile(arg, STR_EMPTY, no_fpos, SOURCE_FILE, SYSINCLUDE_PATH);
break;
case CH_FLAG_HYPHEN:
/* declare hyphenation file */
if( FirstFile(HYPH_FILE) != NO_FILE )
Error(1, 17, "two -h options illegal", FATAL, no_fpos);
if( (arg = GetArg(argv, argc, &i)) == NULL )
Error(1, 18, "usage: -h <filename>", FATAL, no_fpos);
debug0(DFS, D, " calling DefineFile from main (2)");
DefineFile(arg, STR_EMPTY, no_fpos, HYPH_FILE, INCLUDE_PATH);
DefineFile(arg, HYPH_SUFFIX, no_fpos, HYPH_PACKED_FILE, INCLUDE_PATH);
break;
case CH_FLAG_VERSION:
fprintf(stderr, "%s\n", LOUT_VERSION);
fprintf(stderr, "%-28s %s\n",
"Basser Lout written by:", "Jeffrey H. Kingston ([email protected])");
fprintf(stderr, "%-28s %s\n",
"Free source available from:", "ftp://ftp.cs.usyd.edu.au/jeff/lout");
fprintf(stderr, "%-28s %s %s\n",
"This executable compiled:", __TIME__, __DATE__);
fprintf(stderr, "%-28s %s%s%s\n", "System include directory:",
lib, STR_DIR, INCL_DIR);
fprintf(stderr, "%-28s %s%s%s\n", "System database directory:",
lib, STR_DIR, DATA_DIR);
fprintf(stderr, "Database index files created afresh automatically:%s\n",
USE_STAT ? " yes" : " no");
fprintf(stderr, "Safe execution (disabling system()) is default:%s\n",
SAFE_DFT ? " yes" : " no");
fprintf(stderr, "strcoll() used for sorting by default:%s\n",
COLLATE ? " yes" : " no");
fprintf(stderr, "PDF compression on:%s\n",
PDF_COMPRESSION ? " yes" : " no");
fprintf(stderr, "Debugging (-d, -dd, -ddd flags) available:%s\n",
DEBUG_ON ? " yes" : " no");
fprintf(stderr, "\n");
fprintf(stderr, "Basser Lout comes with ABSOLUTELY NO WARRANTY.\n");
fprintf(stderr, "This is free software, and you are welcome to\n");
fprintf(stderr, "redistribute it under certain conditions. For\n");
fprintf(stderr, "details on both points, consult the GNU General\n");
fprintf(stderr, "Public License (distributed with this software).\n");
exit(0);
break;
case CH_FLAG_WORDS:
seen_wordcount = TRUE;
break;
case CH_FLAG_PDF:
BackEnd = PDF_BackEnd;
break;
case CH_FLAG_FFPLAIN:
if( StringEqual(AsciiToFull(argv[i]+1), STR_PDF) )
{
BackEnd = PDF_BackEnd;
break;
}
PlainFormFeed = TRUE;
/* NB NO BREAK */
case CH_FLAG_PLAIN:
BackEnd = Plain_BackEnd;
if( *(argv[i]+2) != '\0' )
{ float len1, len2; FULL_CHAR units1, units2;
if( sscanf(argv[i]+2, "%f%c%f%c",&len1,&units1,&len2,&units2) != 4 )
{ Error(1, 19, "usage: lout -%c<length><length>",
FATAL, no_fpos, *(argv[i]+1));
}
switch( units1 )
{
case CH_UNIT_CM: PlainCharWidth = len1 * CM; break;
case CH_UNIT_IN: PlainCharWidth = len1 * IN; break;
case CH_UNIT_PT: PlainCharWidth = len1 * PT; break;
case CH_UNIT_EM: PlainCharWidth = len1 * EM; break;
default: Error(1, 20, "lout -%c: units must be c, i, p, or m",
FATAL, no_fpos, *(argv[i]+1));
break;
}
switch( units2 )
{
case CH_UNIT_CM: PlainCharHeight = len2 * CM; break;
case CH_UNIT_IN: PlainCharHeight = len2 * IN; break;
case CH_UNIT_PT: PlainCharHeight = len2 * PT; break;
case CH_UNIT_EM: PlainCharHeight = len2 * EM; break;
default: Error(1, 21, "lout -%c: units must be c, i, p, or m",
FATAL, no_fpos, *(argv[i]+1));
break;
}
}
break;
case CH_FLAG_INITALL:
InitializeAll = TRUE;
AllowCrossDb = FALSE;
break;
case CH_FLAG_USAGE:
PrintUsage(stderr);
exit(0);
break;
case CH_FLAG_DEBUG:
debug_init(AsciiToFull(argv[i]));
break;
case CH_FLAG_MEMCHECK:
sscanf(argv[i], "-m%ld", &MemCheckLong);
MemCheck = (POINTER) MemCheckLong;
fprintf(stderr, "checking memory location %ld\n", (long) MemCheck);
break;
case '\0':
/* read stdin as file name */
if( stdin_seen )
Error(1, 23, "standard input specified twice", FATAL, no_fpos);
stdin_seen = TRUE;
debug0(DFS, D, " calling DefineFile from main (3)");
DefineFile(STR_STDIN, STR_EMPTY, no_fpos, SOURCE_FILE, SOURCE_PATH);
break;
case CH_FLAG_OPTION:
/* read command-line document option */
if( sscanf(argv[i]+2, "%[^{ ] { %[^}] }", oname, oval) != 2 ||
StringLength(oname) == 0 || StringLength(oval) == 0 )
Error(1, 24, "error in command-line option %s", FATAL, no_fpos,
argv[i]+2);
y = MakeWord(WORD, oname, no_fpos);
Link(CommandOptions, y);
New(y, ACAT);
Link(CommandOptions, y);
bp = 0;
for( p = oval; *p != '\0'; p++ ) switch( *p )
{
case ' ':
case '\t':
case '\n':
case '{':
case '}':
if( bp > 0 )
{ buff[bp++] = '\0';
if( Down(y) != y )
{ OBJECT g;
New(g, GAP_OBJ);
hspace(g) = 1; vspace(g) = 0;
FposCopy(fpos(g), *no_fpos);
Link(y, g);
}
z = MakeWord(WORD, buff, no_fpos);
Link(y, z);
bp = 0;
}
break;
default:
buff[bp++] = *p;
break;
}
if( bp > 0 )
{ buff[bp++] = '\0';
z = MakeWord(WORD, buff, no_fpos);
Link(y, z);
}
if( Down(y) == y )
Error(1, 25, "error in command-line option %s", FATAL, no_fpos,
argv[i]+2);
break;
case CH_FLAG_SAFE:
/* ensure safe execution by disabling system calls */
SafeExecution = TRUE;
break;
case CH_FLAG_UNSAFE:
/* allow unsafe execution */
SafeExecution = FALSE;
break;
default:
PrintUsage(stderr);
Error(1, 26, "unknown command line flag %s", FATAL, no_fpos, argv[i]);
break;
}
else
{ /* argument is source file, strip any .lout suffix and define it */
arg = AsciiToFull(argv[i]);
len = StringLength(arg) - StringLength(SOURCE_SUFFIX);
if( len >= 0 && StringEqual(&arg[len], SOURCE_SUFFIX) )
StringCopy(&arg[len], STR_EMPTY);
debug0(DFS, D, " calling DefineFile from main (4)");
DefineFile(AsciiToFull(argv[i]), STR_EMPTY, no_fpos,
SOURCE_FILE, SOURCE_PATH);
source_file_count++;
}
} /* for */
if( UseCollate )
{
if (!setlocale (LC_COLLATE, ""))
Error(1, 30, "unable to initialize collation", WARN, no_fpos);
}
/* start timing if required */
ifdebug(DPP, D, ProfileOn("main"));
/* open output file, or stdout if none specified, and initialize printer */
if( StringEqual(outfile, STR_STDOUT) )
{
#if OS_DOS
/* For DOS/Win32 we need to set binary mode on stdout to prevent
PDF compressed streams and xrefs from being corrupted - Uwe 12/98 */
if( BackEnd->code != PLAINTEXT &&
_setmode(_fileno(stdout), _O_BINARY) == -1 )
Error(1, 31, "cannot set binary mode on stdout", FATAL, no_fpos);
#endif
out_fp = stdout;
}
else
{ out_fp = StringFOpen(outfile,
BackEnd->code == PLAINTEXT ? WRITE_TEXT : WRITE_BINARY);
if( out_fp == null )
Error(1, 27, "cannot open output file %s", FATAL, no_fpos, outfile);
}
/* initialize miscellaneous modules */
ColourInit();
LanguageInit();
BackEnd->PrintInitialize(out_fp);
/* append default directories to file search paths */
AddToPath(FONT_PATH, MakeWordThree(lib, STR_DIR, AsciiToFull(FONT_DIR)));
AddToPath(HYPH_PATH, MakeWordThree(lib, STR_DIR, AsciiToFull(HYPH_DIR)));
AddToPath(MAPPING_PATH, MakeWordThree(lib, STR_DIR, AsciiToFull(MAPS_DIR)));
AddToPath(SYSDATABASE_PATH,MakeWordThree(lib,STR_DIR, AsciiToFull(DATA_DIR)));
AddToPath(DATABASE_PATH, MakeWordThree(lib, STR_DIR, AsciiToFull(DATA_DIR)));
AddToPath(SYSINCLUDE_PATH,MakeWordThree(lib, STR_DIR, AsciiToFull(INCL_DIR)));
AddToPath(INCLUDE_PATH, MakeWordThree(lib, STR_DIR, AsciiToFull(INCL_DIR)));
/* use stdin if no source files were mentioned */
if( source_file_count == 0 )
{ debug0(DFS, D, " calling DefineFile from main (5)");
DefineFile(STR_STDIN, STR_EMPTY, no_fpos, SOURCE_FILE, SOURCE_PATH);
}
/* load predefined symbols into symbol table */
StartSym = nilobj; /* Not a mistake */
StartSym = load(KW_START, 0, FALSE, FALSE, TRUE, NO_PREC );
GalleySym = load(KW_GALLEY, 0, FALSE, FALSE, TRUE, NO_PREC );
ForceGalleySym= load(KW_FORCE_GALLEY, 0, FALSE, FALSE, TRUE, NO_PREC );
InputSym = load(KW_INPUT, 0, FALSE, FALSE, TRUE, NO_PREC );
PrintSym = load(KW_PRINT, 0, FALSE, FALSE, TRUE, NO_PREC );
FilterInSym = load(KW_FILTERIN, 0, FALSE, FALSE, FALSE, NO_PREC );
FilterOutSym = load(KW_FILTEROUT, 0, FALSE, FALSE, FALSE, NO_PREC );
FilterErrSym = load(KW_FILTERERR, 0, FALSE, FALSE, FALSE, NO_PREC );
OptGallSym = load(KW_OPTGALL, 0, FALSE, TRUE, FALSE, DEFAULT_PREC);
VerbatimSym = load(KW_VERBATIM,VERBATIM,FALSE, TRUE, FALSE, DEFAULT_PREC);
RawVerbatimSym= load(KW_RAWVERBATIM,RAW_VERBATIM,FALSE,TRUE,FALSE,DEFAULT_PREC);
load(KW_BEGIN, BEGIN, FALSE, FALSE, FALSE, BEGIN_PREC );
load(KW_END, END, FALSE, FALSE, FALSE, END_PREC );
load(KW_ENV, ENV, FALSE, FALSE, FALSE, NO_PREC );
load(KW_ENVA, ENVA, FALSE, FALSE, FALSE, NO_PREC );
load(KW_ENVB, ENVB, FALSE, FALSE, FALSE, NO_PREC );
load(KW_ENVC, ENVC, FALSE, FALSE, FALSE, NO_PREC );
load(KW_ENVD, ENVD, FALSE, FALSE, FALSE, NO_PREC );
load(KW_CENV, CENV, FALSE, FALSE, FALSE, NO_PREC );
load(KW_CLOS, CLOS, FALSE, FALSE, FALSE, NO_PREC );
load(KW_LVIS, LVIS, FALSE, FALSE, FALSE, NO_PREC );
load(KW_LUSE, LUSE, FALSE, FALSE, FALSE, NO_PREC );
load(KW_LEO, LEO, FALSE, FALSE, FALSE, NO_PREC );
load(KW_LBR, LBR, FALSE, FALSE, FALSE, LBR_PREC );
load(KW_RBR, RBR, FALSE, FALSE, FALSE, RBR_PREC );
load(KW_INCLUDE, INCLUDE, FALSE, FALSE, FALSE, NO_PREC );
load(KW_SYSINCLUDE, SYS_INCLUDE, FALSE, FALSE, FALSE, NO_PREC );
load(KW_PREPEND, PREPEND, FALSE, FALSE, FALSE, NO_PREC );
load(KW_SYSPREPEND, SYS_PREPEND, FALSE, FALSE, FALSE, NO_PREC );
load(KW_DATABASE, DATABASE, FALSE, FALSE, FALSE, NO_PREC );
load(KW_SYSDATABASE, SYS_DATABASE, FALSE, FALSE, FALSE, NO_PREC );
load(KW_USE, USE, FALSE, FALSE, FALSE, NO_PREC );
load(KW_NOT_REVEALED, NOT_REVEALED, FALSE, FALSE, FALSE, NO_PREC );
load(KW_CASE, CASE, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_YIELD, YIELD, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_BACKEND, BACKEND, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_XCHAR, XCHAR, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_FONT, FONT, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_SPACE, SPACE, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_YUNIT, YUNIT, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_ZUNIT, ZUNIT, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_BREAK, BREAK, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_UNDERLINE, UNDERLINE, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_COLOUR, COLOUR, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_COLOR, COLOUR, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_OUTLINE, OUTLINE, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_LANGUAGE, LANGUAGE, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_CURR_LANG, CURR_LANG, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_CURR_FAMILY, CURR_FAMILY, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_CURR_FACE, CURR_FACE, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_CURR_YUNIT, CURR_YUNIT, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_CURR_ZUNIT, CURR_ZUNIT, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_COMMON, COMMON, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_RUMP, RUMP, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_MELD, MELD, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_INSERT, INSERT, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_ONE_OF, ONE_OF, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_NEXT, NEXT, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_PLUS, PLUS, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_MINUS, MINUS, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_OPEN, OPEN, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_TAGGED, TAGGED, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_WIDE, WIDE, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HIGH, HIGH, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HSHIFT, HSHIFT, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_VSHIFT, VSHIFT, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_BEGIN_HEADER, BEGIN_HEADER, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_END_HEADER, END_HEADER, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_SET_HEADER, SET_HEADER, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_CLEAR_HEADER, CLEAR_HEADER, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_ONE_COL, ONE_COL, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_ONE_ROW, ONE_ROW, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HSCALE, HSCALE, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_VSCALE, VSCALE, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HCOVER, HCOVER, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_VCOVER, VCOVER, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_KERN_SHRINK, KERN_SHRINK, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_SCALE, SCALE, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HCONTRACT, HCONTRACT, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_VCONTRACT, VCONTRACT, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HLIMITED, HLIMITED, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_VLIMITED, VLIMITED, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HEXPAND, HEXPAND, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_VEXPAND, VEXPAND, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_STARTHVSPAN, START_HVSPAN, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_STARTHSPAN, START_HSPAN, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_STARTVSPAN, START_VSPAN, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HSPAN, HSPAN, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_VSPAN, VSPAN, FALSE, FALSE, FALSE, DEFAULT_PREC);
load(KW_PADJUST, PADJUST, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_HADJUST, HADJUST, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_VADJUST, VADJUST, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_ROTATE, ROTATE, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_BACKGROUND, BACKGROUND, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_INCGRAPHIC, INCGRAPHIC, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_SINCGRAPHIC, SINCGRAPHIC, FALSE, TRUE, FALSE, DEFAULT_PREC);
load(KW_PLAINGRAPHIC, PLAIN_GRAPHIC, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_GRAPHIC, GRAPHIC, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_LINK_SOURCE, LINK_SOURCE, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_LINK_DEST, LINK_DEST, TRUE, TRUE, FALSE, DEFAULT_PREC);
load(KW_CROSS, CROSS, TRUE, TRUE, FALSE, CROSSOP_PREC);
load(KW_FORCE_CROSS, FORCE_CROSS, TRUE, TRUE, FALSE, CROSSOP_PREC);
load(KW_NULL, NULL_CLOS, FALSE, FALSE, TRUE, NO_PREC );
load(KW_PAGE_LABEL, PAGE_LABEL, FALSE, TRUE, TRUE, DEFAULT_PREC);
#define setcat(s, mk, jn) has_mark(s)=mk, has_join(s)=jn
s=load(KW_VCAT_NN, VCAT, TRUE, TRUE, FALSE, VCAT_PREC); setcat(s,FALSE,FALSE);
s=load(KW_VCAT_MN, VCAT, TRUE, TRUE, FALSE, VCAT_PREC); setcat(s,TRUE, FALSE);
s=load(KW_VCAT_NJ, VCAT, TRUE, TRUE, FALSE, VCAT_PREC); setcat(s,FALSE,TRUE);
s=load(KW_VCAT_MJ, VCAT, TRUE, TRUE, FALSE, VCAT_PREC); setcat(s,TRUE, TRUE);
s=load(KW_HCAT_NN, HCAT, TRUE, TRUE, FALSE, HCAT_PREC); setcat(s,FALSE,FALSE);
s=load(KW_HCAT_MN, HCAT, TRUE, TRUE, FALSE, HCAT_PREC); setcat(s,TRUE, FALSE);
s=load(KW_HCAT_NJ, HCAT, TRUE, TRUE, FALSE, HCAT_PREC); setcat(s,FALSE,TRUE);
s=load(KW_HCAT_MJ, HCAT, TRUE, TRUE, FALSE, HCAT_PREC); setcat(s,TRUE, TRUE);
s=load(KW_ACAT_NJ, ACAT, TRUE, TRUE, FALSE, ACAT_PREC); setcat(s,FALSE,TRUE);
s=load(KW_ACAT_MJ, ACAT, TRUE, TRUE, FALSE, ACAT_PREC); setcat(s,TRUE, TRUE);
/* intialize fonts and load @FontDef symbol */
FontInit();
/* intialize current time and load @Moment symbol */
InitTime();
/* initialize filter module */
FilterInit();
/* initialize enviroment table module */
EnvInit();
/* initialise scope chain to <StartSym> */
PushScope(StartSym, FALSE, FALSE);
/* initialise lexical analyser */
LexPush(FirstFile(SOURCE_FILE), 0, SOURCE_FILE, 1, FALSE);
/* process input files */
InitParser(cross_db);
t = NewToken(BEGIN, no_fpos, 0, 0, BEGIN_PREC, StartSym);
res = Parse(&t, StartSym, TRUE, TRUE);
debug0(DGT, D, "calling TransferEnd(res) from main()");
DisposeObject(CommandOptions);
TransferEnd(res);
TransferClose();
/* close various modules */
BackEnd->PrintAfterLastPage();
BackEnd->LinkCheck();
CrossClose();
CloseFiles();
/* remove any leftover filter temporary files */
FilterScavenge(TRUE);
/* print word count, if required */
if( seen_wordcount )
Error(1, 29, "total of all words printed: %d", WARN,no_fpos,TotalWordCount);
/* check for unbalanced error blocks */
CheckErrorBlocks();
/* wrapup */
ifdebug(DST, DD, CheckSymSpread() );
ifdebug(ANY, D, DeleteEverySym() );
debug0(DMA, D, "at end of run:");
ifdebug(DMA, D, DebugMemory() );
ifdebug(DPP, D, ProfileOff("main"));
ifdebug(DPP, D, ProfilePrint());
ifdebug(DET, D, EnvDebug());
#if LOCALE_ON
catclose(MsgCat);
#endif
exit(0);
return 0;
} /* end main */
uint16_t GetWordParam(uint8_t* data, uint16_t param_byte_offset) {
return MakeWord(data[PREFIX_SIZE + param_byte_offset],
data[PREFIX_SIZE + param_byte_offset + 1]);
}
OBJECT ClosureExpand(OBJECT x, OBJECT env, BOOLEAN crs_wanted,
OBJECT *crs, OBJECT *res_env)
{ OBJECT link, y, res, prnt_env, par, prnt;
debug3(DCE, D, "[ ClosureExpand( %s, %s, %s, crs, res_env )",
EchoObject(x), EchoObject(env), bool(crs_wanted));
assert( type(x) == CLOSURE, "ClosureExpand given non-CLOSURE!");
assert( predefined(actual(x)) == FALSE, "ClosureExpand given predefined!" );
/* add tag to x if needed but not provided; add cross-reference to crs */
if( has_tag(actual(x)) ) CrossAddTag(x);
if( crs_wanted && has_tag(actual(x)) )
{ OBJECT tmp = CopyObject(x, no_fpos); AttachEnv(env, tmp);
y = CrossMake(actual(x), tmp, CROSS_TARG);
New(tmp, CROSS_TARG); actual(tmp) = y; Link(tmp, y);
if( *crs == nilobj ) New(*crs, CR_LIST); Link(*crs, tmp);
}
/* case x is a parameter */
res = *res_env = nilobj;
if( is_par(type(actual(x))) )
{ prnt = SearchEnv(env, enclosing(actual(x)));
if( prnt != nilobj )
{
prnt_env = GetEnv(prnt);
for( link = Down(prnt); link != prnt; link = NextDown(link) )
{ Child(par, link);
if( type(par) == PAR && actual(par) == actual(x) )
{ assert( Down(par) != par, "ExpandCLosure: Down(par)!");
Child(res, Down(par));
if( dirty(enclosing(actual(par))) || is_enclose(actual(par)) )
{ debug2(DCE, DD, "copy %s %s", SymName(actual(par)), EchoObject(res));
res = CopyObject(res, no_fpos);
}
else
{
debug2(DCE, DD, "link %s %s",
FullSymName(actual(par), AsciiToFull(".")), EchoObject(res));
DeleteLink(Down(par));
y = MakeWord(WORD, STR_NOCROSS, &fpos(res));
Link(par, y);
}
ReplaceNode(res, x);
if( type(actual(x)) == RPAR && has_body(enclosing(actual(x))) )
{ debug0(DCR, DDD, " calling SetEnv from ClosureExpand (a)");
*res_env = SetEnv(prnt, nilobj); DisposeObject(x);
}
else if( type(actual(x)) == NPAR && imports_encl(actual(x)) )
{ debug0(DCR, DDD, " calling SetEnv from ClosureExpand (x)");
AttachEnv(env, x);
*res_env = SetEnv(x, nilobj);
}
else
{ AttachEnv(env, x);
debug0(DCR, DDD, " calling SetEnv from ClosureExpand (b)");
*res_env = SetEnv(x, prnt_env);
}
break;
}
}
}
else
{
/* fail only if there is no default value available */
if( sym_body(actual(x)) == nilobj )
{
debug3(DCE, D, "failing ClosureExpand( %s, crs, %s, %s, res_env )",
EchoObject(x), bool(crs_wanted), EchoObject(env));
Error(9, 2, "no value for parameter %s of symbol %s:", WARN, &fpos(x),
SymName(actual(x)), SymName(enclosing(actual(x))));
Error(9, 1, "symbol with import list misused", FATAL, &fpos(x));
}
}
}
/* case x is a user-defined symbol or default parameter */
if( res == nilobj )
{ if( sym_body(actual(x)) == nilobj )
res = MakeWord(WORD, STR_NOCROSS, &fpos(x));
else
res = CopyObject(sym_body(actual(x)), &fpos(x));
ReplaceNode(res, x); AttachEnv(env, x);
debug0(DCR, DDD, " calling SetEnv from ClosureExpand (c)");
*res_env = SetEnv(x, nilobj);
}
assert( *res_env!=nilobj && type(*res_env)==ENV, "ClosureExpand: *res_env!");
debug0(DCE, D, "] ClosureExpand returning, res =");
ifdebug(DCE, D, DebugObject(res));
debug1(DCE, D, " environment = %s", EchoObject(*res_env));
return res;
} /* end ClosureExpand */
int BulkReadData::ReadWord(int address){
if(address >= start_address && address < (start_address + length))
return MakeWord(table[address], table[address+1]);
return 0;
}
/* ReadWrite - Takes care of all read and write commands.
* Return - For write, returns length of packet that was successfully sent out.
* - For read, returns length of packet that was succesfully read.
* - If unsuccessful, returns a bad number (-99999)
*/
int DarwinController::ReadWrite(unsigned char *txpacket, unsigned char *rxpacket){
int buf = 30;
int length = 0;
int count = 0;
unsigned char info[MAXNUM_RXPARAM] = {0, };
length = txpacket[LENGTH] + 4;
int return_length = -99999;
int to_length = 0;
int num = 0;
port.ClearPort();
if(port.WritePort(txpacket, length) == length){ // if successfully writes out the entire packet
//printf("in readwrite length: %d\n", length); // for debug purposes
if(txpacket[INSTRUCTION] == SYNC_WRITE){
return length;
}
if(txpacket[INSTRUCTION] == BULK_READ){ //set bulk read vars
num = (txpacket[LENGTH]-3) / 3;
for(int i = 0; i < num; i++){
int _id = txpacket[PARAMETER+(3*i)+2];
int _len = txpacket[PARAMETER+(3*i)+1];
int _addr = txpacket[PARAMETER+(3*i)+3];
to_length += _len + 6;
// uncomment if you want to use bulkreaddata structure
//BulkData[_id].length = _len;
//BulkData[_id].start_address = _addr;
}
} else if(txpacket[INSTRUCTION] == READ){
to_length = txpacket[PARAMETER+1] + 6;
} else {
to_length = 6;
}
int get_length = 0; // total received packet length
int fail_counter = 0; // fail counter for ping
int length = 0; //received current packet length
//printf("getlength: %d, tolength: %d\n", get_length, to_length); // for debug purposes
// set packet time out:
double packetStartTime = Time.getCurrentTime();
double packetWaitTime = 0.012 * (double)length + 5.0;
while(1){ // loop for receiving packet
if(fail_counter >= 5){ //failed 5 times. return.
if(txpacket[INSTRUCTION] == PING){
printf("failed ping\n");
}
return 0; // culprit. things go wrong if this returns -1.
}
length = port.ReadPort(&rxpacket[get_length], to_length - get_length);
get_length += length; // get_length is how long the received packet is
if(get_length == to_length){ //received a packet of correct length
if(txpacket[INSTRUCTION] == BULK_READ){
break;
} else if(rxpacket[0] == 0xFF && rxpacket[1] == 0xFF){
//check checksum of incoming packet
unsigned char checksum = CalculateChecksum(rxpacket);
if(rxpacket[get_length -1] == checksum){
if(txpacket[INSTRUCTION] == PING){
printf("Successful ping\n");
}
//successful read / ping
return length;
}
}
// didn't get packet, write out to port again
get_length = 0;
port.ClearPort();
port.WritePort(txpacket, length);
} else { //check time out status
if(Time.isTimeOut(packetStartTime, packetWaitTime)){
if(get_length == 0){
//printf("timed out\n");
} else {
//printf("rxpacket corrupt\n");
}
if(txpacket[INSTRUCTION] != BULK_READ){
fail_counter++;
get_length = 0;
packetStartTime = Time.getCurrentTime();
port.ClearPort();
port.WritePort(txpacket, length);
}
}
}
}
if(txpacket[INSTRUCTION] != BULK_READ){
// if it isn't bulkread, must return here
return length;
}
/*
for(int i = 0; i < num; i++){
int _id = txpacket[PARAMETER+(3*i)+2];
BulkData[_id].error = -1;
}
*/
return_length = get_length; // make a copy of the received length for return purposes
while(1){ // this loop is purely for bulkread
int i;
for(i = 0; i< get_length - 1; i++){ //find the header
if(rxpacket[i] == 0xFF && rxpacket[i+1] == 0xFF){
break;
} else if(i == (get_length - 2) && rxpacket[get_length - 1] == 0xFF){
break;
}
}
if(i == 0){ //header is at beginning of packet
// Check checksum
unsigned char checksum = CalculateChecksum(rxpacket);
//printf("rxpacket[ID]: %d\n", rxpacket[ID]); // for debugging
if(rxpacket[LENGTH + rxpacket[LENGTH]] == checksum){
for(int j = 0; j < (rxpacket[LENGTH]-2); j++){
//BulkData[rxpacket[ID]].table[BulkData[rxpacket[ID]].start_address + j] = rxpacket[PARAMETER + j];
// printf("j: %d, rxpacket: %d\n", j, rxpacket[PARAMETER + j]); // for debugging
info[count++] = rxpacket[PARAMETER + j];
}
//BulkData[rxpacket[ID]].error = (int)rxpacket[ERRBIT];
int cur_packet_length = LENGTH + 1 + rxpacket[LENGTH];
to_length = get_length - cur_packet_length;
// shift things after the first packet up to the beginning of the packet
for(int j = 0; j <= to_length; j++){
rxpacket[j] = rxpacket[j+cur_packet_length];
}
get_length = to_length;
num--;
} else {
//printf("rx corrupt\n");
for(int j = 0; j <= to_length - 2; j++){
rxpacket[j] = rxpacket[j+2];
}
to_length = get_length -= 2;
}
if(num == 0){ // got all of the packets
break;
} else if(get_length <= 6) {
if(num != 0){
// printf("rx corrupt\n");
}
break;
}
} else { // try to salvage packets
for(int j = 0; j < (get_length - i); j++){
rxpacket[j] = rxpacket[j+i];
}
get_length -= i;
}
}
if(BulkReadJointData){
// Sort into structs here
for(int i = 0; i<20; i++){
ReadData& rd = jointRead[i];
rd.d = info[buf];
rd.i = info[buf+1];
rd.p = info[buf+2];
rd.goal_pos = MakeWord(info[buf+4], info[buf+5]);
rd.max_speed = MakeWord(info[buf+6], info[buf+7]);
rd.torque_limit = MakeWord(info[buf+8], info[buf+9]);
rd.cur_pos = MakeWord(info[buf+10], info[buf+11]);
rd.cur_speed = MakeWord(info[buf+12], info[buf+13]);
rd.load = MakeWord(info[buf+14], info[buf+15]);
rd.registered = info[buf+18];
rd.moving = info[buf+20];
// 23 uchars per motor
buf = buf + 23;
}
}
return return_length;
}
return return_length;
}
