uint8_t avsHeader::receiveData(uint32_t *cmd, uint32_t *frame,uint32_t *payload_size,uint8_t *payload)
{
SktHeader header;
memset(&header,0,sizeof(header));
int rx;
rx=rxData(sizeof(header),(uint8_t *)&header);
*cmd=header.cmd;
*payload_size=header.payloadLen;
*frame=header.frame;
if(header.magic!=(uint32_t)MAGGIC)
{
printf("[avsProxy]Wrong magic %x/%x\n",header.magic,MAGGIC);
return 0;
}
if(header.payloadLen)
{
int togo=header.payloadLen;
return rxData(togo,payload);
}
return 1;
}
int DLS::setOutputFilter (int nsamples, int nspikes, int nerrors)
{
// Request Current Config
char write_data[] = "s0fi\r\n";
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
// Read Current Config
char read_data[] = "g0fi+aa+bb+cc\r\n";
serialRead(read_data);
//
// Check for error indicator (@) and parse error value.
if (read_data[2]=='@') {
char errcode_str [] = "000";
sprintf (errcode_str, "%*s", 3, read_data+4);
return(-1 * atoi (errcode_str));
}
char aa_str [] = "aa";
aa_str[0] = read_data [5];
aa_str[1] = read_data [6];
char bb_str [] = "bb";
bb_str[0] = read_data [8];
bb_str[1] = read_data [9];
char cc_str [] = "cc";
cc_str[0] = read_data [11];
cc_str[1] = read_data [12];
if ((nsamples < 0) || (nsamples > 32))
nsamples = atoi (aa_str);
if (nspikes < 0)
nspikes = atoi (bb_str);
if (nerrors < 0)
nerrors = atoi (cc_str);
debug_print("samples %i\n", nsamples);
debug_print("spikes %i\n", nspikes);
debug_print("errors %i\n", nerrors);
if (2*nspikes+nerrors > 0.4 * nsamples) {
fprintf(stderr, "ERROR: Make sure that (2*nspikes + nerrors) <= nsamples\n");
nspikes = atoi (bb_str);
nerrors = atoi (cc_str);
nsamples = atoi (aa_str);
}
char write_data2[] = "s0fi+aa+bb+cc\r\n";
sprintf(write_data2, "s0fi+%02i+%02i+%02i\r\n",nsamples,nspikes,nerrors);
serialWrite(write_data2, sizeof(write_data2)/sizeof(write_data[0]));
int status = rxData();
if (status < 0)
printErrorMsg(status);
return(status);
}
int DLS::laserOff () {
char write_data[] = "s0p\r\n";
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
int status = rxData();
if (status < 0)
printErrorMsg(status);
return(status);
}
int DLS::readTemperature () {
char write_data[] = "s0t\r\n";
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
int temperature = rxData();
if (temperature < 0)
printErrorMsg(temperature);
return (temperature);
}
int DLS::readTracking ()
{
int status = rxData ();
int distance = status;
// Failure
if (status < 0)
printErrorMsg(status);
return (distance);
}
int DLS::stopTracking () {
char write_data[] = "s0c\r\n";
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
int status = rxData();
if (status < 0)
printErrorMsg(status);
return (status);
}
int DLS::startTracking () {
char write_data[] = "s0h\r\n";
int len = sizeof(write_data)/sizeof(write_data[0]);
serialWrite(write_data, len);
int status = rxData();
if (status < 0)
printErrorMsg(status);
return (status);
}
int DLS::getSignalQuality () {
char write_data[] = "s0m+0\r\n";
int len = sizeof(write_data)/sizeof(write_data[0]);
serialWrite(write_data, len);
int status = rxData();
if (status < 0)
printErrorMsg(status);
int quality = status;
return (quality);
}
int DLS::saveConfiguration()
{
char write_data[] = "s0s\r\n";
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
int status = rxData();
if (status < 0)
printErrorMsg(status);
return(status);
}
static int FillReadBuffer(unsigned char *readBuf, unsigned char *readPtr, int bufSize, int bytesLeft, chanend rxChan)
{
int nRead;
/* move last, small chunk from end of buffer to start, then fill with new data */
memmove(readBuf, readPtr, bytesLeft);
nRead = rxData(rxChan, bufSize - bytesLeft, readBuf + bytesLeft);
/* zero-pad to avoid finding false sync word after last frame (from old data in readBuf) */
if (nRead < bufSize - bytesLeft)
memset(readBuf + bytesLeft + nRead, 0, bufSize - bytesLeft - nRead);
return nRead;
}
int DLS::startTrackingDelay (int delay) {
char write_data[] = "s0h+xxx\r\n";
sprintf(write_data,"s0h+%03i\r\n",delay);
int len = sizeof(write_data)/sizeof(write_data[0]);
serialWrite(write_data, len);
int status = rxData();
if (status < 0)
printErrorMsg(status);
return (status);
}
/*
* ======== USBCDCMOUSE_receiveData ========
*/
unsigned int USBCDCMOUSE_receiveData(unsigned char *pStr,
unsigned int length,
unsigned int timeout)
{
unsigned int retValue = 0;
unsigned int key;
if (USB_getConnectionInformation() & USB_ENUMERATED) {
key = GateMutex_enter(gateRxSerial);
retValue = rxData(pStr, length, timeout);
GateMutex_leave(gateRxSerial, key);
}
return (retValue);
}
int DLS::setMeasuringCharacteristic (int a, int b) {
char write_data[] = "s0uc+a+b\r\n";
sprintf(write_data,"s0uc+%1i+%1i\r\n",a,b);
int len = sizeof(write_data)/sizeof(write_data[0]);
serialWrite(write_data, len);
int status = rxData();
if (status < 0)
printErrorMsg(status);
status |= saveConfiguration();
return (status);
}
int DLS::setOffset(int offset)
{
char write_data[] = "s0uof+xxxxxxxx\r\n";
sprintf(write_data, "s0uof+%+08i\r\n", offset);
debug_print("%s", write_data);
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
int status = rxData();
if (status < 0)
printErrorMsg(status);
debug_print("%i", status);
status |= saveConfiguration();
debug_print("%i", status);
return(status);
}
int DLS::setGain(float gain)
{
gain = fabs(gain);
int gain_numer=0;
int gain_denom=127;
float remainder;
float remainder_min;
// find the best fractional representation of the given gain,
// using the 7 bits we have available (max=127)
for (int i=0; i<128; i++) {
for (int j=1; j<128; j++) {
float fraction = ((float) i) / (float (j));
remainder = fabs(fraction-gain);
if (i==0 && j==1)
remainder_min=remainder;
if (remainder < remainder_min) {
remainder_min=remainder;
gain_numer = i;
gain_denom = j;
}
}
}
char write_data[] = "s0uga+xxxxxxxx+yyyyyyyy\r\n";
sprintf(write_data, "s0uga+%08i+%08i\r\n", gain_numer, gain_denom);
debug_print("%s\n", write_data);
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
int status = rxData();
if (status < 0)
printErrorMsg(status);
status |= saveConfiguration();
return(status);
}
int DLS::measureDistance () {
if (userCalibrated_) {
char write_data[] = "s0ug\r\n";
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
debug_print("%s\n", "User calibrated");
}
else {
char write_data[] = "s0g\r\n";
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
}
int status = rxData ();
int distance = status;
if (status < 0)
printErrorMsg(status);
return distance;
}
/* called by the main application to poll for events */
void usbll_poll( void )
{
int epNum ;
unsigned gstate ;
unsigned isr = UDP->UDP_ISR ;
if( isr != prevISR ){
prevISR = isr ;
}
if( isr & AT91C_UDP_EPINT0 ){
UDP->UDP_ICR = AT91C_UDP_EPINT0 ;
}
if( isr & AT91C_UDP_WAKEUP ){
DEBUGMSG( "UDP_WAKEUP\r\n" );
UDP->UDP_ICR = AT91C_UDP_WAKEUP ;
}
if( isr & AT91C_UDP_RXRSM ){
DEBUGMSG( "UDP_RXRSM\r\n" );
UDP->UDP_ICR = AT91C_UDP_RXRSM ;
}
if( isr & AT91C_UDP_EXTRSM ){
DEBUGMSG( "UDP_EXTRSM\r\n" );
UDP->UDP_ICR = AT91C_UDP_EXTRSM ;
}
if( isr & AT91C_UDP_SOFINT ){
// DEBUGMSG( "UDP_SOFINT\r\n" );
UDP->UDP_ICR = AT91C_UDP_SOFINT ;
}
if( isr & AT91C_UDP_ENDBUSRES ){
// write( DEFUART, "GLBSTATE == " ); writeHex( DEFUART, UDP->UDP_GLBSTATE ); write( DEFUART, "\r\n" );
UDP->UDP_FADDR = AT91C_UDP_FEN ;
UDP_CLEAREPFLAGS(&UDP->UDP_GLBSTATE,AT91C_UDP_CONFG|AT91C_UDP_FADDEN);
UDP->UDP_RSTEP = 0xffffffff ;
UDP->UDP_RSTEP = 0 ;
UDP_SETEPFLAGS(UDP->UDP_CSR + 0, AT91C_UDP_EPEDS);
UDP_CLEAREPFLAGS(UDP->UDP_CSR + 0, AT91C_UDP_DIR);
UDP->UDP_ICR = AT91C_UDP_ENDBUSRES ;
abortTransmitters();
// write( DEFUART, "UDP_ENDBUSRES 0x" );writeHex( DEFUART, UDP->UDP_GLBSTATE ); write( DEFUART, "\r\n" );
}
gstate = UDP->UDP_GLBSTATE;
for( epNum = 0 ; epNum < UDP_MAXENDPOINTS ; epNum++ ){
unsigned int dStatus = UDP->UDP_CSR[epNum];
if( dStatus & AT91C_UDP_TXCOMP ){
transmitComplete(epNum);
} // transmit completed
if( dStatus & AT91C_UDP_RXSETUP ){
unsigned char setupBuf[16];
unsigned short rxLength = ( dStatus >> 16 ) & 0x7FF ;
if( sizeof(struct S_usb_request) <= rxLength ){
struct S_usb_request const *rxSetup = (struct S_usb_request const *)setupBuf ;
rxLength = sizeof(*rxSetup);
unsigned i ;
unsigned char volatile *const fifo = (unsigned char *)&UDP->UDP_FDR[epNum];
for( i = 0 ; i < rxLength ; i++ ){
setupBuf[i] = *fifo ;
}
if( rxSetup->bmRequestType & 0x80) {
UDP_SETEPFLAGS(UDP->UDP_CSR + epNum, AT91C_UDP_DIR);
}
UDP_CLEAREPFLAGS(UDP->UDP_CSR + epNum, AT91C_UDP_RXSETUP);
if( 0 != (UDP->UDP_CSR[epNum] & (AT91C_UDP_TXPKTRDY|AT91C_UDP_TXCOMP|AT91C_UDP_STALLSENT)) ){
write( DEFUART, "rxSetup: txPending\r\n" );
}
if( setupCallback ){
completeTransmit(epNum);
setupCallback( setupOpaque, setupBuf, rxLength );
}
}
else {
write( DEFUART, "rxSetup too small 0x" ); writeHexChar( DEFUART, rxLength ); write( DEFUART, "\r\n" );
UDP_CLEAREPFLAGS(UDP->UDP_CSR + epNum, AT91C_UDP_RXSETUP);
}
}
if( dStatus & AT91C_UDP_STALLSENT ){
UDP_CLEAREPFLAGS(UDP->UDP_CSR + epNum, AT91C_UDP_STALLSENT);
}
unsigned int rxMask = dStatus & (AT91C_UDP_RX_DATA_BK0|AT91C_UDP_RX_DATA_BK1);
if( rxMask ){
rxData(epNum,rxMask);
} // received something
}