static void vis_vdot(OPS op,OPSIZE opsize)
{
OP v1,v2,dot = zero;
int16 delta = opsize==fp_f ? 2 : 4;
uint32 daddr = op[0].word;
uint32 v1addr = op[1].word;
int16 ix1 = INT16(op[2].word) * delta;
uint32 v2addr = op[3].word;
int16 ix2 = INT16(op[4].word) * delta;
int16 i,n = INT16(op[5].word);
if (n <= 0) return;
/* calculates dot = dot + v1[i]*v2[j] for incrementing i,j */
for (i=0; i<n; i++) {
v1 = ReadOp(v1addr, opsize);
if (opsize==fp_f) (void)fp_cvt(&v1,fp_f,fp_t); /* cvt to DBLE(v1) */
v2 = ReadOp(v2addr, opsize);
if (opsize==fp_f) (void)fp_cvt(&v2,fp_f,fp_t); /* cvt to DBLE(v2) */
(void)fp_exec(042,ACCUM, v1, v2); /* ACCU := v1 * v2 */
(void)fp_exec(006,&dot,dot,NOP); /* dot := dot + v1*v2 */
v1addr += ix1; /* forward to next array elements */
v2addr += ix2;
}
if (opsize==fp_f)
(void)vis_trunc(&dot,dot); /* truncate to SNGL(sumnrm) */
WriteOp(daddr, dot, opsize); /* write result */
}
void rcs_loadParams()
{
prm_add("S0 lower limit", PRM_FLAG_SIGNED,
"@c Servo Sets the lower limit of travel for S0 (default -200)", INT16(-200), END);
prm_add("S0 upper limit", PRM_FLAG_SIGNED,
"@c Servo Sets the upper limit of travel for S0 (default 200)", INT16(200), END);
prm_add("S1 lower limit", PRM_FLAG_SIGNED,
"@c Servo Sets the lower limit of travel for S1 (default -200)", INT16(-200), END);
prm_add("S1 upper limit", PRM_FLAG_SIGNED,
"@c Servo Sets the upper limit of travel for S1 (default 200)", INT16(200), END);
prm_add("Servo frequency", PRM_FLAG_ADVANCED,
"@c Servo Sets the PWM frequency of the servos (default 100)", UINT16(100), END);
int16_t lower, upper, freq;
prm_get("S0 lower limit", &lower, END);
prm_get("S0 upper limit", &upper, END);
rcs_setLimits(0, lower, upper);
prm_get("S1 lower limit", &lower, END);
prm_get("S1 upper limit", &upper, END);
rcs_setLimits(1, lower, upper);
prm_get("Servo frequency", &freq, END);
rcs_setFreq(freq);
}
static void vis_minmax(OPS op,OPSIZE opsize,t_bool domax,t_bool doabs)
{
OP v1,vmxmn,res;
int16 delta = opsize==fp_f ? 2 : 4;
uint32 mxmnaddr = op[0].word;
uint32 v1addr = op[1].word;
int16 ix1 = INT16(op[2].word) * delta;
int16 n = INT16(op[3].word);
int16 i,mxmn,sign;
uint16 subop = 020 | (opsize==fp_f ? 0 : 2);
if (n <= 0) return;
mxmn = 0; /* index of maxmin element */
vmxmn = ReadOp(v1addr,opsize); /* initialize with first element */
if (doabs) vis_abs(&vmxmn,opsize); /* ABS(v[1]) if requested */
for (i = 0; i<n; i++) {
v1 = ReadOp(v1addr,opsize); /* get v[i] */
if (doabs) vis_abs(&v1,opsize); /* build ABS(v[i]) if requested */
(void)fp_exec(subop,&res,vmxmn,v1); /* subtract vmxmn - v1[i] */
sign = GET_MSIGN(&res); /* !=0 if vmxmn < v1[i] */
if ((domax && sign) || /* new max value found */
(!domax && !sign)) { /* new min value found */
mxmn = i;
vmxmn = v1; /* save the new max/min value */
}
v1addr += ix1; /* point to next element */
}
res.word = mxmn+1; /* adjust to one-based FTN array */
WriteOp(mxmnaddr, res, in_s); /* save result */
}
static void vis_vpiv(OPS op, OPSIZE opsize)
{
OP s,v1,v2,v3;
int16 delta = opsize==fp_f ? 2 : 4;
uint32 saddr = op[0].word;
uint32 v1addr = op[1].word;
int16 ix1 = INT16(op[2].word) * delta;
uint32 v2addr = op[3].word;
int16 ix2 = INT16(op[4].word) * delta;
uint32 v3addr = op[5].word;
int16 ix3 = INT16(op[6].word) * delta;
int16 i, n = INT16(op[7].word);
int16 oplen = opsize==fp_f ? 0 : 2;
if (n <= 0) return;
s = ReadOp(saddr,opsize);
/* calculates v3[k] = s * v1[i] + v2[j] for incrementing i,j,k */
for (i=0; i<n; i++) {
v1 = ReadOp(v1addr, opsize);
(void)fp_exec(040+oplen, ACCUM, s ,v1); /* ACCU := s*v1 */
v2 = ReadOp(v2addr, opsize);
(void)fp_exec(004+oplen,&v3,v2,NOP); /* v3 := v2 + s*v1 */
WriteOp(v3addr, v3, opsize); /* write result */
v1addr += ix1; /* forward to next array elements */
v2addr += ix2;
v3addr += ix3;
}
}
static void em_1000geodpos(struct gps_state *gps)
{
double speed;
int bearing, solinv;
solinv = INT16(&packet[WD(10)]) & 0x5;
if (solinv) gps->fix &= ~0x3; /* do we know whether it is a 2D or 3D fix? */
else gps->fix |= 0x3;
gps->lat = INT32(&packet[WD(27)]) * 1.0e-8;
gps->lon = INT32(&packet[WD(29)]) * 1.0e-8;
gps->alt = INT32(&packet[WD(31)]) * 1.0e-2;
speed = ((unsigned int)INT32(&packet[WD(34)])) * 1.0e-2;
bearing = radtodeg(INT16(&packet[WD(36)]) * 1.0e-3);
if (bearing < 0) bearing += 2 * M_PI;
gps->bearing = radtodeg(bearing);
gps->spd_up = 0.0;
gps->spd_east = sin(bearing) * speed;
gps->spd_north = cos(bearing) * speed;
gps->time = conv_date(INT16(&packet[WD(21)]),
INT16(&packet[WD(20)]), INT16(&packet[WD(19)])) +
INT16(&packet[WD(24)])+(60*INT16(&packet[WD(23)])) +
(3600*INT16(&packet[WD(22)]));
gps->updated |= GPS_STATE_FIX | GPS_STATE_COORD | GPS_STATE_BEARING | GPS_STATE_SPEED;
}
static void em_decode(struct gps_state *gps)
{
if (packet_idx < 1) return;
#ifndef __arm__
fprintf(stderr, "receiving %x\n", packet[0]);
#endif
/* NMEA lines should start with a '$' */
if (packet[0] == 'E') {
/* recognize earthmate's 'EARTHA' message */
if (packet_idx >= 6 && memcmp(packet, "EARTHA", 6) == 0)
serial_send("EARTHA\r\n", 8);
return;
}
draw_activity(0);
/* verify checksum XXX */
switch(INT16(&packet[0])) {
case 1000: em_1000geodpos(gps); break;
case 1002: em_1002chsum(gps); break;
case 1003: em_1003sats(gps); break;
}
}
static void vis_movswp(OPS op, OPSIZE opsize, t_bool doswp)
{
OP v1,v2;
int16 delta = opsize==fp_f ? 2 : 4;
uint32 v1addr = op[0].word;
int16 ix1 = INT16(op[1].word) * delta;
uint32 v2addr = op[2].word;
int16 ix2 = INT16(op[3].word) * delta;
int16 i,n = INT16(op[4].word);
if (n <= 0) return;
for (i=0; i<n; i++) {
v1 = ReadOp(v1addr, opsize);
v2 = ReadOp(v2addr, opsize);
WriteOp(v2addr, v1, opsize); /* v2 := v1 */
if (doswp) WriteOp(v1addr, v2, opsize); /* v1 := v2 */
v1addr += ix1; /* forward to next array elements */
v2addr += ix2;
}
}
static void vis_vabs(OPS op, OPSIZE opsize)
{
OP v1;
int16 delta = opsize==fp_f ? 2 : 4;
uint32 v1addr = op[0].word;
int16 ix1 = INT16(op[1].word) * delta;
uint32 v2addr = op[2].word;
int32 ix2 = INT16(op[3].word) * delta;
int16 i,n = INT16(op[4].word);
if (n <= 0) return;
/* calculates v2[j] = ABS(v1[i]) for incrementing i,j */
for (i=0; i<n; i++) {
v1 = ReadOp(v1addr, opsize);
vis_abs(&v1,opsize); /* make absolute value */
WriteOp(v2addr, v1, opsize); /* write result */
v1addr += ix1; /* forward to next array elements */
v2addr += ix2;
}
}
/* handle the scalar/vector base ops */
static void vis_svop(uint32 subcode, OPS op, OPSIZE opsize)
{
OP v1,v2;
int16 delta = opsize==fp_f ? 2 : 4;
OP s = ReadOp(op[0].word,opsize);
uint32 v1addr = op[1].word;
int16 ix1 = INT16(op[2].word) * delta;
uint32 v2addr = op[3].word;
int16 ix2 = INT16(op[4].word) * delta;
int16 i, n = INT16(op[5].word);
uint16 fpuop = (uint16) (subcode & 060) | (opsize==fp_f ? 0 : 2);
if (n <= 0) return;
for (i=0; i<n; i++) {
v1 = ReadOp(v1addr, opsize);
(void)fp_exec(fpuop, &v2, s ,v1);
WriteOp(v2addr, v2, opsize);
v1addr += ix1;
v2addr += ix2;
}
}
static void em_1003sats(struct gps_state *gps)
{
double elv, azm;
int j, svn, nsats = INT16(&packet[WD(14)]);
gps->hdop = INT32(&packet[WD(11)]) * 1.0e-2;
for (j = 0; j < nsats; j++) {
svn = INT16(&packet[WD(15 + (3 * j))]);
elv = INT16(&packet[WD(17 + (3 * j))]) * 1.0e-4;
azm = INT16(&packet[WD(16 + (3 * j))]) * 1.0e-4;
if (elv < 0) elv = 0;
if (azm < 0) azm += 2 * M_PI;
new_sat(gps, svn, UNKNOWN_TIME, elv, azm, UNKNOWN_SNR, UNKNOWN_USED);
}
if (nsats)
gps->updated = GPS_STATE_SATS;
}
static void vis_vsmnm(OPS op,OPSIZE opsize,t_bool doabs)
{
uint16 fpuop;
OP v1,sumnrm = zero;
int16 delta = opsize==fp_f ? 2 : 4;
uint32 saddr = op[0].word;
uint32 v1addr = op[1].word;
int16 ix1 = INT16(op[2].word) * delta;
int16 i,n = INT16(op[3].word);
if (n <= 0) return;
/* calculates sumnrm = sumnrm + DBLE(v1[i]) resp DBLE(ABS(v1[i])) for incrementing i */
for (i=0; i<n; i++) {
v1 = ReadOp(v1addr, opsize);
if (opsize==fp_f) (void)fp_cvt(&v1,fp_f,fp_t); /* cvt to DBLE(v1) */
fpuop = (doabs && GET_MSIGN(&v1)) ? 022 : 002; /* use subtract for NRM && V1<0 */
(void)fp_exec(fpuop,&sumnrm, sumnrm, v1); /* accumulate */
v1addr += ix1; /* forward to next array elements */
}
if (opsize==fp_f)
(void)vis_trunc(&sumnrm,sumnrm); /* truncate to SNGL(sumnrm) */
WriteOp(saddr, sumnrm, opsize); /* write result */
}
static void em_1002chsum(struct gps_state *gps)
{
int j, status, svn, snr, used, valid;
int timestamp, datestamp, time;
#define EPOCHDIFF 315532800 /* difference between GPS and UNIX time */
#define LEAP_SECONDS 13 /* hardcoded, bad me, should get it from the receiver */
datestamp = INT16(&packet[WD(10)]) * 7 * 86400 + EPOCHDIFF;
timestamp = INT32(&packet[WD(11)]) + LEAP_SECONDS;
time = datestamp + timestamp;
for (j = 0; j < 12; j++) {
status = INT16(&packet[WD(15 + (3 * j))]);
used = status & 0x1;
valid = status & 0x4;
svn = INT16(&packet[WD(16 + (3 * j))]);
snr = INT16(&packet[WD(17 + (3 * j))]) / 4; /* scaling snr to 0-16 */
new_sat(gps, svn, valid ? time : UNKNOWN_TIME, UNKNOWN_ELV, UNKNOWN_AZM,
snr, used);
}
gps->updated = GPS_STATE_SIGNALS;
}
static inline UInt16 Tdecode16( void *msg_buf)
{
UInt16 toswap = *((UInt16*) msg_buf);
INT16(&toswap);
return toswap;
}
void USBUpdate()
{
int a,b;
// User Application USB tasks
if((USBDeviceState < CONFIGURED_STATE)||(USBSuspendControl==1)) return;
if(!HIDRxHandleBusy(USBOutHandle)) //Check if data was received from the host.
{
switch(ReceivedDataBuffer[0]) //Look at the data the host sent, to see what kind of application specific command it sent.
{
case 0x01:
a=EPPoll(*(int16*)&ReceivedDataBuffer[1]);
if(!USBBusy())
{
ToSendDataBuffer[0]=0x03;
if(a!=0)
ToSendDataBuffer[1]=0;
else
ToSendDataBuffer[1]=255;
USBWrite();
}
break;
case 0x02:
EPBufferSize=ReceivedDataBuffer[3];
for(a=0;a<EPBufferSize;a++)Set8(a,ReceivedDataBuffer[a+4]);
EPSend(*(int16*)&ReceivedDataBuffer[1]);
if(!USBBusy())
{
ToSendDataBuffer[0]=0x02;
USBWrite();
}
break;
case 0x06:
ToSendDataBuffer[1]=KismetExecuteEvent(((int16*)&ReceivedDataBuffer[1])[0],ReceivedDataBuffer[3]);
if(!USBBusy())
{
ToSendDataBuffer[0]=0x06;
USBWrite();
}
break;
case 0x07:
OperationEnabled=0;
if(!USBBusy())
{
ToSendDataBuffer[0]=0x07;
USBWrite();
}
break;
case 0x08:
OperationEnabled=0xff;
if(!USBBusy())
{
ToSendDataBuffer[0]=0x08;
USBWrite();
}
break;
case 0x03://EEPROM Write
MemoryBeginWrite(((int16*)&ReceivedDataBuffer[1])[0]);
MemoryWrite(ReceivedDataBuffer[3]);
MemoryEndWrite();
if(!USBBusy())
{
ToSendDataBuffer[0]=0x03;
USBWrite();
}
break;
case 0x04://EEPROM WRITE PAGE
MemoryBeginWrite(((int16*)&ReceivedDataBuffer[1])[0]);
for(a=0;a<32;a++)
MemoryWrite(ReceivedDataBuffer[3+a]);
MemoryEndWrite();
if(!USBBusy())
{
ToSendDataBuffer[0]=0x04;
USBWrite();
}
break;
case 0x05://EEPROM READ
if(!USBBusy())
{
MemoryBeginRead(((int16*)&ReceivedDataBuffer[1])[0]);
ToSendDataBuffer[0]=0x05;
ToSendDataBuffer[1]=MemoryReadInt8();
MemoryEndRead();
USBWrite();
}
break;
case 0x40://READ TIME
if(!USBBusy())
{
ToSendDataBuffer[0]=0x40;
ToSendDataBuffer[1]=RTCSecond;
ToSendDataBuffer[2]=RTCMinute;
ToSendDataBuffer[3]=RTCHour;
INT16(ToSendDataBuffer[4])=RTCDay;
USBWrite();
}
break;
case 0x41://WRITE TIME
RTCSecond =ReceivedDataBuffer[1];
RTCMinute =ReceivedDataBuffer[2];
RTCHour =ReceivedDataBuffer[3];
RTCDay =INT16(ReceivedDataBuffer[4]);
if(!USBBusy())
{
ToSendDataBuffer[0]=0x41;
USBWrite();
}
break;
case 0x50://SET TIMER
SetTimer(ReceivedDataBuffer[1],ReceivedDataBuffer[2],INT16(ReceivedDataBuffer[3]));
if(!USBBusy())
{
ToSendDataBuffer[0]=0x50;
USBWrite();
}
break;
}
if(!HIDTxHandleBusy(USBInHandle))
{
USBInHandle = HIDTxPacket(HID_EP,(BYTE*)&ToSendDataBuffer[0],64);
}
ReceivedDataBuffer[0]=0;
//Re-arm the OUT endpoint for the next packet
USBOutHandle = HIDRxPacket(HID_EP,(BYTE*)&ReceivedDataBuffer,64);
}
}
