void pbuffInit(PoolBuff pbuff, unsigned int size, PoolBuffItem *items) {
unsigned int i;
pbuff->valid = 0;
pbuff->active = carrayCreate(size);
if(pbuff->active == NULL) { return; }
pbuff->idle = carrayCreate(size);
if(pbuff->idle == NULL) {
carrayDelete(pbuff->active);
return;
}
pbuff->valid = 1;
for(i = 0; i < size; i++) {
pbuffReturn(pbuff, items[i]);
}
}
// =============== Public Functions ============================================
unsigned int cmdSetup(unsigned int queue_size) {
unsigned int i;
input_queue = carrayCreate(queue_size);
if(input_queue == NULL) {
return 0;
}
// initialize the array of func pointers with Nop()
for(i = 0; i < MAX_CMD_FUNC_SIZE; ++i) {
cmd_func[i] = &cmdNop;
}
cmd_func[CMD_ADDRESS_REQUEST] = &cmdAddressRequest;
cmd_func[CMD_ADDRESS_OFFER] = &cmdAddressOffer;
cmd_func[CMD_ADDRESS_ACCEPT] = &cmdAddressAccept;
cmd_func[CMD_DIR_UPDATE_REQUEST] = &cmdDirUpdateRequest;
cmd_func[CMD_DIR_UPDATE_RESPONSE] = &cmdDirUpdateResponse;
cmd_func[CMD_DIR_DUMP_REQUEST] = &cmdDirDumpRequest;
cmd_func[CMD_DIR_DUMP_RESPONSE] = &cmdDirDumpResponse;
cmd_func[CMD_CLOCK_UPDATE_REQUEST] = &cmdRequestClockUpdate;
cmd_func[CMD_CLOCK_UPDATE_RESPONSE] = &cmdResponseClockUpdate;
cmd_func[CMD_SET_ESTIMATE_RUNNING] = &cmdSetEstimateRunning;
cmd_func[CMD_ECHO] = &cmdEcho;
cmd_func[CMD_ROTATE_REF_GLOBAL] = &cmdRotateRefGlobal;
cmd_func[CMD_ROTATE_REF_LOCAL] = &cmdRotateRefLocal;
cmd_func[CMD_SET_TEMP_ROT] = &cmdSetRegulatorTempRotation;
cmd_func[CMD_SET_REGULATOR_OFFSETS] = &cmdSetRegulatorOffsets;
cmd_func[CMD_SET_REGULATOR_MODE] = &cmdSetRegulatorMode;
cmd_func[CMD_SET_REGULATOR_REF] = &cmdSetRegulatorRef;
cmd_func[CMD_SET_REGULATOR_PID] = &cmdSetRegulatorPid;
cmd_func[CMD_SET_REGULATOR_RATE_FILTER] = &cmdSetRegulatorRateFilter;
cmd_func[CMD_SET_RATE_MODE] = &cmdSetRateMode;
cmd_func[CMD_SET_RATE_SLEW] = &cmdSetRateSlew;
cmd_func[CMD_TOGGLE_EIGHT] = &cmdToggleEight;
cmd_func[CMD_RAW_FRAME_REQUEST] = &cmdRequestRawFrame;
cmd_func[CMD_RAW_FRAME_RESPONSE] = &cmdResponseRawFrame;
cmd_func[CMD_SET_BACKGROUND_FRAME] = &cmdSetBackgroundFrame;
cmd_func[CMD_CAM_PARAM_REQUEST] = &cmdCamParamRequest;
cmd_func[CMD_CAM_PARAM_RESPONSE] = &cmdCamParamResponse;
cmd_func[CMD_SET_RC_VALUES] = &cmdSetRemoteControlValues;
cmd_func[CMD_RECORD_TELEMETRY] = &cmdRecordTelemetry;
cmd_func[CMD_REQUEST_TELEMETRY] = &cmdRequestTelemetry;
cmd_func[CMD_RESPONSE_TELEMETRY] = &cmdResponseTelemetry;
cmd_func[CMD_RECORD_SENSOR_DUMP] = &cmdSetLogging;
cmd_func[CMD_GET_MEM_CONTENTS] = &cmdGetMemContents;
cmd_func[CMD_RUN_GYRO_CALIB] = &cmdRunGyroCalib;
cmd_func[CMD_GET_GYRO_CALIB_PARAM] = &cmdGetGyroCalibParam;
cmd_func[CMD_SET_HP] = &cmdSetHP;
cmd_func[CMD_ZERO_ESTIMATE] = &cmdZeroEstimate;
cmd_func[CMD_REQUEST_ATTITUDE] = &cmdRequestAttitude;
cmd_func[CMD_RESPONSE_ATTITUDE] = &cmdResponseAttitude;
cmd_func[CMD_SET_TELEM_SUBSAMPLE] = &cmdSetTelemSubsample;
cmd_func[CMD_SET_SLEW_LIMIT] = &cmdSetSlewLimit;
cmd_func[CMD_TOGGLE_STREAMING] = &cmdToggleStreaming;
cmd_func[CMD_SET_VEL_PROFILE] = &cmdSetVelProfile;
cmd_func[CMD_SET_HALL_INPUT] = &cmdHallPIDSetInput;
cmd_func[CMD_SET_HALL_GAINS] = &cmdSetHallGains;
cmd_func[CMD_HALL_PID_ON] = &cmdHallPIDOn;
return 1;
}
int main() {
// Processor Initialization
SetupClock();
SwitchClocks();
SetupPorts();
sclockSetup();
LED_1 = 1;
LED_2 = 1;
LED_3 = 1;
// Message Passing
fun_queue = carrayCreate(FUN_Q_LEN);
cmdSetup();
// Radio setup
radioInit(RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetChannel(RADIO_CHANNEL);
radioSetSrcAddr(RADIO_SRC_ADDR);
radioSetSrcPanID(RADIO_PAN_ID);
uart_tx_packet = NULL;
uart_tx_flag = 0;
//uartInit(&cmdPushFunc);
tactileInit();
// Need delay for encoders to be ready
delay_ms(100);
amsEncoderSetup();
mpuSetup();
tiHSetup();
dfmemSetup();
telemSetup();
adcSetup();
pidSetup();
LED_1 = 0;
LED_3 = 1;
while(1){
// Send outgoing radio packets
radioProcess();
/*
// Send outgoing uart packets
if(uart_tx_flag) {
uartSendPacket(uart_tx_packet);
uart_tx_flag = 0;
}*/
checkTactileBuffer();
// move received packets to function queue
while (!radioRxQueueEmpty()) {
// Check for unprocessed packet
rx_packet = radioDequeueRxPacket();
if(rx_packet != NULL) {
cmdPushFunc(rx_packet);
}
}
// process commands from function queue
while(!carrayIsEmpty(fun_queue)) {
rx_packet = carrayPopHead(fun_queue);
unsigned int rx_src_addr = rx_packet->src_addr.val;
if(rx_packet != NULL) {
rx_payload = macGetPayload(rx_packet);
if(rx_payload != NULL) {
rx_function = (test_function)(rx_payload->test);
if(rx_function != NULL) {
LED_2 = ~LED_2;
(rx_function)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload), rx_src_addr);
}
}
ppoolReturnFullPacket(rx_packet);
}
}
}
return 0;
}
