// // Fill in the details of a PCMCIA slot and initialize the device // void cf_hwr_init(struct cf_slot *slot) { static int int_init = 0; unsigned long new_state = *SA11X0_GPIO_PIN_LEVEL; if (!int_init) { int_init = 1; #ifdef CYGPKG_KERNEL // Set up interrupts cyg_drv_interrupt_create(SA1110_CF_DETECT, 99, // Priority - what goes here? (cyg_addrword_t) slot, // Data item passed to interrupt handler (cyg_ISR_t *)cf_detect_isr, (cyg_DSR_t *)cf_detect_dsr, &cf_detect_interrupt_handle, &cf_detect_interrupt); cyg_drv_interrupt_attach(cf_detect_interrupt_handle); cyg_drv_interrupt_configure(SA1110_CF_DETECT, true, true); // Detect either edge cyg_drv_interrupt_acknowledge(SA1110_CF_DETECT); cyg_drv_interrupt_unmask(SA1110_CF_DETECT); cyg_drv_interrupt_create(SA1110_CF_IRQ, 99, // Priority - what goes here? (cyg_addrword_t) slot, // Data item passed to interrupt handler (cyg_ISR_t *)cf_irq_isr, (cyg_DSR_t *)cf_irq_dsr, &cf_irq_interrupt_handle, &cf_irq_interrupt); cyg_drv_interrupt_attach(cf_irq_interrupt_handle); cyg_drv_interrupt_unmask(SA1110_CF_IRQ); #endif cyg_drv_interrupt_configure(SA1110_CF_IRQ, false, false); // Falling edge cyg_drv_interrupt_acknowledge(SA1110_CF_IRQ); } slot->attr = (unsigned char *)0x38000000; slot->attr_length = 0x200; slot->io = (unsigned char *)0x30000000; slot->io_length = 0x04000000; slot->mem = (unsigned char *)0x3C000000; slot->mem_length = 0x04000000; slot->int_num = SA1110_CF_IRQ; #ifdef CYG_HAL_STARTUP_ROM // Disable CF bus & power (idle/off) assabet_BCR(SA1110_BCR_CF_POWER | SA1110_BCR_CF_RESET | SA1110_BCR_CF_BUS, SA1110_BCR_CF_POWER_OFF | SA1110_BCR_CF_RESET_DISABLE | SA1110_BCR_CF_BUS_OFF); #endif if ((new_state & SA1110_GPIO_CF_DETECT) == SA1110_GPIO_CF_PRESENT) { if ((_assabet_BCR & SA1110_BCR_CF_POWER) == SA1110_BCR_CF_POWER_ON) { // Assume that the ROM environment has turned the bus on slot->state = CF_SLOT_STATE_Ready; } else { slot->state = CF_SLOT_STATE_Inserted; } } else { slot->state = CF_SLOT_STATE_Empty; } }
/// Main function, primary avionics functions, thread 0, highest priority. int main(int argc, char **argv) { // Data Structures struct imu imuData; struct xray xrayData; struct gps gpsData; struct nav navData; struct control controlData; uint16_t cpuLoad; // Timing variables double etime_daq, etime_datalog, etime_telemetry; // Include datalog definition #include DATALOG_CONFIG // Populate dataLog members with initial values // dataLog.saveAsDoubleNames = &saveAsDoubleNames[0]; dataLog.saveAsDoublePointers = &saveAsDoublePointers[0]; dataLog.saveAsFloatNames = &saveAsFloatNames[0]; dataLog.saveAsFloatPointers = &saveAsFloatPointers[0]; dataLog.saveAsXrayNames = &saveAsXrayNames[0]; dataLog.saveAsXrayPointers = &saveAsXrayPointers[0]; dataLog.saveAsIntNames = &saveAsIntNames[0]; dataLog.saveAsIntPointers = &saveAsIntPointers[0]; dataLog.saveAsShortNames = &saveAsShortNames[0]; dataLog.saveAsShortPointers = &saveAsShortPointers[0]; dataLog.logArraySize = LOG_ARRAY_SIZE; dataLog.numDoubleVars = NUM_DOUBLE_VARS; dataLog.numFloatVars = NUM_FLOAT_VARS; dataLog.numXrayVars = NUM_XRAY_VARS; dataLog.numIntVars = NUM_INT_VARS; dataLog.numShortVars = NUM_SHORT_VARS; double tic,time,t0=0; static int t0_latched = FALSE; int loop_counter = 0; pthread_mutex_t mutex; uint32_t cpuCalibrationData;//, last100ms, last1s, last10s; cyg_cpuload_t cpuload; cyg_handle_t loadhandle; // Populate sensorData structure with pointers to data structures // sensorData.imuData_ptr = &imuData; sensorData.gpsData_ptr = &gpsData; sensorData.xrayData_ptr = &xrayData; // Set main thread to highest priority // struct sched_param param; param.sched_priority = sched_get_priority_max(SCHED_FIFO); pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); // Setup CPU load measurements // cyg_cpuload_calibrate(&cpuCalibrationData); cyg_cpuload_create(&cpuload, cpuCalibrationData, &loadhandle); // Initialize set_actuators (PWM or serial) at zero // // init_actuators(); // set_actuators(&controlData); // Initialize mutex variable. Needed for pthread_cond_wait function // pthread_mutex_init(&mutex, NULL); pthread_mutex_lock(&mutex); // Initialize functions // init_daq(&sensorData, &navData, &controlData); init_telemetry(); while(1){ //Init Interrupts printf("intr init"); cyg_uint32 uChannel = MPC5XXX_GPW_GPIO_WKUP_7; BOOL enable = true; BOOL bInput = 1; // uChannel initialisation GPIO_GPW_EnableGPIO(uChannel, enable); GPIO_GPW_SetDirection(uChannel, bInput); GPIO_GPW_EnableInterrupt(uChannel, enable, MPC5XXX_GPIO_INTTYPE_RISING); GPIO_GPW_ClearInterrupt(uChannel); HAL_WRITE_UINT32(MPC5XXX_GPW+MPC5XXX_GPW_ME, 0x01000000); cyg_uint32 temp; HAL_READ_UINT32(MPC5XXX_ICTL_MIMR, temp); HAL_WRITE_UINT32(MPC5XXX_ICTL_MIMR, temp&0xFFFFFEFF); // wake-up interrupt service routine initialisation cyg_vector_t int1_vector = CYGNUM_HAL_INTERRUPT_GPIO_WAKEUP; cyg_priority_t int1_priority = 0; cyg_bool_t edge = 0; cyg_bool_t rise = 1; cyg_drv_interrupt_create(int1_vector, int1_priority,0,interrupt_1_isr, interrupt_1_dsr,&int1_handle,&int1); cyg_drv_interrupt_attach(int1_handle); cyg_drv_interrupt_configure(int1_vector,edge,rise); cyg_drv_interrupt_unmask(int1_vector); hasp_mpc5xxx_i2c_init(); // Append: Initialise I2C bus and I2C interrupt routine; device defined in i2c_mpc5xxx.h and .c cyg_interrupt_enable(); HAL_ENABLE_INTERRUPTS(); controlData.mode = 1; // initialize to manual mode controlData.run_num = 0; // reset run counter reset_Time(); // initialize real time clock at zero init_scheduler(); // Initialize scheduling threads_create(); // start additional threads init_datalogger(); // Initialize data logging //+++++++++++// // Main Loop // //+++++++++++// while (controlData.mode != 0) { loop_counter++; //.increment loop counter //**** DATA ACQUISITION ************************************************** pthread_cond_wait (&trigger_daq, &mutex); tic = get_Time(); get_daq(&sensorData, &navData, &controlData); etime_daq = get_Time() - tic - DAQ_OFFSET; // compute execution time //************************************************************************ //**** NAVIGATION ******************************************************** // pthread_cond_wait (&trigger_nav, &mutex); // if(navData.err_type == got_invalid){ // check if get_nav filter has been initialized // if(gpsData.navValid == 0) // check if GPS is locked // init_nav(&sensorData, &navData, &controlData);// Initialize get_nav filter // } // else // get_nav(&sensorData, &navData, &controlData);// Call NAV filter //************************************************************************ if (controlData.mode == 2) { // autopilot mode if (t0_latched == FALSE) { t0 = get_Time(); t0_latched = TRUE; } time = get_Time()-t0; // Time since in auto mode } else{ if (t0_latched == TRUE) { t0_latched = FALSE; } //reset_control(&controlData); // reset controller states and set get_control surfaces to zero } // end if (controlData.mode == 2) // Add trim biases to get_control surface commands //add_trim_bias(&controlData); //**** DATA LOGGING ****************************************************** pthread_cond_wait (&trigger_datalogger, &mutex); datalogger(&sensorData, &navData, &controlData, cpuLoad); cyg_drv_interrupt_mask(int1_vector); xray_dump = dataprinter(sensorData, xray_dump); cyg_drv_interrupt_unmask(int1_vector); etime_datalog = get_Time() - tic - DATALOG_OFFSET; // compute execution time //************************************************************************ //**** TELEMETRY ********************************************************* if(loop_counter >= BASE_HZ/TELEMETRY_HZ){ loop_counter = 0; pthread_cond_wait (&trigger_telemetry, &mutex); // // get current cpu load // cyg_cpuload_get (loadhandle, &last100ms, &last1s, &last10s); // cpuLoad = (uint16_t)last100ms; // send_telemetry(&sensorData, &navData, &controlData, cpuLoad); // // etime_telemetry = get_Time() - tic - TELEMETRY_OFFSET; // compute execution time } //************************************************************************ } //end while (controlData.mode != 0) close_scheduler(); close_datalogger(); // dump data } // end while(1) /********************************************************************** * close **********************************************************************/ pthread_mutex_destroy(&mutex); // close_actuators(); //close_nav(); return 0; } // end main