void init(void)
{
int i;
volatile WordVal src_addr = {SRC_ADDR};
volatile WordVal src_pan_id = {SRC_PAN_ID};
SetupClock();
SwitchClocks();
SetupPorts();
for (i = 0; i < 6; i++)
{
LED_RED = ~LED_RED;
delay_ms(50);
LED_YLW1 = ~LED_YLW1;
delay_ms(50);
LED_YLW2 = ~LED_YLW2;
delay_ms(50);
LED_BLU = ~LED_BLU;
delay_ms(50);
}
SetupUART1();
SetupInterrupts();
EnableIntU1TX;
EnableIntU1RX;
radioInit(src_addr, src_pan_id, 150, 150);
atSetPromMode(1); //This turns off Automatic Acknowledgements and puts the radio in prom mode
radioSetChannel(MY_CHAN); //Set to my channel
//atSetAntDiversity(1);
}
int main(void) {
WordVal src_addr_init = {SRC_ADDR};
WordVal src_pan_id_init = {SRC_PAN_ID};
WordVal dst_addr_init = {DST_ADDR};
SetupClock();
SwitchClocks();
SetupPorts();
batSetup();
swatchSetup();
radioInit(src_addr_init, src_pan_id_init, RXPQ_MAX_SIZE, TXPQ_MAX_SIZE);
radioSetChannel(MY_CHAN); //Set to my channel
macSetDestAddr(dst_addr_init);
dfmemSetup();
unsigned char memsize;
memsize = dfmemGetChipSize();
xlSetup();
gyroSetup();
mcSetup();
cmdSetup();
//senSetup();
adcSetup();
pidSetup();
steeringSetup();
//radioReadTrxId(id);
LED_RED = 1;
LED_BLUE = 0;
LED_YELLOW = 0;
//while(1);
if(phyGetState() == 0x16) { LED_GREEN = 1; }
//print("Ready");
//readDFMemBySample(5);
while(1) {
cmdHandleRadioRxBuffer();
//Simple idle ; reduces idle current to 70 mA
// TODO (abuchan, apullin, fgb) : Idle() causes unexpected behavior
//if(radioIsRxQueueEmpty()){
// Idle();
//}
}
}
int main ( void )
{
fun_queue = queueInit(FUN_Q_LEN);
rx_pay_queue = pqInit(12); //replace 12 with a #define const later
test_function tf;
/* Initialization */
SetupClock();
SwitchClocks();
SetupPorts();
SetupInterrupts();
SetupI2C();
SetupADC();
SetupTimer1();
SetupPWM();
SetupTimer2();
gyroSetup();
xlSetup();
dfmemSetup();
WordVal pan_id = {RADIO_PAN_ID};
WordVal src_addr = {RADIO_SRC_ADDR};
WordVal dest_addr = {RADIO_DEST_ADDR};
radioInit(src_addr, pan_id, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetDestAddr(dest_addr);
radioSetChannel(RADIO_MY_CHAN);
char j;
for(j=0; j<3; j++){
LED_2 = ON;
delay_ms(500);
LED_2 = OFF;
delay_ms(500);
}
LED_2 = ON;
EnableIntT2;
while(1){
while(!queueIsEmpty(fun_queue))
{
rx_payload = pqPop(rx_pay_queue);
tf = (test_function)queuePop(fun_queue);
(*tf)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload));
payDelete(rx_payload);
}
}
return 0;
}
int main (void)
{
unsigned int i;
/* Initialization */
SetupClock();
SetupPorts();
batSetup();
cmdSetup();
mcSetup();
SetupADC();
SwitchClocks();
sclockSetup();
radioInit(TXPQ_MAX_SIZE, RXPQ_MAX_SIZE);
radioSetChannel(MY_CHAN);
radioSetSrcPanID(PAN_ID);
radioSetSrcAddr(SRC_ADDR);
dfmemSetup();
camSetup();
cambuffSetup();
gyroSetup();
cmdResetSettings();
for (i = 0; i < 6; i++)
{
LED_GREEN = ~LED_GREEN; delay_ms(50);
LED_RED = ~LED_RED; delay_ms(50);
LED_ORANGE = ~LED_ORANGE; delay_ms(50);
}
LED_GREEN = 0; LED_RED = 0; LED_ORANGE = 0;
/* Program */
while (1)
{
cmdHandleRadioRxBuffer();
radioProcess();
}
}
void cmdRun()
{
char status;
char tlen; //Transmit length
char cmdPtr;
char resPtr;
unsigned char id;
unsigned char plen;
char rlen; //Received packet length
uint8_t ack;
unsigned char cmd;
if (!(OUT1CS&EPBSY) && !contCarrier) {
//Fetch the USB data size, should not be higher than 64
tlen = OUT1BC;
if (tlen>64) tlen=64;
//Get the command string in a ram buffer
memcpy(tbuffer, OUT1BUF, tlen);
//And re-activate the out EP
OUT1BC=BCDUMMY;
// Run commands!
cmdPtr = 0;
resPtr = 0;
while (cmdPtr < tlen) {
cmd = tbuffer[cmdPtr++];
switch (cmd) {
case CMD_PACKET:
if ((tlen-cmdPtr)<3) {
sendError(ERROR_MALFORMED_CMD, cmd, cmdPtr);
cmdPtr = tlen;
break;
}
id = tbuffer[cmdPtr++];
plen = tbuffer[cmdPtr++];
if ((tlen-(cmdPtr+plen))<0) {
sendError(ERROR_MALFORMED_CMD, cmd, cmdPtr);
cmdPtr = tlen;
break;
}
status = radioSendPacket(&tbuffer[cmdPtr], plen,
rpbuffer, &rlen);
cmdPtr += plen;
ledTimeout = 2;
ledSet(LED_GREEN | LED_RED, false);
if(status)
ledSet(LED_GREEN, true);
else
ledSet(LED_RED, true);
//Check if there is enough place in rbuffer, flush it if not
if ((resPtr+rlen+4)>64) {
//Wait for IN1 to become free
while(IN1CS&EPBSY);
memcpy(IN1BUF, rbuffer, resPtr);
IN1BC = resPtr;
resPtr = 0;
}
//Prepare the USB answer, state and ack data
ack=status?1:0;
if (ack)
{
if (radioGetRpd()) ack |= 0x02;
ack |= radioGetTxRetry()<<4;
}
if(!(status&BIT_TX_DS)) rlen=0;
rbuffer[resPtr] = 0;
rbuffer[resPtr+1] = id;
rbuffer[resPtr+2] =ack;
rbuffer[resPtr+3] = rlen;
memcpy(&rbuffer[resPtr+4], rpbuffer, rlen);
resPtr += rlen+4;
break;
case SET_RADIO_CHANNEL:
if (((tlen-cmdPtr)<1) || (tbuffer[cmdPtr]>125)) {
sendError(ERROR_MALFORMED_CMD, cmd, cmdPtr);
cmdPtr = tlen;
break;
}
radioSetChannel(tbuffer[cmdPtr++]);
break;
case SET_DATA_RATE:
if (((tlen-cmdPtr)<1) || (tbuffer[cmdPtr]>3)) {
sendError(ERROR_MALFORMED_CMD, cmd, cmdPtr);
cmdPtr = tlen;
break;
}
radioSetDataRate(tbuffer[cmdPtr++]);
break;
default:
sendError(ERROR_UNKNOWN_CMD, cmd, cmdPtr);
break;
}
}
// Send data that are still in the TX buffer
if (resPtr != 0) {
//Wait for IN1 to become free
while(IN1CS&EPBSY);
memcpy(IN1BUF, rbuffer, resPtr);
IN1BC = resPtr;
resPtr = 0;
}
}
}
//Handles vendor control messages and ack them
void handleUsbVendorSetup()
{
__xdata struct controllStruct *setup = usbGetSetupPacket();
//The vendor control messages are valide only when the device is configured
if (usbGetState() >= CONFIGURED)
{
if(setup->request == LAUNCH_BOOTLOADER)
{
//Ack the launch request
usbAckSetup();
launchBootloader();
//Will never come back ...
return;
}
else if(setup->request == SET_RADIO_CHANNEL)
{
radioSetChannel(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_DATA_RATE)
{
radioSetDataRate(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_RADIO_ADDRESS)
{
if(setup->length != 5)
{
usbDismissSetup();
return;
}
//Arm and wait for the out transaction
OUT0BC = BCDUMMY;
while (EP0CS & OUTBSY);
//Set address of the pipe given by setup's index
radioSetAddress(OUT0BUF);
//Ack the setup phase
usbAckSetup();
return;
}
else if(setup->request == SET_RADIO_POWER)
{
radioSetPower(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_RADIO_ARD)
{
radioSetArd(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_RADIO_ARC)
{
radioSetArc(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_CONT_CARRIER)
{
radioSetContCarrier((setup->value)?true:false);
contCarrier = (setup->value)?true:false;
ledTimeout = -1;
ledSet(LED_RED, (setup->value)?true:false);
usbAckSetup();
return;
}
else if(setup->request == ACK_ENABLE)
{
needAck = (setup->value)?true:false;
usbAckSetup();
return;
}
else if(setup->request == CHANNEL_SCANN && setup->requestType == 0x40)
{
int i;
char rlen;
char status;
char inc = 1;
unsigned char start, stop;
scannLength = 0;
if(setup->length < 1)
{
usbDismissSetup();
return;
}
//Start and stop channels
start = setup->value;
stop = (setup->index>125)?125:setup->index;
if (radioGetDataRate() == DATA_RATE_2M)
inc = 2; //2M channel are 2MHz wide
//Arm and wait for the out transaction
OUT0BC = BCDUMMY;
while (EP0CS & OUTBSY);
memcpy(tbuffer, OUT0BUF, setup->length);
for (i=start; i<stop+1 && scannLength<MAX_SCANN_LENGTH; i+=inc)
{
radioSetChannel(i);
status = radioSendPacket(tbuffer, setup->length, rbuffer, &rlen);
if (status)
IN0BUF[scannLength++] = i;
ledTimeout = 2;
ledSet(LED_GREEN | LED_RED, false);
if(status)
ledSet(LED_GREEN, true);
else
ledSet(LED_RED, true);
}
//Ack the setup phase
usbAckSetup();
return;
}
else if(setup->request == CHANNEL_SCANN && setup->requestType == 0xC0)
{
//IN0BUF already contains the right data
//(if a scann has been launched before ...)
IN0BC = (setup->length>scannLength)?scannLength:setup->length;
while (EP0CS & INBSY);
//Ack the setup phase
usbAckSetup();
return;
}
else if(setup->request == SET_MODE && setup->requestType == 0x40)
{
mode = setup->value;
if (mode == MODE_PRX)
{
radioSetMode(RADIO_MODE_PRX);
}
else
{
radioSetMode(RADIO_MODE_PTX);
}
usbAckSetup();
return;
}
}
//Stall in error if nothing executed!
usbDismissSetup();
}
int main(void) {
wakeTime = 0;
dcCounter = 0;
WordVal src_addr_init = {RADIO_SRC_ADDR};
WordVal src_pan_id_init = {RADIO_SRC_PAN_ID};
WordVal dst_addr_init = {RADIO_DST_ADDR};
SetupClock();
SwitchClocks();
SetupPorts();
tiHSetup();
//swatchSetup();
radioInit(src_addr_init, src_pan_id_init, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetChannel(RADIO_CHANNEL); //Set to my channel
macSetDestAddr(dst_addr_init);
cmdSetup();
if(phyGetState() == 0x16) { LED_RED = 1; }
while(1){
cmdHandleRadioRxBuffer();
}
LED_GREEN = 0;
LED_RED = 1;
LED_YELLOW = 1;
_LATG9 = 1;
_LATC15 = 1;
//testRadio();
/*
LED_GREEN = 1;
mpuSetup();
LED_GREEN = 0;
LED_RED = 1;
LED_YELLOW = 1;
//batSetup();
//int old_ipl;
//mSET_AND_SAVE_CPU_IP(old_ipl, 1)
//LED_YELLOW = 1;
//dfmemSetup();
//xlSetup();
//gyroSetup();
//tiHSetup();
//mcSetup();
//cmdSetup();
//adcSetup();
//telemSetup(); //Timer 5
//mcSetDutyCycle(1,70.0);
//mcSetDutyCycle(2,70.0);
//mcSetDutyCycle(3,70.0);
//mcSetDutyCycle(4,70.0);
#ifdef HALL_SENSORS
//hallSetup(); // Timer 1, Timer 2
//hallSteeringSetup(); //doesn't exist yet
#else //No hall sensors, standard BEMF control
//legCtrlSetup(); // Timer 1
//steeringSetup(); //Timer 5
#endif
//tailCtrlSetup();
//ovcamSetup();
/*
//radioReadTrxId(id);
LED_RED = 1; //Red is use an "alive" indicator
LED_GREEN = 0;
LED_YELLOW = 0;
//tiHSetFloat(1,50.0);
//tiHSetFloat(2,75.0);
//tiHSetup();
//LED_GREEN = 1;
//tiHSetFloat(1,.800);
//LED_YELLOW = 1;
//Radio startup verification
//if(phyGetState() == 0x16) { LED_GREEN = 1; }
//Sleeping and low power options
//_VREGS = 1;
//gyroSleep();
//tiHSetFloat(1,98.0);
//tiHSetFloat(2,30.0);
//tiHSetFloat(3,99.0);
//tiHSetFloat(4,99.0);
LED_GREEN = 0;
LED_RED = 0;
LED_YELLOW = 1;
int i = 0;
while (1)
{
delay_ms(2000);
int i = i+1;
LED_GREEN = i%2;
// cmdHandleRadioRxBuffer();
#ifndef __DEBUG //Idle will not work with debug
//Simple idle:
if (radioIsRxQueueEmpty()) {
Idle();
//_T1IE = 0;
}
#endif
//delay_ms(1000);
//cmdEcho(0, 1 , (unsigned char*)(&i) );
//i++;
//if(radioIsRxQueueEmpty() && (t1_ticks >= wakeTime + 5000) ){
//Idle();
//LED_RED = 0;
//gyroSleep();
//Sleep();
//}
}
/*
if(g_radio_duty_cycle){
if(dcCounter == 0){
//LED_GREEN = 1;
atSetRXAACKON();
}else{
//LED_GREEN = 0;
atSetTRXOFF();
}
}
else{
//LED_GREEN = 1;
}
dcCounter = (dcCounter + 1) % 8;
if(radioIsRxQueueEmpty() && !inMotion){
//gyroSleep();
LED_RED = 0;
_SWDTEN = 1; //restart wdt
Sleep();
//Idle();
}
//should be asleep here, waiting for WTD wakeup
ClrWdt(); //clear wdt
_SWDTEN = 0; //software disable wdt
LED_RED = 1;
//spin up clock
if(_COSC != 0b010){
while(OSCCONbits.LOCK!=1);
}
//gyroWake();
}
}
void testRadio(void)
{
//designed to be used with testRadio.py.
//test radio.py should produce many echoes that cycle through the ASCII characters
//comment out all code in main and use test Radio to test the radio only.
//This code does not initialize non-radio-nessisary components.
wakeTime = 0;
dcCounter = 0;
WordVal src_addr_init = {RADIO_SRC_ADDR};
WordVal src_pan_id_init = {RADIO_SRC_PAN_ID};
WordVal dst_addr_init = {RADIO_DST_ADDR};
SetupClock();
SwitchClocks();
SetupPorts();
int old_ipl;
mSET_AND_SAVE_CPU_IP(old_ipl, 1)
swatchSetup();
radioInit(src_addr_init, src_pan_id_init, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetChannel(RADIO_CHANNEL); //Set to my channel
macSetDestAddr(dst_addr_init);
cmdSetup();
LED_GREEN = ON;
int i = 0;
while (1) {
i++;
cmdHandleRadioRxBuffer();
}
LED_RED = OFF;
}*/
}
void xbeeHandleAt(Payload rx_pld)
{
unsigned char frame;
unsigned char length;
// unsigned char test;
//This should really be some sort of dynamic queue to be generalized
unsigned char bytes[10];
//unsigned short int command;
WordVal command;
WordVal data;
length = payGetPayloadLength(rx_pld);
frame = rx_pld->pld_data[0];
//test = rx_pld->pld_data[1];
//test = rx_pld->pld_data[2];
/*
command = rx_pld->pld_data[1];
command = command << 8;
command = command + rx_pld->pld_data[2];
*/
command.byte.HB = rx_pld->pld_data[1];
command.byte.LB = rx_pld->pld_data[2];
switch (command.val)
{
case AT_CHANNEL:
if (length != 3)
{
radioSetChannel(rx_pld->pld_data[3]);
}
if (frame != 0)
{
bytes[0] = radioGetChannel();
xbeeHandleATR(frame, command, bytes, 1);
}
break;
case AT_PAN_ID:
if (length != 3)
{
data.byte.HB = rx_pld->pld_data[3];
data.byte.LB = rx_pld->pld_data[4];
radioSetSrcPanID(data.val);
}
if (frame != 0)
{
data.val = (unsigned int)radioGetSrcPanID();
bytes[0] = data.byte.HB;
bytes[1] = data.byte.LB;
xbeeHandleATR(frame, command, bytes, 2);
}
break;
case AT_SRC_ADR:
if (length != 3)
{
data.byte.HB = rx_pld->pld_data[3];
data.byte.LB = rx_pld->pld_data[4];
radioSetSrcAddr(data.val);
}
if (frame != 0)
{
data.val = radioGetSrcAddr();
bytes[0] = data.byte.HB;
bytes[1] = data.byte.LB;
xbeeHandleATR(frame, command, bytes, 2);
}
break;
case AT_ACK_LAST:
if (frame != 0)
{
bytes[0] = 0;
bytes[1] = trxGetLastACKd();
xbeeHandleATR(frame, command, bytes, 2);
}
break;
case AT_SNIFFER:
if (length != 3)
trxSetPromMode(rx_pld->pld_data[3]); //Put radio in sniffer mode
break;
}
}
int main() {
// Processor Initialization
SetupClock();
SwitchClocks();
SetupPorts();
sclockSetup();
LED_1 = 1;
LED_2 = 1;
LED_3 = 1;
// Message Passing
fun_queue = carrayCreate(FUN_Q_LEN);
cmdSetup();
// Radio setup
radioInit(RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetChannel(RADIO_CHANNEL);
radioSetSrcAddr(RADIO_SRC_ADDR);
radioSetSrcPanID(RADIO_PAN_ID);
uart_tx_packet = NULL;
uart_tx_flag = 0;
//uartInit(&cmdPushFunc);
tactileInit();
// Need delay for encoders to be ready
delay_ms(100);
amsEncoderSetup();
mpuSetup();
tiHSetup();
dfmemSetup();
telemSetup();
adcSetup();
pidSetup();
LED_1 = 0;
LED_3 = 1;
while(1){
// Send outgoing radio packets
radioProcess();
/*
// Send outgoing uart packets
if(uart_tx_flag) {
uartSendPacket(uart_tx_packet);
uart_tx_flag = 0;
}*/
checkTactileBuffer();
// move received packets to function queue
while (!radioRxQueueEmpty()) {
// Check for unprocessed packet
rx_packet = radioDequeueRxPacket();
if(rx_packet != NULL) {
cmdPushFunc(rx_packet);
}
}
// process commands from function queue
while(!carrayIsEmpty(fun_queue)) {
rx_packet = carrayPopHead(fun_queue);
unsigned int rx_src_addr = rx_packet->src_addr.val;
if(rx_packet != NULL) {
rx_payload = macGetPayload(rx_packet);
if(rx_payload != NULL) {
rx_function = (test_function)(rx_payload->test);
if(rx_function != NULL) {
LED_2 = ~LED_2;
(rx_function)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload), rx_src_addr);
}
}
ppoolReturnFullPacket(rx_packet);
}
}
}
return 0;
}
int main(void) {
//wakeTime = 0;
//dcCounter = 0;
// Processor Initialization
SetupClock();
SwitchClocks();
SetupPorts();
sclockSetup();
LED_1 = 0;
LED_2 = 0;
LED_3 = 0;
cmdSetup();
radioInit(RADIO_TXPQ_MAX_SIZE, RADIO_RXPQ_MAX_SIZE);
radioSetChannel(RADIO_CHANNEL);
radioSetSrcPanID(RADIO_PAN_ID);
radioSetSrcAddr(RADIO_SRC_ADDR);
dfmemSetup();
uint64_t id = dfmemGetUnqiueID();
telemSetup(); //Timer 5, HW priority 4
mpuSetup();
imuSetup(); //Timer 4, HW priority 3
tiHSetup();
adcSetup();
//AMS Encoders
//encSetup();
//"Open Loop" vibration & jitter generator, AP 2014
//olVibeSetup();
legCtrlSetup(); //Timer 1, HW priority 5
steeringSetup(); //Timer 5, HW priority 4
//Tail control is a special case
//tailCtrlSetup();
//Camera is untested with current code base, AP 12/6/2012
//ovcamSetup();
LED_RED = 1; //Red is use an "alive" indicator
LED_GREEN = 0;
LED_YELLOW = 0;
//Radio startup verification
//if (phyGetState() == 0x16) {
// LED_GREEN = 1;
//}
//Sleeping and low power options
//_VREGS = 1;
//gyroSleep();
/////FUNCTION TEST, NOT FOR PRODUCTION
//olVibeStart();
////////////////////
while (1) {
cmdHandleRadioRxBuffer();
radioProcess();
}
}
