static void cmdGetImuLoop(unsigned char status, unsigned char length, unsigned char *frame) {
unsigned int count;
unsigned long tic;
unsigned char *tic_char;
Payload pld;
LED_RED = 1;
count = frame[0] + (frame[1] << 8);
tic_char = (unsigned char*) &tic;
swatchReset();
tic = swatchTic();
while (count) {
pld = payCreateEmpty(16); // data length = 16
paySetData(pld, 4, tic_char);
payAppendData(pld, 4, 6, xlReadXYZ());
payAppendData(pld, 10, 6, gyroReadXYZ());
paySetStatus(pld, status);
paySetType(pld, CMD_GET_IMU_DATA);
radioSendPayload(macGetDestAddr(), pld);
count--;
payDelete(pld);
delay_ms(4);
tic = swatchTic();
}
LED_RED = 0;
}
void peSendResponse(unsigned char cmd) {
cmdPld = payCreateEmpty(1);
cmdPld->pld_data[0] = 0;
cmdPld->pld_data[1] = CMD_PYROELEC_COMMAND;
cmdPld->pld_data[2] = cmd;
radioSendPayload((WordVal)macGetDestAddr(), cmdPld);
}
//Recieved UART Xbee packet, send packet out over the radio
void xbeeHandleTx(Payload uart_pld){
MacPacket tx_packet;
WordVal dst_addr;
Payload rx_pld;
//char test;
//Get destination address from uart_pld package
//Frame ID and options packet are currently ignored for this type of packet...
dst_addr.byte.HB = uart_pld->pld_data[DEST_ADDR_HB_POS - RX_DATA_OFFSET];
dst_addr.byte.LB = uart_pld->pld_data[DEST_ADDR_LB_POS - RX_DATA_OFFSET];
//test = dst_addr.byte.LB;
//Create new packet with just the data that needs to be sent by the radio
int payloadLength = payGetPayloadLength(uart_pld)-(RX_FRAME_OFFSET-RX_DATA_OFFSET)-PAYLOAD_HEADER_LENGTH-1;
rx_pld = payCreateEmpty(payloadLength);
//test = payGetPayloadLength(rx_pld);
payAppendData(rx_pld, -PAYLOAD_HEADER_LENGTH,
payGetPayloadLength(rx_pld),
&(uart_pld->pld_data[RX_DATA_OFFSET]));
//Place packet in radio queue for sending
tx_packet = radioRequestPacket(payloadLength);
tx_packet->payload = rx_pld;
tx_packet->payload_length = payGetPayloadLength(rx_pld);//rx_pld_len.byte.LB - (RX_FRAME_OFFSET - RX_DATA_OFFSET);
//tx_packet->dest_pan_id = src_pan_id; //Already set when macCreatePacket is called.
tx_packet->dest_addr = dst_addr;
radioEnqueueTxPacket(tx_packet);
}
static void cmdTxSavedImuData(unsigned char status, unsigned char length, unsigned char *frame) {
unsigned int page, byte;
unsigned int i, j;
Payload pld;
//senGetMemLocIndex(&page, &byte);
page = 0x0200;
byte = 0;
LED_RED = 1;
dfmemEraseSector(0x0100); // erase Sector 1 (page 256 - 511)
for (i = 0x0100; i < 0x0200; ++i) {
j = 0;
while (j < 512) {
pld = payCreateEmpty(18); // data length = 16
dfmemRead(i, j, 16, pld->pld_data);
paySetStatus(pld, status);
paySetType(pld, CMD_GET_IMU_DATA);
while (!radioReceivePayload());
j += 16;
}
delay_ms(200);
}
LED_RED = 0;
}
void peSendData(int a, int b, int c, int d) {
int* ptr;
dataPld = payCreateEmpty(8);
dataPld->pld_data[0] = 0;
dataPld->pld_data[1] = CMD_PYROELEC_DATA;
ptr = (int*)((dataPld->pld_data)+2);
ptr[0] = a;
ptr[1] = b;
ptr[2] = c;
ptr[3] = d;
radioSendPayload((WordVal)macGetDestAddr(), dataPld);
}
void peSendFrame(void) {
framePlds[frameNum] = payCreateEmpty(66);
framePlds[frameNum]->pld_data[0] = 0;
framePlds[frameNum]->pld_data[1] = CMD_PYROELEC_FRAME;
framePlds[frameNum]->pld_data[2] = 0;
framePlds[frameNum]->pld_data[3] = 0;
memcpy((framePlds[frameNum]->pld_data)+4, rec_buf, 64);
radioSendPayload((WordVal)macGetDestAddr(), framePlds[frameNum]);
framePlds[frameNum+1] = payCreateEmpty(66);
framePlds[frameNum+1]->pld_data[0] = 0;
framePlds[frameNum+1]->pld_data[1] = CMD_PYROELEC_FRAME;
framePlds[frameNum+1]->pld_data[2] = 1;
framePlds[frameNum+1]->pld_data[3] = 0;
memcpy((framePlds[frameNum+1]->pld_data)+4, rec_buf+64, 64);
radioSendPayload((WordVal)macGetDestAddr(), framePlds[frameNum+1]);
if(frameNum == 0)
frameNum = 2;
else
frameNum = 0;
}
static void cmdSleep(unsigned char status, unsigned char length, unsigned char *frame) {
char sleep = frame[0];
if (sleep) {
//g_radio_duty_cycle = 1;
} else {
//g_radio_duty_cycle = 0;
Payload pld;
pld = payCreateEmpty(1);
paySetData(pld, 1, (unsigned char*) (&sleep)); //echo back a CMD_SLEEP with '0', incdicating a wakeup
paySetStatus(pld, status);
paySetType(pld, CMD_SLEEP);
radioSendPayload((WordVal) macGetDestAddr(), pld);
}
}
static void cmdEraseSector(unsigned char status, unsigned char length, unsigned char *frame) {
//unsigned int numSamples = frame[0] + (frame[1] << 8);
unsigned long numSamples = *((unsigned long*) (frame));
telemErase(numSamples);
//Send a confirmation packet
Payload pld;
pld = payCreateEmpty(4);
paySetData(pld, 4, (unsigned char*) (&numSamples));
paySetStatus(pld, status);
paySetType(pld, CMD_ERASE_SECTORS);
radioSendPayload((WordVal) macGetDestAddr(), pld);
}
static void cmdSetCtrldTurnRate(unsigned char status, unsigned char length, unsigned char *frame) {
int rate;
Payload pld;
rate = frame[0] + (frame[1] << 8);
steeringSetAngRate(rate);
//Send confirmation packet
pld = payCreateEmpty(2);
//pld->pld_data[0] = status;
//pld->pld_data[1] = CMD_SET_CTRLD_TURN_RATE;
memcpy((pld->pld_data) + 2, frame, sizeof (int));
payAppendData(pld, 0, sizeof (rate), (unsigned char*) (&rate));
paySetStatus(pld, status);
paySetType(pld, CMD_SET_CTRLD_TURN_RATE);
radioSendPayload((WordVal) macGetDestAddr(), pld);
}
static void cmdSetHallGains(unsigned char status, unsigned char length, unsigned char *frame) {
//Unpack unsigned char* frame into structured values
PKT_UNPACK(_args_cmdSetPIDGains, argsPtr, frame);
hallSetGains(0, argsPtr->Kp1, argsPtr->Ki1, argsPtr->Kd1, argsPtr->Kaw1, argsPtr->Kff1);
hallSetGains(1, argsPtr->Kp2, argsPtr->Ki2, argsPtr->Kd2, argsPtr->Kaw2, argsPtr->Kff2);
//Send confirmation packet
Payload pld;
pld = payCreateEmpty(20);
//pld->pld_data[0] = status;
//pld->pld_data[1] = CMD_SET_HALL_GAINS;
paySetType(pld, CMD_SET_HALL_GAINS);
paySetStatus(pld, status);
memcpy((pld->pld_data) + 2, frame, 20);
radioSendPayload((WordVal) macGetDestAddr(), pld);
}
// set up velocity profile structure - assume 4 set points for now, generalize later
static void cmdSetVelProfile(unsigned char status, unsigned char length, unsigned char *frame) {
Payload pld;
PKT_UNPACK(_args_cmdSetVelProfile, argsPtr, frame);
hallSetVelProfile(0, argsPtr->intervalsL, argsPtr->deltaL, argsPtr->velL);
hallSetVelProfile(1, argsPtr->intervalsR, argsPtr->deltaR, argsPtr->velR);
//Send confirmation packet
pld = payCreateEmpty(sizeof(_args_cmdSetVelProfile));
//pld->pld_data[0] = status;
paySetStatus(pld, status);
//pld->pld_data[1] = CMD_SET_VEL_PROFILE;
paySetType(pld, CMD_SET_VEL_PROFILE);
// packet length = 48 bytes (24 ints)
memcpy((pld->pld_data) + 2, frame, sizeof(_args_cmdSetVelProfile));
radioSendPayload((WordVal) macGetDestAddr(), pld);
}
static void cmdGetPIDTelemetry(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
unsigned int count;
unsigned long tic;
unsigned char *tic_char = (unsigned char*)&tic;
//unsigned long sampNum = 0;
int i;
unsigned short idx = 0;
MacPacket packet; Payload pld;
unsigned char* telem_ptr;
count = frame[0] + (frame[1] << 8);
swatchReset();
tic = swatchTic();
while(count){
pld = payCreateEmpty(36); // data length = 12
//*(long*)(pld->pld_data + idx) = tic;
pld->pld_data[2] = tic_char[0];
pld->pld_data[3] = tic_char[1];
pld->pld_data[4] = tic_char[2];
pld->pld_data[5] = tic_char[3];
idx += sizeof(tic);
telem_ptr = pidGetTelemetry();
//memcpy((pld->pld_data)+idx , telem_ptr, 4*sizeof(int));
for(i = 0; i < (4*sizeof(int)+6*sizeof(long)); ++i) {
pld->pld_data[i+6] = telem_ptr[i];
}
pld->pld_data[0] = status;
pld->pld_data[1] = CMD_GET_PID_TELEMETRY;
// radioSendPayload(macGetDestAddr(), pld);
// Enqueue the packet for broadcast
while(!radioEnqueueTxPacket(packet));
count--;
//delay_ms(2); // ~3ms delay
//delay_us(695);
delay_ms(10);
tic = swatchTic();
}
}
static void cmdSetCtrldTurnRate(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
int rate;
int idx = 0;
MacPacket packet; Payload pld;
rate = frame[0] + (frame[1] << 8);
setSteeringAngRate(rate);
//Send confirmation packet
pld = payCreateEmpty(sizeof(int));
pld->pld_data[0] = status;
pld->pld_data[1] = CMD_SET_CTRLD_TURN_RATE;
memcpy((pld->pld_data)+2, frame, sizeof(int));
// radioSendPayload((WordVal)macGetDestAddr(), pld);
// ip2.5c radio command:
// Enqueue the packet for broadcast
while(!radioEnqueueTxPacket(packet));
}
static void cmdSetSteeringGains(unsigned char status, unsigned char length, unsigned char *frame) {
//int Kp, Ki, Kd, Kaw, ff;
// int steerMode;
// int idx = 0;
Payload pld;
PKT_UNPACK(_args_cmdSetSteeringGains, argsPtr, frame);
//_args_cmdSetSteeringGains* argsPtr = (_args_cmdSetSteeringGains*) (frame);
steeringSetGains(argsPtr->Kp, argsPtr->Ki, argsPtr->Kd, argsPtr->Kaw, argsPtr->Kff);
steeringSetMode(argsPtr->steerMode);
pld = payCreateEmpty(sizeof(_args_cmdSetSteeringGains));
pld->pld_data[0] = status;
pld->pld_data[1] = CMD_SET_STEERING_GAINS;
memcpy((pld->pld_data) + 2, frame, sizeof(_args_cmdSetSteeringGains));
radioSendPayload((WordVal) macGetDestAddr(), pld);
}
static void cmdSetPIDGains(unsigned char status, unsigned char length, unsigned char *frame) {
//int Kp, Ki, Kd, Kaw, ff;
//int idx = 0;
//Unpack unsigned char* frame into structured values
//_args_cmdSetPIDGains* argsPtr = (_args_cmdSetPIDGains*) (frame);
PKT_UNPACK(_args_cmdSetPIDGains, argsPtr, frame);
legCtrlSetGains(0, argsPtr->Kp1, argsPtr->Ki1, argsPtr->Kd1, argsPtr->Kaw1, argsPtr->Kff1);
legCtrlSetGains(1, argsPtr->Kp2, argsPtr->Ki2, argsPtr->Kd2, argsPtr->Kaw2, argsPtr->Kff2);
//Send confirmation packet
Payload pld;
pld = payCreateEmpty(20);
pld->pld_data[0] = status;
pld->pld_data[1] = CMD_SET_PID_GAINS;
memcpy((pld->pld_data) + 2, frame, 20);
radioSendPayload((WordVal) macGetDestAddr(), pld);
}
static void cmdSetSteeringGains(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
int Kp, Ki, Kd, Kaw, ff;
int idx = 0;
MacPacket packet; Payload pld;
Kp = frame[idx] + (frame[idx+1] << 8); idx+=2;
Ki = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kd = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2;
ff = frame[idx] + (frame[idx+1] << 8); idx+=2;
steeringSetGains(Kp,Ki,Kd,Kaw, ff);
//Send confirmation packet
pld = payCreateEmpty(10);
pld->pld_data[0] = status;
pld->pld_data[1] = CMD_SET_STEERING_GAINS;
memcpy((pld->pld_data)+2, frame, 10);
// radioSendPayload((WordVal)macGetDestAddr(), pld);
// ip2.5c radio command:
// Enqueue the packet for broadcast
while(!radioEnqueueTxPacket(packet));
}
//read data from the UART, and call the proper function based on the Xbee code
void __attribute__((__interrupt__, no_auto_psv)) _U1RXInterrupt(void)
{
static unsigned char uart_rx_state = UART_RX_WAITING;
static unsigned char uart_rx_cnt = 0;
static Payload uart_pld;
static WordVal uart_pld_len;
static byte uart_checksum;
static unsigned char packet_type = 0;
static unsigned char test;
unsigned char c;
if(U1STAbits.OERR)
{
U1STAbits.OERR = 0;
}
c = ReadUART1();
if (uart_rx_state == UART_RX_WAITING && c == RX_START)
{
uart_rx_state = UART_RX_ON;
packet_type = 0;
uart_rx_cnt = 1;
uart_checksum = 0x00;
}else if (uart_rx_state == UART_RX_ON)
{
switch (uart_rx_cnt)
{
//XBee interface uses two bytes for payload length, despite the
//fact that packets can't be longer than 128 bytes. The high byte
//is extracted, but never used here.
case LEN_HB_POS:
uart_pld_len.byte.HB = c;
uart_rx_cnt++;
break;
case LEN_LB_POS:
uart_pld_len.byte.LB = c;
//We create a payload structure to store the data incoming from the uart
uart_pld = payCreateEmpty(uart_pld_len.byte.LB-PAYLOAD_HEADER_LENGTH);
test = uart_pld_len.byte.LB;
uart_rx_cnt++;
break;
case API_ID_POS:
//Currently, we're only supporting the 16-bit TX/RX API,
//and the AT command mode
packet_type = c;
uart_checksum += c;
uart_rx_cnt++;
break;
default:
if (uart_rx_cnt == (uart_pld_len.byte.LB + RX_DATA_OFFSET-1))
{
if (uart_checksum + c == 0xFF) //We have a legit packet
{
//Check for type of packet and call relevant function
switch (packet_type)
{
case AT_COMMAND_MODE:
xbeeHandleAt(uart_pld);
break;
case TX_16BIT:
xbeeHandleTx(uart_pld);
break;
default:
//do nothing, but probably should send an error
break;
}
payDelete(uart_pld);
}else //Start over
{
payDelete(uart_pld);
}
uart_rx_state = UART_RX_WAITING;
}else
{
uart_pld->pld_data[uart_rx_cnt-RX_DATA_OFFSET] = c;
uart_checksum += c;
uart_rx_cnt++;
}
break;
}
}
_U1RXIF = 0;
}
void peSetup(void) {
// setup peripheral
// SPI Setup
// SPI interrupt is not used.
int i;
IFS2bits.SPI2IF = 0; // Clear the Interrupt Flag
IEC2bits.SPI2IE = 0; // Disable the Interrupt
// SPI1CON1 Register Settings
SPI_CON1bits.MSTEN = 1; // Master mode Enabled
SPI_CON1bits.DISSDO = 0; // SDOx pin is controlled by the module
SPI_CON1bits.SSEN = 0; // SS1 pin controlled by module
SPI_CON1bits.DISSCK = 0; // Internal Serial Clock is Enabled
// Set up SCK frequency of 625kHz for 40 MIPS
SPI_CON1bits.SPRE = 0b111; // Secondary prescale 1:1
SPI_CON1bits.PPRE = 0b00; // Primary prescale 64:1
SPI_CON1bits.SMP = 0; // Input data is sampled at middle of data output time
SPI_CON1bits.CKE = 0; // Serial output data changes on idle to active transition
SPI_CON1bits.CKP = 0; // Idle state for clock is a low level;
// active state is a high level
SPI_CON1bits.MODE16 = 0; // Communication is byte-wide (8 bits)
// SPI2CON2 Register Settings
SPI_CON2 = 0x0000; // Framed SPI2 support disabled
// SPI2STAT Register Settings
SPI_STATbits.SPISIDL = 1; // Discontinue module when device enters idle mode
SPI_STATbits.SPIROV = 0; // Clear Overflow
SPI_STATbits.SPIEN = 1; // Enable SPI module
CS_DIR = 0;
FAKE_CS_DIR = 0;
cmdPld = payCreateEmpty(1);
cmdPld->pld_data[0] = 0;
cmdPld->pld_data[1] = CMD_PYROELEC_COMMAND;
dataPld = payCreateEmpty(8);
dataPld->pld_data[0] = 0;
dataPld->pld_data[1] = CMD_PYROELEC_DATA;
for(i=0; i<4; i++) {
framePlds[i] = payCreateEmpty(66);
framePlds[i]->pld_data[0] = 0;
framePlds[i]->pld_data[1] = CMD_PYROELEC_FRAME;
framePlds[i]->pld_data[2] = (i&1);
framePlds[i]->pld_data[3] = 0;
}
for(i=0;i<N_HIST;i++)
centerHist[i] = 0;
peResetModule();
peSendCommand('M');
delay_ms(5);
peTransaction(NULL,0,NULL,100);
}