void estYawDrift(void)
{
// Don't update Yaw Drift while hovering, since that doesn't work right yet
if (gps_nav_valid() && !dcm_flags._.skip_yaw_drift)
{
if ((estimatedWind[0] == 0 && estimatedWind[1] == 0) || (air_speed_magnitudeXY < WIND_NAV_AIR_SPEED_MIN))
{
dirovergndHGPS[0] = -cosine(actual_dir);
dirovergndHGPS[1] = sine(actual_dir);
dirovergndHGPS[2] = 0;
}
else
{
dirovergndHGPS[0] = -cosine(calculated_heading);
dirovergndHGPS[1] = sine(calculated_heading);
dirovergndHGPS[2] = 0;
}
}
}
void dead_reckon(void)
{
int16_t air_speed_x, air_speed_y, air_speed_z;
union longww accum;
union longww energy;
if (dcm_flags._.dead_reckon_enable == 1) // wait for startup of GPS
{
// integrate the accelerometers to update IMU velocity
IMUintegralAccelerationx.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[0]);
IMUintegralAccelerationy.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[1]);
IMUintegralAccelerationz.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[2]);
// integrate IMU velocity to update the IMU location
IMUlocationx.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationx._.W1)>>4);
IMUlocationy.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationy._.W1)>>4);
IMUlocationz.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationz._.W1)>>4);
if (dead_reckon_clock > 0)
// apply drift adjustments only while valid GPS data is in force.
// This is done with a countdown clock that gets reset each time new data comes in.
{
dead_reckon_clock --;
IMUintegralAccelerationx.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[0]);
IMUintegralAccelerationy.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[1]);
IMUintegralAccelerationz.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[2]);
IMUlocationx.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[0]);
IMUlocationy.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[1]);
IMUlocationz.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[2]);
IMUvelocityx.WW = IMUintegralAccelerationx.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[0]);
IMUvelocityy.WW = IMUintegralAccelerationy.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[1]);
IMUvelocityz.WW = IMUintegralAccelerationz.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[2]);
}
else // GPS has gotten disconnected
{
yaw_drift_reset();
dcm_flags._.gps_history_valid = 0; // restart GPS history variables
IMUvelocityx.WW = IMUintegralAccelerationx.WW;
IMUvelocityy.WW = IMUintegralAccelerationy.WW;
IMUvelocityz.WW = IMUintegralAccelerationz.WW;
}
if (gps_nav_valid() && (dcm_flags._.reckon_req == 1))
{
// compute error indications and restart the dead reckoning clock to apply them
dcm_flags._.reckon_req = 0;
dead_reckon_clock = DR_PERIOD;
locationErrorEarth[0] = GPSlocation.x - IMUlocationx._.W1;
locationErrorEarth[1] = GPSlocation.y - IMUlocationy._.W1;
locationErrorEarth[2] = GPSlocation.z - IMUlocationz._.W1;
velocityErrorEarth[0] = GPSvelocity.x - IMUintegralAccelerationx._.W1;
velocityErrorEarth[1] = GPSvelocity.y - IMUintegralAccelerationy._.W1;
velocityErrorEarth[2] = GPSvelocity.z - IMUintegralAccelerationz._.W1;
}
}