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