int main(int argc, char *argv[])
{
int blink[3], flip[2] = {0, 0};
int do_ao_only = FALSE;
uint8_t i = 0;
if (do_ao_only) {
if (init_dac(0.0, 25.0, FALSE) < 0) {
printf("Missing Analog AO subdevice\n");
return -1;
}
while (TRUE) {
set_dac_volts(1, ((double) sine_wave[i])*0.007);
set_dac_volts(0, ((double) sine_wave[255 - i++])*0.007);
usleep(10);
// printf("%d\n", i);
}
} else {
if (init_daq(0.0, 25.0, FALSE) < 0) {
printf("Missing Analog subdevice(s)\n");
return -1;
}
if (init_dio() < 0) {
printf("Missing Digital subdevice(s)\n");
return -1;
}
set_dio_output(0);
set_dio_output(1);
set_dio_input(6);
set_dio_input(7);
put_dio_bit(0, 1);
put_dio_bit(1, 1);
blink[2] = 0;
while (1) {
get_data_sample();
if (blink[2]++ >= 100) {
printf(" \r");
printf(" %2.3fV %2.3fV %2.3fV %2.3fV %2.3fV %2.3fV %2.3fV %u %u %u %u %u %u raw %x, %x",
bmc.pv_voltage, bmc.cc_voltage, bmc.input_voltage, bmc.b1_voltage, bmc.b2_voltage, bmc.system_voltage, bmc.logic_voltage,
bmc.datain.D0, bmc.datain.D1, bmc.datain.D2, bmc.datain.D3, bmc.datain.D6, bmc.datain.D7, bmc.adc_sample[0], bmc.adc_sample[1]);
// usleep(4990);
blink[2] = 0;
if ((bmc.datain.D0 == 0)) {
if (((blink[0]++) % 150) == 0) {
flip[0] = !flip[0];
}
printf(" Flip led 0 %x ", flip[0]);
bmc.dataout.D0 = flip[0];
set_dac_volts(0, bmc.cc_voltage);
} else {
set_dac_volts(0, 0.666);
bmc.dataout.D0 = 0;
}
if ((bmc.datain.D1 == 0)) {
if (((blink[1]++) % 150) == 0) {
flip[1] = !flip[1];
}
printf(" Flip led 1 %x ", flip[1]);
set_dac_volts(1, 0.333);
bmc.dataout.D1 = flip[1];
} else {
set_dac_volts(1, 1.666);
bmc.dataout.D1 = 0;
}
}
}
}
return 0;
}
/// 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
