// Called at HEARTBEAT_HZ void udb_heartbeat_callback(void) { #if (BAROMETER_ALTITUDE == 1) if (udb_heartbeat_counter % (HEARTBEAT_HZ / 40) == 0) { do_I2C_stuff(); // TODO: this should always be be called at 40Hz } #else //#if (MAG_YAW_DRIFT == 1 && HILSIM != 1) #if (MAG_YAW_DRIFT == 1) // This is a simple counter to do stuff at 4hz // if (udb_heartbeat_counter % 10 == 0) if (udb_heartbeat_counter % (HEARTBEAT_HZ / 4) == 0) { rxMagnetometer(mag_drift_callback); } #endif #endif // BAROMETER_ALTITUDE // when we move the IMU step to the MPU call back, to run at 200 Hz, remove this if (dcm_flags._.calib_finished) { dcm_run_imu_step(); } dcm_heartbeat_callback(); // this was called dcm_servo_callback_prepare_outputs(); // if (!dcm_flags._.init_finished) // { // if (udb_heartbeat_counter % (HEARTBEAT_HZ / 40) == 0) // { // dcm_run_init_step(udb_heartbeat_counter / (HEARTBEAT_HZ / 40)); // } // } if (udb_heartbeat_counter % (HEARTBEAT_HZ / 40) == 0) { if (!dcm_flags._.calib_finished) { dcm_flags._.calib_finished = dcm_run_calib_step(udb_heartbeat_counter / (HEARTBEAT_HZ / 40)); } if (!dcm_flags._.init_finished) { dcm_flags._.init_finished = gps_run_init_step(udb_heartbeat_counter / (HEARTBEAT_HZ / 40)); } } #if (HILSIM == 1) send_HILSIM_outputs(); #endif }
// Called at 40Hz void udb_servo_callback_prepare_outputs(void) { //#if (MAG_YAW_DRIFT == 1) // This is a simple counter to do stuff at 4hz // Approximate time passing between each telemetry line, even though // we may not have new GPS time data each time through. /* if (tow.WW > 0) */ tow.WW += 25 ; dcm_fourHertzCounter++ ; if ( dcm_fourHertzCounter >= 10 ) { rxMagnetometer() ; // now actually all interrupt driven but this will kickstart rxAccel() ; // now actually all interrupt driven but this will kickstart dcm_fourHertzCounter = 0 ; } //#endif int iNumSamples; int iIndex; iAnalog_Tail = iAnalog_Head&0x3f; // should actually be iAnalog_Head udb_setDSPLibInUse(true) ; if ( iAnalog_Tail > 10 ) { MDSFIR( iAnalog_Tail, &AD1_Filt[1][1][0], &AD1_Filt[0][1][0], &filter_aspgFilterX); MDSFIR( iAnalog_Tail, &AD1_Filt[1][2][0], &AD1_Filt[0][2][0], &filter_aspgFilterY); MDSFIR( iAnalog_Tail, &AD1_Filt[1][3][0], &AD1_Filt[0][3][0], &filter_aspgFilterZ); _DI(); // have to run this int disabled - should be fast iNumSamples = iAnalog_Head - iAnalog_Tail; // get any during FIR ? if ( iNumSamples ) // copy those samples to begin of buffer { for ( iIndex = 0; iIndex < iNumSamples; iIndex++ ) { AD1_Filt[0][1][iIndex] = AD1_Filt[0][1][iAnalog_Tail + iIndex]; AD1_Filt[0][2][iIndex] = AD1_Filt[0][2][iAnalog_Tail + iIndex]; AD1_Filt[0][3][iIndex] = AD1_Filt[0][3][iAnalog_Tail + iIndex]; }; iAnalog_Head = iIndex; } else iAnalog_Head = 0; _EI(); FLT_Value[1] = averageSample( &AD1_Filt[1][1][0], iAnalog_Tail ); FLT_Value[2] = averageSample( &AD1_Filt[1][2][0], iAnalog_Tail ); FLT_Value[3] = averageSample( &AD1_Filt[1][3][0], iAnalog_Tail ); // FLT_Value[1] = 0; // FLT_Value[2] = 0; // FLT_Value[3] = 0; lastGyroSamples = iAnalog_Tail; }; iI2C_Tail = iI2C_Head&0x3f; // should actually be iI2C_Head if ( iI2C_Tail > 10 ) { MDSFIR( iI2C_Tail, &AD1_Filt[1][4][0], &AD1_Filt[0][4][0], &filter_aspg_I2CX_Filter); MDSFIR( iI2C_Tail, &AD1_Filt[1][5][0], &AD1_Filt[0][5][0], &filter_aspg_I2CY_Filter); MDSFIR( iI2C_Tail, &AD1_Filt[1][6][0], &AD1_Filt[0][6][0], &filter_aspg_I2CZ_Filter); _DI(); // have to run this int disabled - should be fast iNumSamples = iI2C_Head - iI2C_Tail; // get any during FIR ? if ( iNumSamples ) // copy those samples to begin of buffer { for ( iIndex = 0; iIndex < iNumSamples; iIndex++ ) { AD1_Filt[0][4][iIndex] = AD1_Filt[0][4][iI2C_Tail + iIndex]; AD1_Filt[0][5][iIndex] = AD1_Filt[0][5][iI2C_Tail + iIndex]; AD1_Filt[0][6][iIndex] = AD1_Filt[0][6][iI2C_Tail + iIndex]; }; iI2C_Head = iIndex; } else iI2C_Head = 0; _EI(); FLT_Value[4] = averageSample( &AD1_Filt[1][4][0], iI2C_Tail ); FLT_Value[5] = averageSample( &AD1_Filt[1][5][0], iI2C_Tail ); FLT_Value[6] = averageSample( &AD1_Filt[1][6][0], iI2C_Tail ); lastAccelSamples = iI2C_Tail; }; udb_setDSPLibInUse(false) ; // udb_xaccel.value = udb_xaccel.value + (( (udb_xaccel.input>>1) - (udb_xaccel.value>>1) )>> FILTERSHIFT ) ; // udb_xrate.value = udb_xrate.value + (( (udb_xrate.input>>1) - (udb_xrate.value>>1) )>> FILTERSHIFT ) ; // udb_yaccel.value = udb_yaccel.value + (( ( udb_yaccel.input>>1) - (udb_yaccel.value>>1) )>> FILTERSHIFT ) ; // udb_yrate.value = udb_yrate.value + (( (udb_yrate.input>>1) - (udb_yrate.value>>1) )>> FILTERSHIFT ) ; // udb_zaccel.value = udb_zaccel.value + (( (udb_zaccel.input>>1) - (udb_zaccel.value>>1) )>> FILTERSHIFT ) ; // this is the spot to add per channel analog scaling analog_pin( (int)FLT_Value[gyro_x], &DIO[31] ); // see radioIn_aspg.c for maping analog_pin( (int)FLT_Value[gyro_y], &DIO[32] ); // see radioIn_aspg.c for maping analog_pin( (int)FLT_Value[gyro_z], &DIO[33] ); // see radioIn_aspg.c for maping // udb_xrate.value = FLT_Value[gyro_x]; // udb_yrate.value = FLT_Value[gyro_y]; // udb_zrate.value = FLT_Value[gyro_z]; udb_xrate.value = DIO[31].qValue; udb_yrate.value = DIO[32].qValue; udb_zrate.value = DIO[33].qValue; // this is the spot to add per channel analog scaling analog_pin( (int)FLT_Value[accel_x]*2, &DIO[34] ); // see radioIn_aspg.c for maping analog_pin( (int)FLT_Value[accel_y]*2, &DIO[35] ); // see radioIn_aspg.c for maping analog_pin( (int)FLT_Value[accel_z]*2, &DIO[36] ); // see radioIn_aspg.c for maping // udb_xaccel.value = FLT_Value[accel_x]*2; // udb_yaccel.value = FLT_Value[accel_y]*2; // udb_zaccel.value = FLT_Value[accel_z]*2; udb_xaccel.value = DIO[34].qValue; udb_yaccel.value = DIO[35].qValue; udb_zaccel.value = DIO[36].qValue; if (dcm_has_calibrated) { dcm_run_imu_step() ; } dcm_servo_callback_prepare_outputs() ; #if ( HILSIM == 1) send_HILSIM_outputs() ; #endif return ; }