/*********************************
main entry point
*********************************/
int main ( void )
{
// Init the basic hardware
InitCnsts();
InitHardware();
// Start the main 1Khz timer and PWM timer
InitTimer1();
InitTimer2();
InitPWM();
// Initialize A2D,
InitA2D();
UART1_Init(XBEE_SPEED); // for communication and control signals
UART2_Init(LOGGING_RC_SPEED); // for spektrum RC satellite receiver
// Wait for a bit before doing rate gyro bias calibration
// TODO: test this length of wait
uint16_t i=0;for(i=0;i<60000;i++){Nop();}
// turn on the leds until the bias calibration is complete
led_on(LED_RED);
led_on(LED_GREEN);
// Initialize the AHRS
AHRS_init();
// Initialize the Controller variables
Controller_Init();
// MAIN CONTROL LOOP: Loop forever
while (1)
{
// Gyro propagation
if(loop.GyroProp){
loop.GyroProp = 0;
// Call gyro propagation
AHRS_GyroProp();
}
// Attitude control
if(loop.AttCtl){
loop.AttCtl = 0;
// Call attitude control
Controller_Update();
}
// Accelerometer correction
if( loop.ReadAccMag ){
loop.ReadAccMag = 0;
AHRS_AccMagCorrect( );
}
// Send data over modem - runs at ~20Hz
if(loop.SendSerial){
loop.SendSerial = 0;
// Send debug packet
UART1_SendAHRSpacket();
}
// Process Spektrum RC data
if(loop.ProcessSpektrum){
loop.ProcessSpektrum = 0;
UART2_ProcessSpektrumData();
}
// Read data from UART RX buffers - 500 Hz
if(loop.ReadSerial){
loop.ReadSerial = 0;
// Read serial data
//UART2_FlushRX_Spektrum();
}
// Toggle Red LED at 1Hz
if(loop.ToggleLED){
loop.ToggleLED = 0;
// Toggle LED
led_toggle(LED_RED);
}
} // End while(1)
}
/*
** ===================================================================
** Method : PE_low_level_init (component MKL25Z128LK4)
**
** Description :
** Initializes beans and provides common register initialization.
** The method is called automatically as a part of the
** application initialization code.
** This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void PE_low_level_init(void)
{
#ifdef PEX_RTOS_INIT
PEX_RTOS_INIT(); /* Initialization of the selected RTOS. Macro is defined by the RTOS component. */
#endif
/* Initialization of the SIM module */
/* PORTA_PCR4: ISF=0,MUX=7 */
PORTA_PCR4 = (uint32_t)((PORTA_PCR4 & (uint32_t)~(uint32_t)(
PORT_PCR_ISF_MASK
)) | (uint32_t)(
PORT_PCR_MUX(0x07)
));
/* Initialization of the RCM module */
/* RCM_RPFW: RSTFLTSEL=0 */
RCM_RPFW &= (uint8_t)~(uint8_t)(RCM_RPFW_RSTFLTSEL(0x1F));
/* RCM_RPFC: RSTFLTSS=0,RSTFLTSRW=0 */
RCM_RPFC &= (uint8_t)~(uint8_t)(
RCM_RPFC_RSTFLTSS_MASK |
RCM_RPFC_RSTFLTSRW(0x03)
);
/* Initialization of the FTFL_FlashConfig module */
/* Initialization of the PMC module */
/* PMC_LVDSC1: LVDACK=1,LVDIE=0,LVDRE=1,LVDV=0 */
PMC_LVDSC1 = (uint8_t)((PMC_LVDSC1 & (uint8_t)~(uint8_t)(
PMC_LVDSC1_LVDIE_MASK |
PMC_LVDSC1_LVDV(0x03)
)) | (uint8_t)(
PMC_LVDSC1_LVDACK_MASK |
PMC_LVDSC1_LVDRE_MASK
));
/* PMC_LVDSC2: LVWACK=1,LVWIE=0,LVWV=0 */
PMC_LVDSC2 = (uint8_t)((PMC_LVDSC2 & (uint8_t)~(uint8_t)(
PMC_LVDSC2_LVWIE_MASK |
PMC_LVDSC2_LVWV(0x03)
)) | (uint8_t)(
PMC_LVDSC2_LVWACK_MASK
));
/* PMC_REGSC: BGEN=0,ACKISO=0,BGBE=0 */
PMC_REGSC &= (uint8_t)~(uint8_t)(
PMC_REGSC_BGEN_MASK |
PMC_REGSC_ACKISO_MASK |
PMC_REGSC_BGBE_MASK
);
/* SMC_PMPROT: ??=0,??=0,AVLP=0,??=0,ALLS=0,??=0,AVLLS=0,??=0 */
SMC_PMPROT = 0x00U; /* Setup Power mode protection register */
/* Common initialization of the CPU registers */
/* PORTA_PCR20: ISF=0,MUX=7 */
PORTA_PCR20 = (uint32_t)((PORTA_PCR20 & (uint32_t)~(uint32_t)(
PORT_PCR_ISF_MASK
)) | (uint32_t)(
PORT_PCR_MUX(0x07)
));
/* NVIC_IPR1: PRI_6=0 */
NVIC_IPR1 &= (uint32_t)~(uint32_t)(NVIC_IP_PRI_6(0xFF));
/* ### PPG_LDD "PpgLdd1" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)PpgLdd1_Init(NULL);
/* ### BitIO_LDD "BitIoLdd1" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)BitIoLdd1_Init(NULL);
/* ### PWM_LDD "PwmLdd2" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)PwmLdd2_Init(NULL);
/* ### PWM_LDD "PwmLdd3" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)PwmLdd3_Init(NULL);
/* ### PWM_LDD "PwmLdd4" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)PwmLdd4_Init(NULL);
/* ### PWM_LDD "PwmLdd6" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)PwmLdd6_Init(NULL);
/* ### PWM_LDD "PwmLdd1" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)PwmLdd1_Init(NULL);
/* ### PWM_LDD "PwmLdd5" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)PwmLdd5_Init(NULL);
/* ### Asynchro serial "Controller" init code ... */
Controller_Init();
/* ### BitIO_LDD "BitIoLdd2" component auto initialization. Auto initialization feature can be disabled by component property "Auto initialization". */
(void)BitIoLdd2_Init(NULL);
__EI();
}
