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 } }