void sdkInit(void) { // UART_setPacketInfo(packetInfo); statusData.have_SSDK_parameters = 0; ssdk_status.have_parameters = 0; baudrate.state = 0; extPositionValid = 0; hli_config.mode_position_control = HLI_MODE_POSCTRL_OFF; hli_config.mode_state_estimation = HLI_MODE_STATE_ESTIMATION_OFF; hli_config.position_control_axis_enable = 0; hli_config.battery_warning_voltage = BATTERY_WARNING_VOLTAGE; // set default packet rates subscription.imu = HLI_DEFAULT_PERIOD_IMU; subscription.rcdata = HLI_DEFAULT_PERIOD_RCDATA; subscription.gps = HLI_DEFAULT_PERIOD_GPS; subscription.ssdk_debug = HLI_DEFAULT_PERIOD_SSDK_DEBUG; subscription.ekf_state = HLI_DEFAULT_PERIOD_EKF_STATE; // register packets to receive packetExtPosition = registerPacket(HLI_PACKET_ID_POSITION_UPDATE, &ext_position_update); packetCmdHL = registerPacket(HLI_PACKET_ID_CONTROL_HL, &extPositionCmd); packetMotors = registerPacket(HLI_PACKET_ID_MOTORS, &motors); packetCmdLL = registerPacket(HLI_PACKET_ID_CONTROL_LL, &cmdLL); packetTimeSync = registerPacket(HLI_PACKET_ID_TIMESYNC, &timeSync); packetSSDKParams = registerPacket(HLI_PACKET_ID_SSDK_PARAMETERS, &ssdk_params); // defined in ert_main.h packetControlEnable = registerPacket(HLI_PACKET_ID_CONTROL_ENABLE, &controlEnable); packetBaudrate = registerPacket(HLI_PACKET_ID_BAUDRATE, &baudrate); packetSubscription = registerPacket(HLI_PACKET_ID_SUBSCRIPTION, &subscription); packetConfig = registerPacket(HLI_PACKET_ID_CONFIG, &hli_config); packetCamera = registerPacket(HLI_PACKET_ID_CAMERA, &camera); UART0_rxFlush(); UART0_txFlush(); // init ssdk onboard_matlab_initialize(); // init dekf, also packet subscription takes place here DEKF_init(&dekf, &extPosition); extPosition.bitfield = 0; extPosition.count = 0; extPosition.heading = 0; extPosition.x = 0; extPosition.y = 0; extPosition.z = 0; extPosition.vX = 0; extPosition.vY = 0; extPosition.vZ = 0; extPosition.qualX = 0; extPosition.qualY = 0; extPosition.qualZ = 0; extPosition.qualVx = 0; extPosition.qualVy = 0; extPosition.qualVz = 0; extPositionCmd.heading = 0; extPositionCmd.bitfield = 0; extPositionCmd.x = 0; extPositionCmd.y = 0; extPositionCmd.z = 0; extPositionCmd.vZ = 2000; extPositionCmd.vY = 2000; extPositionCmd.vZ = 2000; extPositionCmd.vYaw = 45000; startAutoBaud(); }
/********************************************************** MAIN **********************************************************/ int main (void) { static int vbat1; //battery_voltage (lowpass-filtered) unsigned int TimerT1, TimerT2; init(); buzzer(OFF); LL_write_init(); //initialize AscTec Firefly LED fin on I2C1 (not necessary on AscTec Hummingbird or Pelican) I2C1Init(); I2C1_setRGBLed(255,0,0); /* Initialize I2C controller. by Xun */ I2CInit(I2CMASTER); ADC0triggerSampling(1<<VOLTAGE_1); //activate ADC sampling generateBuildInfo(); HL_Status.up_time=0; LED(1,ON); ACISDK(); //AscTec Communication Interface: publish variables, set callbacks, etc. //update parameters stored by ACI: //... PTU_init(); //initialize camera PanTiltUnit #ifdef MATLAB //ee_read((unsigned int*)&matlab_params); //read params from eeprom onboard_matlab_initialize(); //initialize matlab code #endif while(1) { if(mainloop_trigger) { TimerT1 = T0TC; if(GPS_timeout<ControllerCyclesPerSecond) GPS_timeout++; else if(GPS_timeout==ControllerCyclesPerSecond) { GPS_timeout=ControllerCyclesPerSecond+1; GPS_Data.status=0; GPS_Data.numSV=0; } //battery monitoring ADC0getSamplingResults(0xFF,adcChannelValues); vbat1=(vbat1*14+(adcChannelValues[VOLTAGE_1]*9872/579))/15; //voltage in mV HL_Status.battery_voltage_1=vbat1; mainloop_cnt++; if(!(mainloop_cnt%10)) buzzer_handler(HL_Status.battery_voltage_1); if(mainloop_trigger) mainloop_trigger--; mainloop(); // CPU Usage calculation TimerT2 = T0TC; if (mainloop_trigger) { HL_Status.cpu_load = 1000; mainloop_overflows++; } else if (TimerT2 < TimerT1) HL_Status.cpu_load = (T0MR0 - TimerT1 + TimerT2)*1000/T0MR0; // load = "timer cycles" / "timer cycles per controller cycle" * 1000 else HL_Status.cpu_load = (TimerT2 - TimerT1)*1000/T0MR0; // load = "timer cycles" / "timer cycles per controller cycle" * 1000 } } return 0; }