void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ hw_init(); sys_time_init(); #ifdef LED led_init(); #endif #ifdef ADC adc_init(); #endif #endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #ifdef USE_INFRARED ir_init(); #endif #ifdef USE_GYRO gyro_init(); #endif #ifdef USE_GPS gps_init(); #endif #ifdef USE_UART0 Uart0Init(); #endif #ifdef USE_UART1 Uart1Init(); #endif #ifdef USE_UART2 Uart2Init(); #endif #ifdef USE_UART3 Uart3Init(); #endif #ifdef USE_USB_SERIAL VCOM_init(); #endif #ifdef USE_GPIO GpioInit(); #endif #ifdef USE_I2C0 i2c0_init(); #endif #ifdef USE_I2C1 i2c1_init(); #endif #ifdef USE_I2C2 i2c2_init(); #endif /************* Links initialization ***************/ #if defined USE_SPI spi_init(); #endif #if defined MCU_SPI_LINK link_mcu_init(); #endif #ifdef MODEM modem_init(); #endif /************ Internal status ***************/ h_ctl_init(); v_ctl_init(); estimator_init(); #ifdef ALT_KALMAN alt_kalman_init(); #endif nav_init(); modules_init(); /** - start interrupt task */ int_enable(); /** wait 0.5s (historical :-) */ sys_time_usleep(500000); #if defined GPS_CONFIGURE gps_configure_uart(); #endif #if defined DATALINK #if DATALINK == XBEE xbee_init(); #endif #endif /* DATALINK */ #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN); #endif power_switch = FALSE; /************ Multi-uavs status ***************/ #ifdef TRAFFIC_INFO traffic_info_init(); #endif }
void mcu_init(void) { mcu_arch_init(); #ifdef PERIPHERALS_AUTO_INIT sys_time_init(); #ifdef USE_LED led_init(); #endif /* for now this means using spektrum */ #if defined RADIO_CONTROL & defined RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT & defined RADIO_CONTROL_BIND_IMPL_FUNC RADIO_CONTROL_BIND_IMPL_FUNC(); #endif #ifdef USE_UART0 uart0_init(); #endif #ifdef USE_UART1 uart1_init(); #endif #ifdef USE_UART2 uart2_init(); #endif #ifdef USE_UART3 uart3_init(); #endif #ifdef USE_UART4 uart4_init(); #endif #ifdef USE_UART5 uart5_init(); #endif #ifdef USE_I2C0 i2c0_init(); #endif #ifdef USE_I2C1 i2c1_init(); #endif #ifdef USE_I2C2 i2c2_init(); #endif #ifdef USE_ADC adc_init(); #endif #ifdef USE_USB_SERIAL VCOM_init(); #endif #if USE_SPI #if SPI_MASTER #if USE_SPI0 spi0_init(); #endif #if USE_SPI1 spi1_init(); #endif #if USE_SPI2 spi2_init(); #endif #if USE_SPI3 spi3_init(); #endif spi_init_slaves(); #endif // SPI_MASTER #if SPI_SLAVE #if USE_SPI0_SLAVE spi0_slave_init(); #endif #if USE_SPI1_SLAVE spi1_slave_init(); #endif #if USE_SPI2_SLAVE spi2_slave_init(); #endif #if USE_SPI3_SLAVE spi3_slave_init(); #endif #endif // SPI_SLAVE #endif // USE_SPI #ifdef USE_DAC dac_init(); #endif #else INFO("PERIPHERALS_AUTO_INIT not enabled! Peripherals (including sys_time) need explicit initialization.") #endif /* PERIPHERALS_AUTO_INIT */ }
int main(int argc, char** argv) { // int select = 102; int Tdelay = 10 ; long Mdelay = 10000 ; // Switch statement control variable int cnt = 30000 ; int dutcycl = 24000 ; int mot = 5000 ; int period = 3125 ; int pos = 0 ; int position = 16000 ; int stat = 0 ; // return status int valu = 0 ; //returned value int catch = 500 ; char IMU_buf[96]={0x55} ; int IMU_val[6] ; // IMU 3 axis value long int nanoseconds=0 ; long int dutcyc = 0 ; int PWM_Positions[10] = {16000,18000,20000,22000,24000,26000,28000,30000,32000,16000} ; // Initialization begins here system.init() ; dateTime.init() ; _TRISE6 = 0; // configure port as output _RE6 = 1; // set the output (active high) Turn on 6Volt supply // Init I21C2 // Init Servo motor PWM9_init() ; SET_PWM9_Period(period) ; SET_PWM9_DutyCycle(dutcycl) ; // Init A2D Channel 10- motor current // A2D_c10_init() ; Analog_init() ; // i2c2_TalkToDevice(RTC_ADDR, RTC_SET_LENGTH, STG_SET_TIME, 0, 0) ; i2c2_init(75) ; // I2C2_KHz i2c2_reset_bus() ; while (cnt>0) { switch (select) { case 0 : while (mot>0) { pos=0 ; while (pos <9) { position=PWM_Positions[pos]; SET_PWM9_DutyCycle(position) ; delay(Mdelay) ; pos++ ; } --mot ; } break ; case 1 : valu = Read_Switch_Align() ; break ; case 2 : valu = Read_Switch_Home() ; break ; case 3 : nanoseconds= RSP78_init () ; break ; case 4 : dutcyc = RSP78_Read_Sig_Period() ; break ; case 5 : valu = A2D_c10_read() ; break ; case 6 : valu = A2D_c10_sample(5,1) ; break ; case 7 : SET_PWM9_Period( period) ; break ; case 8 : SET_PWM9_DutyCycle(dutcycl) ; break ; case 9 : break ; // ********************************************* // Camera 1 Testing // ****************************************** case 10 : initializeCam1() ; break ; case 11 : setOutputCam1(0) ; break ; case 12 : setOutputCam1(1) ; break ; case 13 : setOffCam1() ; break ; case 14 : setOnCam1() ; break ; // ********************************************* // Camera 2 Testing // ***************************************** case 20 : initializeCam2() ; break ; case 21 : setOutputCam2(0) ; break ; case 22 : setOutputCam2(1) ; break ; case 23 : setOffCam2() ; break ; case 24 : setOnCam2() ; break ; // ********************************************* // LED 1 Testing // ********************************************* case 30 : initializeLED1() ; break ; case 31 : setOutputLED1(0) ; break ; case 32 : setOutputLED1(1) ; break ; case 33 : setOffLED1() ; break ; case 34 : setOnLED1() ; break ; // ********************************************* // LED2 Testing // ********************************************* case 40 : initializeLED2() ; break ; case 41 : setOutputLED2(0) ; break ; case 42 : setOutputLED2(1) ; break ; case 43 : setOffLED2() ; break ; case 44 : setOnLED2() ; break ; // ********************************************* // Servo 1 Testing // ********************************************* case 50 : initializeServo() ; break ; case 51 : setOutputServo(0) ; break ; case 52 : setOutputServo(1) ; break ; case 53 : setOffServo() ; break ; case 54 : setOnServo() ; break ; // ********************************************* // PING I2C2 Device Testing // ********************************************* case 60 : i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers break ; case 61 : stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ; break ; case 62 : setI2C2_RTCTime(RTC_I2C_TimeStampData) ; // RGB Gesture select = 63 ; break ; case 63 : getI2C2_RTCTime(RTC_I2C_TimeStampData) ; //Temp Humidity Pressure break ; case 64 : stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ; break ; case 65 : stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ; stat=i2c2_TalkToDevice(0x39, 0, NullPtr, 0, NullPtr) ; stat=i2c2_TalkToDevice(0x77, 0, NullPtr, 0, NullPtr) ; stat=i2c2_TalkToDevice(0x1D, 0, NullPtr, 0, NullPtr) ; stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ; break ; case 66 : // dt=datetime.get ; break ; case 69 : stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ; // Real Time Clock Set Harmon Time stat=i2c2_TalkToDevice(0x39, 0, NullPtr, 0, NullPtr) ; // RGBC stat=i2c2_TalkToDevice(0x77, 0, NullPtr, 0, NullPtr) ; // THP stat=i2c2_TalkToDevice(0x1D, 0, NullPtr, 0, NullPtr) ; // Accel/Magne stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ; // Gyro break ; // ********************************************* // RTC I2C2 Test both Real Time Clocks // ********************************************* case 70 : // Clear Diagnostic Countrers stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; delay(1) ; break ; case 71 : // Ping using talk to device stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ; delay(1) ; break ; case 72 : stat=i2c2_TalkToDevice(RTC_ADDR, RTC_GET_LENGTH, RTC_SET_TIME, RTC_RSP_LENGTH, RTC_GET_TIME) ; // get I2C RTC BCD string delay(1) ; break ; case 73 : stat=i2c2_TalkToDevice(RTC_ADDR, RTC_SET_LENGTH, RTC_SET_TIME, 0, NullPtr) ; // set I2C RTC BCD string delay(1) ; break ; case 74 : stat=getI2C2_RTCTime(RTC_I2C_TimeStampData) ; // Get I2C RTC into TimeStamp STring delay(1) ; break ; case 75 : stat = setI2C2_RTCTime(RTC_I2C_TimeStampData) ; // Set I2c RTC from TimeStamp STring delay(1) ; break ; case 76 : getTimeStamp(RTC_I2C_TimeStampData) ; // get PIC RTC into timestamp string delay(1) ; break ; case 77 : setPIC_RTCTime(RTC_I2C_TimeStampData) ; // set PIC RTC from timestamp string delay(1) ; break ; case 78 : delay(1) ; break ; case 79 : stat=getI2C2_RTCTime(RTC_I2C_TimeStampData) ; setPIC_RTCTime(RTC_I2C_TimeStampData) ; delay(1) ; break ; // ********************************************* // GYR I2C2 Decive Testing // ********************************************* case 80 : // Clear the Diagnostic buffer stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers delay (1) ; break ; case 81 : stat=enableIMU() ; delay (1) ; break ; case 82 : stat= dumpIMU(IMU_buf) ; delay (1) ; break ; case 83 : stat= readACC(IMU_val) ; delay (1) ; break ; case 84 : stat=readMAG(IMU_val) ; delay (1) ; break ; case 85 : stat=readGYR(IMU_val) ; delay (1) ; break ; case 86 : stat=i2c2_TalkToDevice(ACC_Addr, 0, NullPtr, 0, NullPtr) ; delay (1) ; break ; case 87 : stat=gyro3_main(argc, argv) ; delay (1) ; break ; case 88 : stat=AMG5_main(argc, argv) ; delay (1) ; break ; case 89 : delay (1) ; break ; // ********************************************* // RGB I2C2 Decive Testing // ********************************************* case 90 : stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers RGB_Configure1 () ; select=94 ; // read back values break ; case 91 : RGB_Configure2 () ; break ; case 92 : RGB_Configure3 () ; break ; case 93 : RGB_ReadStatus () ; break ; case 94 : RGB_ReadValues () ; break ; case 95 : RGB_ClearIrqs () ; break ; case 99 : stat=RGB_Configure1 () ; stat=RGB_ReadStatus () ; stat=RGB_ReadValues () ; stat=RGB_ReadStatus () ; stat=RGB_ClearIrqs () ; break ; // ********************************************* // THP I2C2 Device Testing // ********************************************* case 100 : stat= i2c2_TalkToDevice(THP_Addr,0, NullPtr, 0, NullPtr) ; // ping device break ; case 101 : // i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; stat = THP_Configure1 () ; // select=105 ; // read back values break ; case 102 : i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; stat = THP_Configure2 () ; select=106 ; // read back values break ; case 103 : stat = THP_Configure3 () ; break ; case 104 : // stat = THP_ReadStatus () ; // part of read values // select=104 ; // read back values break ; case 105 : // stat = THP_Diag_RdIrqs () ; THP_Init () ; break ; case 106 : stat = THP_Read_Values () ; break ; case 107 : // stat = THP_Diag_RdIrqs () ; break ; case 108 : // stat = THP_Init () ; // stat = THP_Diag_RdIrqs () ; break ; case 109 : stat = THP_Configure1 () ; // THP_ReadStatus () ; stat = THP_Read_Values () ; // stat = THP_ReadStatus () ; // THP_Diag_RdIrqs () ; break ; default: delay(10) ; } // end of switch // ********************************************* // SWITCH ENDS // ********************************************* if(cnt % catch==0) { delay(10) ; } delay (Tdelay) ; cnt-- ; } // end of while return (0); }