// Led status while attached
void LedTasks(void)
{
if (status.ledOverride >= 0)
{
LED_SET(status.ledOverride);
return;
}
else if (status.actionCountdown)
{
LED_SET(LED_RED);
}
else
{
if (inactive == 0)
{
if (status.batteryFull) {LED_SET(LED_GREEN);} // full - flushed
else {LED_SET(LED_YELLOW);} // charging - flushed
}
else
{
LED_SET(LED_RED); // unflushed
}
}
return;
}
static int __s3c4510b_start_xmit(struct sk_buff *skb, struct net_device *dev)
{
int len;
u32 addr;
struct eth_priv *priv = (struct eth_priv *) dev->priv;
// _DPRINTK("entered with dev = 0x%08x", (unsigned int)dev);
len = skb->len < ETH_ZLEN ? ETH_ZLEN : skb->len;
dev->trans_start = jiffies;
if ( unlikely( priv->m_curTX_FD->m_frameDataPtr.bf.owner)) {
_EPRINTK("Ethernet TX Frame. CPU not owner");
return -EBUSY;
}
/* this needs to be word aligned for the BDMA -- round down */
addr = ((u32)skb->data & ~0x3) | CACHE_DISABLE_MASK;
priv->m_curTX_FD->m_frameDataPtr.bf.dataPtr = addr;
/* Set TX Frame flags */
priv->m_curTX_FD->m_opt.bf.widgetAlign = (u32)skb->data - addr; /* compenstate for alignment */
priv->m_curTX_FD->m_opt.bf.frameDataDir = 1;
priv->m_curTX_FD->m_opt.bf.littleEndian = 1;
priv->m_curTX_FD->m_opt.bf.macTxIrqEnbl = 1;
priv->m_curTX_FD->m_opt.bf.no_crc = 0;
priv->m_curTX_FD->m_opt.bf.no_padding = 0;
/* Set TX Frame length */
priv->m_curTX_FD->m_status.bf.len = len;
priv->m_curTX_FD->skb = skb;
/* Change ownership to BDMA */
priv->m_curTX_FD->m_frameDataPtr.bf.owner = 1;
/* Change the Tx frame descriptor for next use */
priv->m_curTX_FD = priv->m_curTX_FD->m_nextFD;
LED_SET(3);
/* Enable MAC and BDMA Tx control register */
outl( ETH_BTxBRST | /* BDMA Tx burst size 16 words */
ETH_BTxMSL110 | /* BDMA Tx wait to fill 6/8 of the BDMA */
ETH_BTxSTSKO | /* BDMA Tx interrupt(Stop) on non-owner TX FD */
ETH_BTxEn, /* BDMA Tx Enable */
REG_BDMATXCON);
outl( ETH_EnComp | /* interrupt when the MAC transmits or discards packet */
ETH_TxEn | /* MAC transmit enable */
ETH_EnUnder | /* interrupt on Underrun */
ETH_EnNCarr | /* interrupt on No Carrier */
ETH_EnExColl | /* interrupt if 16 collision occur */
ETH_EnLateColl | /* interrupt if collision occurs after 512 bit times(64 bytes times) */
ETH_EnTxPar, /* interrupt if the MAC transmit FIFO has a parity error */
REG_MACTXCON);
return 0;
}
void Aufgabe16()
{
// Alle LEDs aus
SET(P4OUT, (BIT0 | BIT1 | BIT2));
powerState = LPM_4;
// Taster 0 interruptfähig schalten
CLEAR(P1IFG, TASTE0);
// Flanke auf 0 -> Interrupt beim drücken (statt loslassen)
// fürdie invertierten Taster entspricht dies einer HF
CLEAR(P1IES, TASTE0);
SET(P1IE, TASTE0);
// Interrupts global einschalten
_bis_SR_register(GIE);
while(true) {
if( powerState == ACTIVE ) {
// Ampel
LED_SET(LED_GELB);
} else if ( powerState == LPM_4 ) {
LED_OFF;
LPM4;
}
}
}
__interrupt void TIMERB0_ISR (void)
{
LED_SET(LED_ROT);
wait_ms(500);
LED_OFF;
// Clears the interrupt flag
CLEAR(TBCCTL0, CCIFG);
}
void LOCAL_CHECK_TIMEOUT(unsigned short timeout)
{
if (timeout == 0)
{
#ifdef __DEBUG
LED_SET(LED_MAGENTA);
while(1);
#endif
DebugReset(I2C_ERR);
}
}
void sp_send(const uint _debug) {
if (sp_fd >= 0) {
if (strlen(sp_tx_buf) > 0) {
LCD_DEBUG(_debug,"- sp_tx_buf = [%s]\tbefore send\n", sp_tx_buf);
LED_SET(LED_LCD,1);
write(sp_fd, sp_tx_buf, strlen(sp_tx_buf));
}
memset(sp_tx_buf, 0, sizeof (sp_tx_buf));
LCD_DEBUG(_debug,"- sp_tx_buf = [%s]\tafter send\n", sp_tx_buf);
}
}
// For release hardware this can help identify crash sources
void DebugReset(SwResetReason_t reason)
{
LED_SET(LED_MAGENTA);
DelayMs(3000);
switch (reason){
case (OTHER_INT): LED_SET(LED_OFF); break;
case (STACK_ERR): LED_SET(LED_MAGENTA); break;
case (ADDR_ERR): LED_SET(LED_RED); break;
case (MATH_ERR): LED_SET(LED_BLUE); break;
case (BT_ERR): LED_SET(LED_CYAN); break;
case (I2C_ERR): LED_SET(LED_GREEN); break;
case (SW_WDT): LED_SET(LED_YELLOW); break;
default: LED_SET(LED_WHITE); break;
}
DelayMs(3000);
Reset();
}
void svrParseRfInfo(char* res, P_GWS_KPI pKpi, int nId) {
char* begin = NULL;
char* sep = NULL;
char* end = NULL;
int i = 0, nParsed = 0, nRest = 0;
// printf("\n***[%s]***\n",res);
for (i = 0; i < MAX_GWS_VAR; i++) {
if ((begin = strstr(res, gwsVars[i].m_title)) != NULL) {
// printf("\n@1 Info:keyword[%s] found at %d\n",gwsVars[i].m_title,begin);
if ((sep = strstr(begin, " #")) > begin) {
sep = strstr(sep, "- ");
} else sep = strstr(begin, ":");
if (sep > begin) {
// printf("\n@2 Info:seperator <:> found at %d\n",sep);
if ((end = strstr(sep, "\n")) != NULL) {
// printf("\n@3 Info:\\n found at %d\n",end);
*end = 0x00;
SetKpiValue(pKpi, nId, gwsVars[i].m_index, sep + 2);
nParsed ++;
nRest = strlen(end + 1);
memmove(begin, end + 1, nRest + 1);
// printf("{%s = ",gwsVars[i].m_title);
// printf("%s}\n",VB_AsString(&gwsVars[i].m_value));
// printf("\n@4 After erase varbind <%s>\n",res);
}
}
}
}
int rest = strlen(res);
if (rest > 32) {
memmove(res, res + rest - 32, 32);
res[32] = 0;
}
if (nParsed) {
LED_SET(LED_RADIO,0);
}
}
// Exceptions and un-handled interrupts
void __attribute__((interrupt,auto_psv)) _DefaultInterrupt(void)
{
// Set reset reason and call reset function
SwResetReason_t reason = OTHER_INT;
if (INTCON1bits.STKERR) reason = STACK_ERR;
else if (INTCON1bits.ADDRERR) reason = ADDR_ERR;
else if (INTCON1bits.MATHERR) reason = MATH_ERR;
INTCON1 = 0;
#ifndef __DEBUG
DebugReset(reason);
#else
{
uint8_t usb_entry, usb_now;
usb_entry = usb_now = USB_BUS_SENSE;
// Magenta LED
LED_SET(LED_MAGENTA);
// Wait till usb changes state
while(usb_now == usb_entry){usb_now = USB_BUS_SENSE;}
// Break here, (dis)connect usb and step into the faulty code
Nop(); Nop(); Nop();
}
#endif
}
int Pop_queue_head(const uint _debug,char* rx_buf) {
int i,head = sp_rx_queue.first;
if (NULL == rx_buf || !sp_rx_queue.dirty) return 0;
// LCD_DEBUG(_debug," head = %d ",head);
for (i = 0; i < SP_QUEUE_SIZE; i ++) {
if (If_cache_ready(&sp_rx_queue.queue[head])) {
int len = sp_rx_queue.queue[head].leng;
memcpy(rx_buf,sp_rx_queue.queue[head].buff,sp_rx_queue.queue[head].leng);
rx_buf[sp_rx_queue.queue[head].leng] = 0;
LCD_DEBUG(_debug," --- <%d> [%s]",head, rx_buf);
sp_rx_queue.queue[head].leng = 0;
sp_rx_queue.queue[head].flag = SP_RX_CACHE_EMPTY;
LED_SET(LED_LCD,0);
return len;
} else {
// LCD_DEBUG(_debug,"[%d,f=%x,l=%d]",head,sp_rx_queue.queue[head].flag,sp_rx_queue.queue[head].leng);
head ++;
head %= SP_QUEUE_SIZE;
}
}
// LCD_DEBUG(_debug,"none\n");
sp_rx_queue.dirty = false;
return 0;
}
uint8_t wibo_run(void)
{
uint8_t isLeave=0;
uint8_t isStay=0;
unsigned long timeout = WIBO_TIMEOUT;
while(!isLeave) {
#if !defined(NO_LEDS)
LED_CLR(PROGLED);
#endif
if (!(isStay))
{
while(!(wibo_available()) && (timeout--)) _delay_ms(1); // minimum frame time @ 250kbps ~ 2ms.
if (!(wibo_available())) // no packets received, bye bye!
{
isLeave=1;
return isLeave;
}
}
else
{
while(!(wibo_available())); // wait for next packet
}
trx_reg_write(RG_IRQ_STATUS, TRX_IRQ_RX_END); /* clear the flag */
trx_frame_read(rxbuf.data, sizeof(rxbuf.data) / sizeof(rxbuf.data[0]),
&tmp); /* dont use LQI, write into tmp variable */
#if !defined(NO_LEDS)
LED_SET(PROGLED);
/* light as long as actions are running */
#endif
switch (rxbuf.hdr.cmd)
{
case P2P_PING_REQ:
isStay=1;
if (0 == deaf)
{
pingrep.hdr.dst = rxbuf.hdr.src;
pingrep.hdr.seq++;
pingrep.crc = datacrc;
trx_reg_write(RG_TRX_STATE, CMD_TX_ARET_ON);
/* no need to make block atomic since no IRQs are used */
TRX_SLPTR_HIGH()
;
TRX_SLPTR_LOW()
;
trx_frame_write(sizeof(p2p_ping_cnf_t) + sizeof(BOARD_NAME) + 2,
(uint8_t*) &pingrep);
/*******************************************************/
#if defined(_DEBUG_SERIAL_)
printf("Pinged by 0x%04X"EOL, rxbuf.hdr.src);
#endif
#if defined(TRX_IF_RFA1)
while (!(trx_reg_read(RG_IRQ_STATUS) & TRX_IRQ_TX_END))
;
trx_reg_write(RG_IRQ_STATUS, TRX_IRQ_TX_END); /* clear the flag */
#else
while (!(trx_reg_read(RG_IRQ_STATUS) & TRX_IRQ_TRX_END))
;
#endif /* defined(TRX_IF_RFA1) */
trx_reg_write(RG_TRX_STATE, CMD_RX_AACK_ON);
} /* (0 == deaf) */
break;
case P2P_WIBO_TARGET:
isStay=1;
target = rxbuf.wibo_target.targmem;
#if defined(_DEBUG_SERIAL_)
printf("Set Target to %c"EOL, target);
#endif
break;
case P2P_WIBO_RESET:
isStay=1;
#if defined(_DEBUG_SERIAL_)
printf("Reset"EOL);
#endif
addr = SPM_PAGESIZE; /* misuse as counter */
ptr = pagebuf;
do
{
*ptr++ = 0xFF;
} while (--addr);
addr = 0;
datacrc = 0;
pagebufidx = 0;
deaf = 0;
break;
case P2P_WIBO_ADDR:
isStay=1;
#if defined(_DEBUG_SERIAL_)
printf("Set address: 0x%08lX"EOL, rxbuf.wibo_addr.address);
#endif
addr = rxbuf.wibo_addr.address;
pagebufidx = 0;
break;
case P2P_WIBO_DATA:
isStay=1;
#if defined(_DEBUG_SERIAL_)
printf("Data[%d]", rxbuf.wibo_data.dsize);
uint8_t len = rxbuf.wibo_data.dsize;
if (len > 10) len = 10;
for(uint8_t j=0;j<len;j++)
{
printf(" %02X", rxbuf.wibo_data.data[j]);
}
if (len != rxbuf.wibo_data.dsize)
printf("...");
printf(EOL);
#endif
tmp = rxbuf.wibo_data.dsize;
ptr = rxbuf.wibo_data.data;
do
{
datacrc = _crc_ccitt_update(datacrc, *ptr);
pagebuf[pagebufidx++] = *ptr;
if (pagebufidx >= PAGEBUFSIZE)
{
/* LED off to save current and avoid flash corruption
* because of possible voltage drops
*/
#if !defined(NO_LEDS)
LED_CLR(PROGLED);
#endif
if (target == 'F') /* Flash memory */
{
boot_program_page(addr, pagebuf);
}
else if (target == 'E')
{
/* not implemented */
}
else
{
/* unknown target, dry run */
}
/* also for dry run! */
addr += SPM_PAGESIZE;
pagebufidx = 0;
}
ptr++;
} while (--tmp);
break;
#if defined(WIBO_FLAVOUR_BOOTLUP)
case P2P_WIBO_BOOTLUP:
isStay=1;
bootlup();
break;
#endif
case P2P_WIBO_FINISH:
isStay=1;
#if defined(_DEBUG_SERIAL_)
printf("Finish"EOL);
#endif
if (target == 'F') /* Flash memory */
{
boot_program_page(addr, pagebuf);
}
else if (target == 'E')
{
/* not implemented */
}
else
{
/* unknown target, dry run */
}
/* also for dry run! */
addr += SPM_PAGESIZE;
pagebufidx = 0;
break;
case P2P_WIBO_EXIT:
#if defined(_DEBUG_SERIAL_)
printf("Exit"EOL);
#endif
#if !defined(NO_LEDS)
LED_CLR(PROGLED);
#endif
isLeave=1;
break;
case P2P_WIBO_DEAF:
isStay=1;
deaf = 1;
break;
default:
/* unknown or unhandled command */
if (!(isStay)) {
if (!(timeout--)) {
isLeave = 1;
}
}
break;
}; /* switch (rxbuf.hdr.cmd) */
}
return isLeave;
}
void wibo_init(uint8_t channel, uint16_t pan_id, uint16_t short_addr, uint64_t ieee_addr)
{
#if defined(WIBO_FLAVOUR_KEYPRESS) || defined(WIBO_FLAVOUR_MAILBOX)
uint8_t run_bootloader = 0;
#endif
/* only stay in bootloader if key is pressed */
#if defined(WIBO_FLAVOUR_KEYPRESS)
#if defined(NO_KEYS)
#error "No Keys defined for WIBO_FLAVOUR_KEYPRESS"
#endif
KEY_INIT();
if(KEY_GET() != 0)
{
run_bootloader = 1;
}
#endif /* defined(WIBO_FLAVOUR_KEYPRESS) */
#if defined(WIBO_FLAVOUR_MAILBOX)
#if !defined(WIBO_FLAVOUR_MAILBOX_REGISTER) || !defined(WIBO_FLAVOUR_MAILBOX_CODE)
#error "WIBO_FLAVOUR_MAILBOX not defined correctly"
#endif
if(WIBO_FLAVOUR_MAILBOX_REGISTER == WIBO_FLAVOUR_MAILBOX_CODE)
{
run_bootloader = 1;
}
//WIBO_MAILBOX_CLR();
#endif /* defined(WIBO_FLAVOUR_MAILBOX) */
#if defined(WIBO_FLAVOUR_KEYPRESS) || defined(WIBO_FLAVOUR_MAILBOX)
if(run_bootloader == 0)
{
app();
}
#endif
#if !defined(NO_LEDS)
LED_INIT();
LED_SET(PROGLED);
#endif
nodeconfig.channel=channel;
nodeconfig.pan_id=pan_id;
nodeconfig.short_addr=short_addr;
nodeconfig.ieee_addr = ieee_addr;
trx_io_init(DEFAULT_SPI_RATE);
TRX_RESET_LOW();
TRX_SLPTR_LOW();
TRX_RESET_HIGH();
#if defined(DI_TRX_IRQ)
DI_TRX_IRQ();
#endif
trx_reg_write(RG_TRX_STATE, CMD_FORCE_TRX_OFF);
#if (RADIO_TYPE == RADIO_AT86RF230A) || (RADIO_TYPE == RADIO_AT86RF230B)
trx_reg_write(RG_PHY_TX_PWR, 0x80); /* set TX_AUTO_CRC bit, and TX_PWR = max */
#else
trx_reg_write(RG_TRX_CTRL_1, 0x20); /* set TX_AUTO_CRC bit */
#endif
/* setup network addresses for auto modes */
pingrep.hdr.pan = nodeconfig.pan_id;
pingrep.hdr.src = nodeconfig.short_addr;
trx_set_panid(nodeconfig.pan_id);
trx_set_shortaddr(nodeconfig.short_addr);
/* use register write to save code space, overwrites Bits CCA_REQUEST CCA_MODE[1] CCA_MODE[0]
* which is accepted
*/
trx_reg_write(RG_PHY_CC_CCA, nodeconfig.channel);
#if RADIO_TYPE == RADIO_AT86RF212
/* reset value, BPSK-40 */
/* trx_reg_write(RG_TRX_CTRL_2, 0x24); */
/* +5dBm acc. to datasheet AT86RF212 table 7-15 */
trx_reg_write(RG_PHY_TX_PWR, 0x84);
#endif /* RADIO_TYPE == RADIO_AT86RF212 */
trx_reg_write(RG_CSMA_SEED_0, nodeconfig.short_addr); /* some seeding */
trx_reg_write(RG_TRX_STATE, CMD_RX_AACK_ON);
trx_reg_write(RG_IRQ_STATUS, TRX_IRQ_RX_END); /* clear the flag */
#if defined(_DEBUG_SERIAL_)
void sendchar(char c);
static FILE usart_stdio = FDEV_SETUP_STREAM(sendchar, NULL, _FDEV_SETUP_WRITE);
stdout = stderr = &usart_stdio;
printf("WIBO Bootlapp Serial Debug"EOL);
printf("PANID=%04X SHORTADDR=%04X CHANNEL=%d"EOL,
nodeconfig.pan_id, nodeconfig.short_addr, nodeconfig.channel);
#endif
}
void SettingCommandMode(void)
{
unsigned char run = TRUE;
unsigned char print_all = TRUE;
char* line;
while(run)
{
// Print all settings
if(print_all == TRUE)
{
unsigned short val;
unsigned char i;
print_all = FALSE;
// Print all settings
PrintConst("\r\nSettings:\r\n");
PrintConst("Device:");
PrintConst(Byte2Hex(settings.deviceId.val8[3]));
PrintConst(Byte2Hex(settings.deviceId.val8[2]));
PrintConst(Byte2Hex(settings.deviceId.val8[1]));
PrintConst(Byte2Hex(settings.deviceId.val8[0]));
PrintConst("\r\n");
for(i=0;i<(sizeof(setting_val)/sizeof(setting_vals_t));i++)
{
PrintConst(setting_val[i].name);
val = 0;
if (setting_val[i].len == 0) {PrintConst("\r\n");continue;}
else if (setting_val[i].len == 1) {val = ((unsigned char*)setting_val[i].address)[0];}
else if(setting_val[i].len == 2) {val = (((unsigned short)((unsigned char*)setting_val[i].address)[1])<<8)+((unsigned char*)setting_val[i].address)[0];}
PrintConst(" = ");
PrintUint(val);
PrintConst("\r\n");
}
PrintConst("Commands:save | defaults | renew | reset | sample | stream | exit (esc)\r\n");
}
// Get a uart line
LED_SET(LED_RED);
COMM_CLEAR_ERRS(); // Check uart errors
line = UartGetLine();
// Inspect line
if (line == NULL)
run = FALSE; // Exit
else
{
unsigned char i = 0;
LED_SET(LED_OFF);
// Check commands first
if(line[0] == '\0')
{
print_all = TRUE;
continue;
}
else if (strncasecmp(line,"save",4) == 0)
{
SettingsSave();
print_all = TRUE;
PrintConst("\r\nSaved.\r\n");
continue;
}
else if (strncasecmp(line,"defaults",8) == 0)
{
SetttingsAction(SETTINGS_CLEAR);
print_all = TRUE;
PrintConst("\r\nDone.\r\n");
continue;
}
else if (strncasecmp(line,"renew",5) == 0)
{
SetttingsAction(SETTINGS_RESET_ALL);
print_all = TRUE;
PrintConst("\r\nDone.\r\n");
continue;
}
else if (strncasecmp(line,"exit",4) == 0)
{
PrintConst("\r\nOk.\r\n");
run = 0;
continue;
}
else if (strncasecmp(line,"sample",6) == 0)
{
SampleSensors();
SampleChannel(PIR_CHANNEL);
PrintSensorValues();
continue;
}
else if (strncasecmp(line,"stream",6) == 0)
{
RunStreamer();
PrintConst("\f");
print_all = TRUE;
continue;
}
else if (strncasecmp(line,"reset",5) == 0)
{
PrintConst("\r\nOk.\r\n");
LED_SET(LED_OFF);
Reset();
}
// Now compare to every named variable in settings
else
{
for(i=0;i<num_of_settings;i++)
{
unsigned char len = strlen(setting_val[i].name);
if (strncasecmp(line,setting_val[i].name,len) == 0)
{
unsigned long temp = my_atoi(line + len);
if(temp != 0xffffffff)
{
unsigned char val[2];
val[0] = temp;
val[1] = temp>>8;
if(setting_val[i].len == 2) {((unsigned char*)setting_val[i].address)[1] = val[1];
((unsigned char*)setting_val[i].address)[0] = val[0];}
else if (setting_val[i].len == 1) {((unsigned char*)setting_val[i].address)[0] = val[0];}
else ;
}
i = 0; // Indicate it was processed using index var
print_all = TRUE;
break;
}
}// for
}
// See if it was processed
if(i!=0)
{
PrintConst("\r\nNot recognised:<");
PrintConst(line);
PrintConst(">\r\n");
}
} // if line != null
static irqreturn_t __s3c4510b_rx_int(int irq, void *dev_id, struct pt_regs *regs)
{
struct sk_buff *skb;
struct net_device *dev = (struct net_device *) dev_id;
struct eth_priv *priv = (struct eth_priv *) dev->priv;
volatile RX_FrameDesc *pRxFD;
volatile RX_FrameDesc *cRxFD;
spin_lock(&priv->lock);
LED_SET(4);
pRxFD = priv->m_curRX_FD;
cRxFD = (RX_FrameDesc *)inl(REG_BDMARXPTR);
/* clear received frame bit */
outl( ETH_S_BRxRDF, REG_BDMASTAT);
do {
if ( likely( pRxFD->m_status.bf.good)) {
skb = pRxFD->skb;
__skb_prepare( dev, pRxFD);
/* reserve two words used by protocol layers */
skb_reserve(skb, 2);
skb_put(skb, pRxFD->m_status.bf.len);
skb->protocol = eth_type_trans(skb, dev);
priv->stats.rx_packets++;
priv->stats.rx_bytes += pRxFD->m_status.bf.len;
netif_rx(skb);
}
else {
priv->stats.rx_errors++;
if( pRxFD->m_status.bf.overFlow)
priv->stats.rx_fifo_errors++;
if( pRxFD->m_status.bf.overMax)
priv->stats.rx_length_errors++;
if( pRxFD->m_status.bf.crcErr)
priv->stats.rx_crc_errors++;
if( pRxFD->m_status.bf.longErr)
priv->stats.rx_length_errors++;
if( pRxFD->m_status.bf.alignErr)
priv->stats.rx_frame_errors++;
/**
** No good category for these errors
if( pRxFD->m_status.bf.parityErr)
**/
}
/* set owner back to CPU */
pRxFD->m_frameDataPtr.bf.owner = 1;
/* clear status */
pRxFD->m_status.ui = 0x0;
/* advance to next descriptor */
pRxFD = pRxFD->m_nextFD;
} while ( pRxFD != cRxFD);
priv->m_curRX_FD = pRxFD;
LED_CLR(4);
spin_unlock(&priv->lock);
return IRQ_HANDLED;
}
// Attached to USB
void RunAttached(void)
{
// Do this first to give the card time to start up
SD_ENABLE(); // Turn on SD card
// Enable peripherals
RtcInterruptOn(0); // Keeps time upto date
LED_SET(LED_WHITE);
CLOCK_PLL(); // PLL clock
// Initialize sensors
// Check if we have an accelerometer
AccelVerifyDeviceId();
// Check if we have a gyro
#ifdef USE_GYRO
GyroVerifyDeviceId();
#endif
#ifdef HAS_PROX
// Check for prox
ProxVerifyDeviceId();
ProxStartup();
#endif
// Initialize sensors
AccelStartup(ACCEL_RANGE_4G|ACCEL_RATE_100);
#ifdef USE_GYRO
GyroStartup();
#endif
AdcInit();
#ifndef NO_DISPLAY
DisplayClear();
Display_print_xy(" <= USB =>",0,2,2);
#endif
status.diskMounted = (status.lockCode == 0x0000) ? 1 : 0;
status.stream = 0;
// MDD_MediaInitialize(); // KL FIX: The SD card is re-inited in the usb framework which causes a lockup in some cases
// Power up module if off
PMD4bits.USB1MD = 0;
USBInitializeSystem(); // Initializes buffer, USB module SFRs and firmware
#ifdef USB_INTERRUPT
USBDeviceAttach();
#endif
while(USB_BUS_SENSE && restart != 1)
{
// Check bus status and service USB interrupts.
#ifndef USB_INTERRUPT
USBDeviceTasks(); // Interrupt or polling method. If using polling, must call
#endif
USBProcessIO();
if ((USBGetDeviceState() >= CONFIGURED_STATE) && (USBIsDeviceSuspended() == FALSE))
{
const char *line = _user_gets();
status.attached = 1;
if (line != NULL)
{
status.stream = 0; // Disable streaming
SettingsCommand(line, SETTINGS_USB);
}
}
else
{
status.attached = -1;
}
TimedTasks();
// Stream accelerometer data
if (status.stream)
{
#define STREAM_RATE 10
#define STREAM_INTERVAL (0x10000UL / STREAM_RATE)
static unsigned long lastSampleTicks = 0;
unsigned long now = RtcTicks();
if (lastSampleTicks == 0) { lastSampleTicks = now; }
if (now - lastSampleTicks > STREAM_INTERVAL)
{
accel_t accelSample;
#ifdef USE_GYRO
gyro_t gyroSample;
#endif
extern unsigned char scratchBuffer[];
char * ptr = (char *)scratchBuffer;
unsigned short len;
lastSampleTicks += STREAM_INTERVAL;
if (now - lastSampleTicks > 2 * STREAM_INTERVAL) { lastSampleTicks = now; } // not keeping up with sample rate
#ifdef HAS_PROX
// Sample sensors
if(ProxSampleReady())
{
ProxReadSample();
ProxStartSample();
}
#endif
AccelSingleSample(&accelSample);
#ifdef USE_GYRO
GyroSingleSample(&gyroSample);
#endif
// Write ascii to scratch buffer
ptr = (char *)scratchBuffer;
ptr += sprintf(ptr, "\f$ACCEL=%d,%d,%d\r\n", accelSample.x, accelSample.y, accelSample.z);
#ifdef USE_GYRO
ptr += sprintf(ptr, "$GYRO=%d,%d,%d\r\n", gyroSample.x, gyroSample.y, gyroSample.z);
#endif
#ifdef HAS_PROX
ptr += sprintf(ptr, "$PROX=%d\r\n", prox.proximity);
ptr += sprintf(ptr, "$LIGHT=%d\r\n", prox.light);
#endif
len = (unsigned short)((void*)ptr - (void*)scratchBuffer);
// Stream over USB
if ((status.stream) && status.attached == 1) { usb_write(scratchBuffer, len); }
}
}
}
#if defined(USB_INTERRUPT)
USBDeviceDetach();
#endif
status.attached = -1;
return;
}
