void clksyncHandleRequest(MacPacket packet) {
Payload pld;
MacPacket response;
unsigned long* frame;
unsigned long s0, m1, m2;
pld = macGetPayload(packet);
frame = (unsigned long*) payGetData(pld);
s0 = frame[0]; // Read requester time of flight
m1 = packet->timestamp + sclockGetOffsetTicks(); // Read local time of reception
response = radioRequestPacket(12); // Sending 3 longs
if(response == NULL) { return; }
macSetDestAddr(response, macGetSrcAddr(packet));
macSetDestPan(response, macGetSrcPan(packet));
pld = macGetPayload(response); // Create response packet
paySetType(pld, CMD_CLOCK_UPDATE_RESPONSE);
paySetData(pld, 4, (unsigned char*) &s0);
payAppendData(pld, 4, 4, (unsigned char*) & m1);
// Empty TX queue to minimize time of flight error
while(!radioTxQueueEmpty()) { radioProcess(); }
m2 = sclockGetGlobalTicks(); // Get approximate time of flight
payAppendData(pld, 8, 4, (unsigned char*) & m2);
while(!radioEnqueueTxPacket(response)) { radioProcess(); }
radioProcess();
}
// ==== PRIVATE FUNCTIONS =====================================================
static void clksyncSendRequest(SyncStatus sync) {
MacPacket packet;
Payload pld;
unsigned long s0;
packet = radioRequestPacket(4);
if(packet == NULL) { return; }
macSetDestAddr(packet, sync->master_addr);
macSetDestPan(packet, sync->master_pan);
pld = macGetPayload(packet);
paySetType(pld, CMD_CLOCK_UPDATE_REQUEST);
while(!radioTxQueueEmpty()) { radioProcess(); }
s0 = sclockGetGlobalTicks();
pld = macGetPayload(packet);
paySetData(pld, 4, (unsigned char*) &s0);
while(!radioEnqueueTxPacket(packet)) { radioProcess(); }
radioProcess();
sync->requests++;
if(sync->requests > MAX_REQUEST_ATTEMPTS) {
sync->state = STATE_REQUEST_TIMEOUT;
}
}
static void cmdDirDumpRequest(MacPacket packet) {
Payload pld;
MacPacket response;
unsigned int *frame, req_addr, req_pan, i, size;
pld = macGetPayload(packet);
frame = (unsigned int*) payGetData(pld);
req_addr = frame[0];
req_pan = frame[1];
// Send all if both addresses 0
if(req_addr == 0 && req_pan == 0) {
size = dirGetSize();
DirEntry entries[size];
dirGetEntries(entries); // Assume we get size # of entries
i = 0;
while(i < size) {
response = radioRequestPacket(sizeof(DirEntryStruct));
if(response == NULL) { continue; }
macSetDestAddr(response, macGetSrcAddr(packet));
pld = macGetPayload(response);
paySetType(pld, CMD_DIR_DUMP_RESPONSE);
paySetData(pld, sizeof(DirEntryStruct), (unsigned char*) entries[i]);
while(!radioEnqueueTxPacket(response));
i++;
}
} else {
DirEntry entry;
entry = dirQueryAddress(req_addr, req_pan);
if(entry == NULL) { return; }
while(1) {
response = radioRequestPacket(sizeof(DirEntryStruct));
if(response == NULL) { continue; }
macSetDestAddr(response, macGetSrcAddr(packet));
pld = macGetPayload(response);
paySetType(pld, CMD_DIR_DUMP_RESPONSE);
//paySetData(pld, sizeof(DirEntryStruct), (unsigned char*) &entry);
memcpy(payGetData(pld), entry, sizeof(DirEntryStruct));
while(!radioEnqueueTxPacket(response));
break;
}
}
}
static void cmdDirUpdateResponse(MacPacket packet) {
typedef struct {
unsigned long long UUID;
unsigned long timestamp;
unsigned int address;
}UpdateEntry;
Payload pld;
unsigned int i, num_entries;
DirEntry entry;
UpdateEntry *update;
pld = macGetPayload(packet);
update = (UpdateEntry*) payGetData(pld);
num_entries = payGetDataLength(pld)/sizeof(UpdateEntry);
for(i = 0; i < num_entries; i++) {
entry = dirQueryID(update[i].UUID); // Retrieve entry
if(entry == NULL) { // If not seen, create
entry = dirAddNew();
if(entry == NULL) { continue; } // Check for creation failure
// Skip updates older than current info
} else if(update[i].timestamp < entry->timestamp){ continue; }
entry->uuid = update[i].UUID;
entry->address = update[i].address;
entry->pan_id = netGetLocalPanID();
entry->timestamp = update[i].timestamp;
}
}
void trxWriteFrameBuffer(MacPacket packet) {
unsigned int i;
unsigned char phy_len;
Payload pld;
// Linearize contents in buffer
i = 0;
phy_len = packet->payload_length + MAC_HEADER_LENGTH + CRC_LENGTH;
frame_buffer[i++] = phy_len; //packet->payload_length + MAC_HEADER_LENGTH + CRC_LENGTH;
frame_buffer[i++] = packet->frame_ctrl.val.byte.LB;
frame_buffer[i++] = packet->frame_ctrl.val.byte.HB;
frame_buffer[i++] = packet->seq_num;
frame_buffer[i++] = packet->dest_pan_id.byte.LB;
frame_buffer[i++] = packet->dest_pan_id.byte.HB;
frame_buffer[i++] = packet->dest_addr.byte.LB;
frame_buffer[i++] = packet->dest_addr.byte.HB;
frame_buffer[i++] = packet->src_addr.byte.LB;
frame_buffer[i++] = packet->src_addr.byte.HB;
pld = macGetPayload(packet);
if(pld == NULL) { return; }
memcpy(frame_buffer + i, payToString(pld), payGetPayloadLength(pld));
spic1BeginTransaction();
spic1Transmit(TRX_CMD_FW);
spic1MassTransmit(phy_len, frame_buffer, phy_len*3); // 3*length microseconds timeout seems to work well
}
static void cmdSetRegulatorTempRotation(MacPacket packet) {
Quaternion *rot = payGetData(macGetPayload(packet));
rgltrSetTempRot(rot);
}
static void cmdCamParamResponse(MacPacket packet) {
Payload pld;
unsigned char *frame;
CamParamStruct *params;
LStrobeParamStruct lstrobe_params;
DirEntry entry;
unsigned int addr, pan;
pld = macGetPayload(packet);
frame = payGetData(pld);
params = (CamParamStruct*) frame;
addr = macGetSrcAddr(packet);
pan = macGetSrcPan(packet);
entry = dirQueryAddress(addr, pan);
if(entry == NULL) { return; }
entry->frame_period = params->frame_period;
entry->frame_start = params->frame_start;
lstrobe_params.period = 5*(params->frame_period/4);
lstrobe_params.period_offset = (params->frame_start/4) % (params->frame_period/4);
lstrobe_params.on_time = 625/4; // 1 ms
lstrobe_params.off_time = lstrobe_params.period - lstrobe_params.on_time;
lstrobeSetParam(&lstrobe_params);
lstrobeStart();
}
static void cmdRotateRefLocal(MacPacket packet) {
Quaternion *rot = payGetData(macGetPayload(packet));
rateApplyLocalRotation(rot);
}
static void cmdSetRateSlew(MacPacket packet) {
Payload pld = macGetPayload(packet);
Rate slew = (Rate)payGetData(pld);
rateSetGlobalSlew(slew);
}
void cmdSetSlewLimit(MacPacket packet) {
float *data;
data = (float *) payGetData(macGetPayload(packet));
slewSetLimit(*data);
}
static void cmdSetRegulatorRef(MacPacket packet) {
Payload pld = macGetPayload(packet);
Quaternion *ref = (Quaternion*)payGetData(pld);
rgltrSetQuatRef(ref);
}
static void cmdHallPIDSetInput(MacPacket packet) {
Payload pld;
unsigned int *frame;
pld = macGetPayload(packet);
frame = (unsigned int*) payGetData(pld);
hallPIDSetInput(frame[0],frame[1]);
}
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;
}
static void cmdSetRemoteControlValues(MacPacket packet) {
Payload pld = macGetPayload(packet);
float* frame = (float *)payGetData(pld);
// parameters are: thrust, steer, and elevator
rgltrSetRemoteControlValues(frame[0], frame[1], frame[2]);
}
void cmdSetTelemSubsample(MacPacket packet) {
Payload pld = macGetPayload(packet);
unsigned int* frame = (unsigned int*) payGetData(pld);
unsigned int count = frame[0];
telemSetSubsampleRate(count);
}
void __attribute__((interrupt, no_auto_psv)) _T2Interrupt(void) {
MacPacket rx_packet;
Payload rx_payload;
if (!radioRxQueueEmpty())
{
// Check for unprocessed packet
rx_packet = radioDequeueRxPacket();
if(rx_packet == NULL) return;
// Retrieve payload
rx_payload = macGetPayload(rx_packet);
// Switch on packet type
Test* test = (Test*) malloc(sizeof(Test));
if(!test) return;
test->packet = rx_packet;
switch(payGetType(rx_payload))
{
case RADIO_TEST:
test->tf = &test_radio;
queuePush(fun_queue, test);
break;
case GYRO_TEST:
test->tf = &test_gyro;
queuePush(fun_queue, test);
break;
case HALL_TEST:
test->tf = &test_hall;
queuePush(fun_queue, test);
break;
case ACCEL_TEST:
test->tf = &test_accel;
queuePush(fun_queue, test);
break;
case DFLASH_TEST:
test->tf = &test_dflash;
queuePush(fun_queue, test);
break;
case MOTOR_TEST:
test->tf = &test_motor;
queuePush(fun_queue, test);
break;
case SMA_TEST:
test->tf = &test_sma;
queuePush(fun_queue, test);
break;
default: // temporary to check out what is happening to packets
test->tf = &test_radio;
queuePush(fun_queue, test);
break;
}
}
_T2IF = 0;
}
static void cmdSetHallGains(MacPacket packet) {
Payload pld;
unsigned char *frame;
hallGains *params;
pld = macGetPayload(packet);
frame = payGetData(pld);
params = (hallGains*) frame;
hallSetGains(¶ms[0]);
}
static void cmdToggleEight(MacPacket packet) {
Payload pld = macGetPayload(packet);
unsigned char flag = *(payGetData(pld));
if(flag == 0) {
rgltrStopEight();
} else if(flag == 1) {
rgltrStartEight();
}
}
// set up velocity profile structure - assume 4 set points for now, generalize later
static void cmdSetVelProfile(MacPacket packet){
Payload pld;
unsigned char *frame;
hallVelCmd *params;
pld = macGetPayload(packet);
frame = payGetData(pld);
params = (hallVelCmd*) frame;
hallSetVelProfile(¶ms[0]);
}
static void cmdRunGyroCalib(MacPacket packet) {
Payload pld = macGetPayload(packet);
unsigned int* frame = (unsigned int*) payGetData(pld);
unsigned int count = frame[0];
radioSetWatchdogState(0);
gyroRunCalib(count);
radioSetWatchdogState(1);
}
static void cmdSetRateMode(MacPacket packet) {
Payload pld = macGetPayload(packet);
unsigned char flag = *(payGetData(pld));
if(flag == 0) {
rateDisable();
} else if(flag == 1) {
rateEnable();
}
}
static void cmdSetEstimateRunning(MacPacket packet) {
Payload pld = macGetPayload(packet);
//unsigned char status = payGetStatus(pld);
unsigned char* frame = payGetData(pld);
if (frame[0] == 0) {
attSetRunning(0);
} else {
attSetRunning(1);
}
}
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));
}
static void cmdSetRegulatorPid(MacPacket packet) {
Payload pld;
unsigned char *frame;
PidParamsStruct *params;
pld = macGetPayload(packet);
frame = payGetData(pld);
params = (PidParamsStruct*) frame;
rgltrSetYawPid(¶ms[0]);
rgltrSetPitchPid(¶ms[1]);
rgltrSetRollPid(¶ms[2]);
}
// ====== Telemetry and Sensors ===============================================
static void cmdSetLogging(MacPacket packet) {
Payload pld;
unsigned char *frame, flag;
pld = macGetPayload(packet);
frame = payGetData(pld);
flag = frame[0];
if(flag) {
telemStartLogging();
} else {
telemStopLogging();
}
}
void cmdPushFunc(MacPacket rx_packet) {
Payload rx_payload;
unsigned char command;
rx_payload = macGetPayload(rx_packet);
if(rx_payload != NULL) {
command = payGetType(rx_payload);
if(command < MAX_CMD_FUNC && cmd_func[command] != NULL) {
rx_payload->test = cmd_func[command];
carrayAddTail(fun_queue, rx_packet);
} else {
cmdError(); // halt on error - could also just ignore....
}
}
}
static void cmdSetRegulatorRateFilter(MacPacket packet) {
Payload pld;
unsigned int *frame;
RateFilterParamsStruct params;
pld = macGetPayload(packet);
frame = (unsigned int*) payGetData(pld);
params.order = frame[0];
params.type = frame[1];
params.xcoeffs = frame + 2; // Order + 1 floats per array;
params.ycoeffs = params.xcoeffs + (params.order + 1); // Typed pointer magic
rgltrSetYawRateFilter(¶ms);
rgltrSetPitchRateFilter(¶ms);
rgltrSetRollRateFilter(¶ms);
}
/*-----------------------------------------------------------------------------
* 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));
}
void clksyncHandleResponse(MacPacket packet) {
Payload pld;
unsigned long* frame;
unsigned long s0, m1, m2, s3;
long residual_offset;
pld = macGetPayload(packet);
frame = (unsigned long *) payGetData(pld);
s0 = frame[0];
m1 = frame[1];
m2 = frame[2];
s3 = packet->timestamp + sclockGetOffsetTicks();
residual_offset = (s0 + s3 - m1 - m2)/2;
status.accumulator += residual_offset;
status.responses++;
if(status.responses >= SAMPLES_PER_ITERATION) {
clksyncProcessSamples(&status);
}
}
// TODO: Move mac packet creation from Radio_DMA to Mac_Packet
void telemSendB(unsigned int addr) {
MacPacket packet;
Payload pld;
TelemetryStructB telemetryB;
// Populate the telemetry fields
telemPopulateB(&telemetryB);
// Create a radio packet
packet = radioRequestPacket(TELEMETRY_B_SIZE);
if(packet == NULL) { return; }
macSetDestAddr(packet, addr);
macSetDestPan(packet, netGetLocalPanID());
// Write the telemetry struct into the packet payload
pld = macGetPayload(packet);
paySetType(pld, CMD_RESPONSE_TELEMETRY);
paySetData(pld, TELEMETRY_B_SIZE, (unsigned char *) &telemetryB);
if(!radioEnqueueTxPacket(packet)) {
radioReturnPacket(packet); // Delete packet if append fails
}
}
