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; } } }
/***************************************************************************** * 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 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; } }
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(); }
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(); }
/***************************************************************************** * 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 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; }
void cmdSetTelemSubsample(MacPacket packet) { Payload pld = macGetPayload(packet); unsigned int* frame = (unsigned int*) payGetData(pld); unsigned int count = frame[0]; telemSetSubsampleRate(count); }
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]); }
static void cmdToggleEight(MacPacket packet) { Payload pld = macGetPayload(packet); unsigned char flag = *(payGetData(pld)); if(flag == 0) { rgltrStopEight(); } else if(flag == 1) { rgltrStartEight(); } }
static void cmdSetHallGains(MacPacket packet) { Payload pld; unsigned char *frame; hallGains *params; pld = macGetPayload(packet); frame = payGetData(pld); params = (hallGains*) frame; hallSetGains(¶ms[0]); }
// 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 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(); } }
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); }
void cmdHandleRadioRxBuffer(void) { Payload pld; unsigned char command, status; if ((pld = radioReceivePayload()) != NULL) { status = payGetStatus(pld); command = payGetType(pld); //Due to bugs, command may be a surprious value; check explicitly if (command <= MAX_CMD_FUNC) { cmd_func[command](status, pld->data_length, payGetData(pld)); } payDelete(pld); } return; }
/*----------------------------------------------------------------------------- * 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_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 }
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); } }
/***************************************************************************** * 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 }
static void cmdSetRegulatorMode(MacPacket packet) { unsigned char* frame = payGetData(macGetPayload(packet)); rgltrSetMode(frame[0]); }