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;
}
/*****************************************************************************
* Function Name : test_hall
* Description : send out over the radio a the current position readings from the
Austria Microsystems AS5048B absolute Hall sensors
* Parameters : type - The type field of the hall test packet
* status - Status field of Hall test packet (not yet used)
* length - The length of the payload data array
* data - not used
* Return Value : success indicator - 0 for failed, 1 for succeeded
*****************************************************************************/
unsigned char test_hall(unsigned char type, unsigned char status,\
unsigned char length, unsigned char* data)
{ int i;
MacPacket packet;
Payload pld;
// refresh Hall reading
for(i = 0; i< NUM_ENC; i++)
{
amsGetPos(i);
}
// Get a new packet from the pool
packet = radioRequestPacket(sizeof(encPos));
if(packet == NULL) return 0;
//macSetDestAddr( RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetStatus(pld, STATUS_UNUSED);
paySetType(pld, type);
// Read Hall data into the payload
memcpy(payGetData(pld), & encPos, sizeof(encPos)); // copy Hall data to packet
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
return 1; //success
}
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;
}
/*****************************************************************************
* Function Name : test_motor
* Description : Turns on a specified motor for a specified period of time
* and duty cycle
* Parameters : type - The type field of the motor test packet
* status - Status field of the motor test packet (not yet used)
* length - The length of the payload data array
* data - data[0] = motor number
* data[1:2] = on time (milli secs)
* data[3:4] = duty cycle (percent)
* Return Value : success indicator - 0 for failed, 1 for succeeded
*****************************************************************************/
unsigned char test_motor(unsigned char type, unsigned char status, \
unsigned char length, unsigned char* data)
{ unsigned int motor_id;
int on_time;
int dutycycle;
char ack_string[40]="motor OK\n";
MacPacket packet;
Payload pld;
motor_id = (unsigned int) data[0];
on_time = (unsigned long)( (data[3] << 8) + data[2]);
dutycycle = (int)((data[5] << 8) + data[4]);
tiHSetDC(motor_id, dutycycle);
swatchDelayMs(on_time);
tiHSetDC(motor_id, 0);
// send an ack packet back - could have data later...
// Get a new packet from the pool
packet = radioRequestPacket(sizeof(ack_string));
if(packet == NULL) return 0;
//macSetDestAddr(packet, RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetStatus(pld, STATUS_UNUSED);
paySetType(pld, type);
// Read message data into the payload
memcpy(payGetData(pld), & ack_string, sizeof(ack_string)); // copy ack_string to packet
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
return 1; //success
}
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);
}
// 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 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);
}
static void cmdGetMemContents(MacPacket packet) {
Payload pld;
MacPacket data_packet;
unsigned char *frame;
DfmemGeometryStruct geo;
pld = macGetPayload(packet);
frame = payGetData(pld);
dfmemGetGeometryParams(&geo);
unsigned int start_page = frame[0] + (frame[1] << 8);
unsigned int end_page = frame[2] + (frame[3] << 8);
unsigned int tx_data_size = frame[4] + (frame[5] << 8);
unsigned int page, j;
unsigned char count = 0;
// Send back memory contents
for (page = start_page; page < end_page; ++page) {
j = 0;
while (j + tx_data_size <= geo.bytes_per_page) {
data_packet = NULL;
while(data_packet == NULL) {
data_packet = radioRequestPacket(tx_data_size);
}
macSetDestAddr(data_packet, macGetSrcAddr(packet));
macSetDestPan(data_packet, macGetSrcPan(packet));
pld = macGetPayload(data_packet);
dfmemRead(page, j, tx_data_size, payGetData(pld));
paySetStatus(pld, count++);
paySetType(pld, CMD_RESPONSE_TELEMETRY);
while(!radioEnqueueTxPacket(data_packet));
j += tx_data_size;
delay_ms(20);
}
}
// Signal end of transfer
LED_GREEN = 0; LED_RED = 0; LED_ORANGE = 0;
}
unsigned char uartSendPayload(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
MacPacket packet;
Payload pld;
packet = ppoolRequestFullPacket(length);
if(packet == NULL)
return 0;
pld = packet->payload;
paySetType(pld, type);
paySetStatus(pld, status);
paySetData(pld, length, frame);
if(uartSendPacket(packet)) {
return 1;
} else {
ppoolReturnFullPacket(packet);
return 0;
}
}
static void cmdGetGyroCalibParam(MacPacket packet) {
//Payload pld = macGetPayload(packet);
//unsigned char status = payGetStatus(pld);
//unsigned char* frame = payGetData(pld);
unsigned int srcAddr = macGetSrcAddr(packet);
Payload pld;
MacPacket response;
response = radioRequestPacket(12);
if(response == NULL) { return; }
macSetDestAddr(response, srcAddr);
pld = response->payload;
paySetData(pld, 12, gyroGetCalibParam());
paySetStatus(pld, 0);
paySetType(pld, CMD_GET_GYRO_CALIB_PARAM);
while(!radioEnqueueTxPacket(response));
}
/*-----------------------------------------------------------------------------
* AUX functions
-----------------------------------------------------------------------------*/
static void cmdEcho(MacPacket packet) {
Payload pld = macGetPayload(packet);
unsigned char status = payGetStatus(pld);
unsigned char* frame = payGetData(pld);
unsigned int length = payGetDataLength(pld);
unsigned int srcAddr = macGetSrcAddr(packet);
MacPacket response;
response = radioRequestPacket(length);
if(response == NULL) { return; }
macSetDestAddr(response, srcAddr);
pld = response->payload;
paySetData(pld, length, frame);
paySetStatus(pld, status);
paySetType(pld, CMD_ECHO);
while(!radioEnqueueTxPacket(response));
}
/*****************************************************************************
* Function Name : test_radio
* Description : Send out a packet containing the data in the array pointed to
* by the 'data' argument passed in.
* Parameters : type - The type field of the radio test packet
* status - Status field of radio test packet (not yet used)
* length - The length of the payload data array
* data - Pointer to the character array containing the payload
* data to send back
* Return Value : success indicator - 0 for failed, 1 for succeeded
*****************************************************************************/
unsigned char test_radio(unsigned char type, unsigned char status,\
unsigned char length, unsigned char* data)
{
MacPacket packet;
Payload pld;
// Get a new packet from the pool
packet = radioRequestPacket(length);
if(packet == NULL) return 0;
//macSetDestAddr(RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetType(pld, type);
paySetStatus(pld, status);
paySetData(pld, length, data);
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
return 1; //success
}
static void cmdCamParamRequest(MacPacket packet) {
Payload pld;
CamParamStruct params;
MacPacket response;
pld = macGetPayload(packet);
camGetParams(¶ms);
response = radioRequestPacket(sizeof(CamParamStruct));
if(response == NULL) { return; }
macSetDestAddr(response, macGetSrcAddr(packet));
pld = macGetPayload(response);
paySetType(pld, CMD_CAM_PARAM_RESPONSE);
paySetStatus(pld, 0);
paySetData(pld, sizeof(CamParamStruct), (unsigned char*)¶ms);
while(!radioEnqueueTxPacket(response));
}
/*****************************************************************************
* Function Name : test_gyro
* Description : Create and send out over the radio a number of test packets that
* contain the three X,Y, and Z values read from the gyro.
* Parameters : type - The type field of the gyro test packet
* status - Status field of gyro test packet (not yet used)
* length - The length of the payload data array
* data - not used
* Return Value : success indicator - 0 for failed, 1 for succeeded
*****************************************************************************/
unsigned char test_gyro(unsigned char type, unsigned char status,\
unsigned char length, unsigned char* data)
{ MacPacket packet;
Payload pld;
// refresh MPU reading
mpuUpdate();
// Get a new packet from the pool
packet = radioRequestPacket(sizeof(mpu_data));
if(packet == NULL) return 0;
//macSetDestAddr(RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetStatus(pld, STATUS_UNUSED);
paySetType(pld, type);
// Read gyro data into the payload
memcpy(payGetData(pld), & mpu_data, sizeof(mpu_data)); // copy gyro data to packet
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
return 1; //success
}
// ====== Camera and Vision ===================================================
// TODO: Use a struct to simplify the packetization
static void cmdRequestRawFrame(MacPacket packet) {
unsigned int srcAddr, srcPan, height, width, i, temp;
unsigned int sent, to_send, block_size = 75;
MacPacket response;
Payload pld;
CamFrame frame;
CamRow *row;
CvResultStruct info;
srcAddr = macGetSrcAddr(packet);
srcPan = macGetSrcPan(packet);
frame = NULL;
while(frame == NULL) {
frame = camGetFrame();
}
cvProcessFrame(frame, &info);
height = DS_IMAGE_ROWS;
width = DS_IMAGE_COLS;
for(i = 0; i < height; i++) {
row = &(frame->pixels[i]);
to_send = width;
while(to_send > 0) {
response = radioRequestPacket(block_size + 6);
if(response == NULL) { continue; }
pld = macGetPayload(response);
paySetType(pld, CMD_RAW_FRAME_RESPONSE);
paySetStatus(pld, 0);
macSetDestAddr(response, srcAddr);
macSetDestPan(response, srcPan);
temp = frame->frame_num;
paySetData(pld, 2, (unsigned char *)&temp);
temp = i;
payAppendData(pld, 2, 2, (unsigned char*)&temp);
temp = width - to_send;
payAppendData(pld, 4, 2, (unsigned char*)&temp);
temp = (block_size < to_send) ? block_size : to_send;
payAppendData(pld, 6, temp, *row + (width - to_send));
while(!radioEnqueueTxPacket(response));
to_send = to_send - temp;
}
}
sent = 0;
while(!sent) {
response = radioRequestPacket(10);
if(response == NULL) { continue; }
pld = macGetPayload(response);
paySetType(pld, CMD_CENTROID_REPORT);
paySetStatus(pld, 1);
macSetDestAddr(response, srcAddr);
macSetDestPan(response, srcPan);
temp = info.centroid[0];
paySetData(pld, 2, (unsigned char*)&temp);
temp = info.centroid[1];
payAppendData(pld, 2, 2, (unsigned char*)&temp);
temp = info.max[0];
payAppendData(pld, 4, 2, (unsigned char*)&temp);
temp = info.max[1];
payAppendData(pld, 6, 2, (unsigned char*)&temp);
temp = info.max_lum;
payAppendData(pld, 8, 1, (unsigned char*)&temp);
temp = info.avg_lum;
payAppendData(pld, 9, 1, (unsigned char*)&temp);
while(!radioEnqueueTxPacket(response));
sent = 1;
}
camReturnFrame(frame);
}
/*****************************************************************************
* Function Name : test_dflash
* Description : Write four different strings to a page in the data flash,
* then read them back and send their contents out over the
* radio. Bonus points if you can identify the film without
* reverting to the internet.
* Parameters : type - The type field of the dflash test packet
* status - Status field of the dflash test packet (not yet used)
* length - The length of the payload data array
* data - not used
* Return Value : success indicator - 0 for failed, 1 for succeeded
*****************************************************************************/
unsigned char test_dflash(unsigned char type, unsigned char status,
unsigned char length, unsigned char* data)
{
MacPacket packet;
Payload pld;
char mem_data[256] = {};
//char* str1 = "You must be here to fix the cable."; // 38+1
char str1[] = "D"; // 38+1
char str2[] = "Lord. You can imagine where it goes from here."; //46+1
char str3[] = "He fixes the cable?"; //19+1
char str4[] = "Don't be fatuous, Jeffrey."; //26+1
int page = 0x100;
strcpy(mem_data, str1);
strcpy(mem_data + strlen(str1), str2);
strcpy(mem_data + strlen(str1) + strlen(str2), str3);
strcpy(mem_data + strlen(str1) + strlen(str2) + strlen(str3), str4);
// Write into dfmem
dfmemWrite((unsigned char *)(mem_data), sizeof(mem_data), page, 0, 1);
// ---------- string 1 -----------------------------------------------------
// Get a new packet from the pool
packet = radioRequestPacket(strlen(str1));
if(packet == NULL) return 0;
//macSetDestAddr(packet, RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetStatus(pld, STATUS_UNUSED);
paySetType(pld, type);
// Read out dfmem into the payload
dfmemRead(page, 0, strlen(str1), payGetData(pld));
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
// ---------- string 2 -----------------------------------------------------
// Get a new packet from the pool
packet = radioRequestPacket(strlen(str2));
if(packet == NULL) return 0;
//macSetDestAddr(packet, RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetStatus(pld, STATUS_UNUSED);
paySetType(pld, type);
// Read out dfmem into the payload
dfmemRead(page, strlen(str1), strlen(str2), payGetData(pld));
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
// ---------- string 3 -----------------------------------------------------
// Get a new packet from the pool
packet = radioRequestPacket(strlen(str3));
if(packet == NULL) return 0;
//macSetDestAddr(packet, RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetStatus(pld, STATUS_UNUSED);
paySetType(pld, type);
// Read out dfmem into the payload
dfmemRead(page, strlen(str1) + strlen(str2), strlen(str3),
payGetData(pld));
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
// ---------- string 4 -----------------------------------------------------
// Get a new packet from the pool
packet = radioRequestPacket(strlen(str4));
if(packet == NULL) return 0;
//macSetDestAddr(packet, RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetStatus(pld, STATUS_UNUSED);
paySetType(pld, type);
// Read out dfmem into the payload
dfmemRead(page, strlen(str1) + strlen(str2) + strlen(str3), strlen(str4),
payGetData(pld));
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
return 1; //success
}