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