// 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 ;
}
