So I fixed this I believe a month ago and ran into a massive issue of “where was that bug I fixed” since it wasn’t backed up / checked in.  I really haven’t found time to play with my skateboard so here I go.

So the bug revolved around the need to have the gyro nulled out.

data.GyroRawData[i] *= data.GyroVoltCorrection[i];

So by correcting for the voltage, I was altering the “raw” value.  This isn’t that big of a deal, however I use that raw value on calibrating the gyro initially.  This is something I want to get away from but until I figure out how to do that, I put the gyro on the ground for 30 seconds and allow it to get enough data to create “null” values.

I actually refactored how I was doing that entire loop.  1 loop instead of 3.  Code still needs a bit of polishing but I’m about 95% sure I can boot up the skateboard with this code base and be able to get on it.

double gyro_temp_differential;
double gyro_temp_differential_squared;
double gyro_null;
double gyro_raw;
double gyro_rates_temp_compensated;
double gyro_scale_factor;

private void applyCorrections(double maxX, double maxY, double maxZ)
{
    for (int i = 0; i < 3; i++)
    {
        data.GyroVoltCorrection[i] = ((2.5 / data.GyroVolt[i]) * 1024) / 5.0;
        gyro_raw = data.GyroRawData[i] * data.GyroVoltCorrection[i];
        data.GyroTemp[i] *= data.GyroVoltCorrection[i];

        data.AccelerationCorrectedRadians[i] = ((data.AccelerationRawData[i] - raw_accels_null[i]) *
                                                analogToVoltage) /
                                               (gOffset);

        //if we convert to rads BEFORE the correction, can the extra precision of
        //the float's mantissa increase the scale precision? probably not, but we'll
        //try it later just to be sure.

        //calculate null temp point (temp out at current vout)
        //current temp - reference temp (25 degrees celsius)
        gyro_temp_differential = (data.GyroTemp[i] - raw_gyro_temperatures_null[i]) * analogToVoltage;

        //previous var square
        gyro_temp_differential_squared = Math.Pow(gyro_temp_differential, 2);

        //current null value (center) from reference null value
        // magic numbers, I know....
        gyro_null = (raw_gyro_rates_null[i] * analogToVoltage) + 0.0086 * (gyro_temp_differential) +
                           0.03597 * (gyro_temp_differential_squared);

        //compensate current output with temp correction
        gyro_rates_temp_compensated = (gyro_raw * analogToVoltage) - gyro_null;

        //convert to degrees by applying the scale factor according to temp
        gyro_scale_factor = 12.744 + 1.26056 * gyro_temp_differential + 0.6728 * (gyro_temp_differential_squared);

        //transform P into corrected degrees per second
        data.GyroCorrectedRadians[i] = gyro_rates_temp_compensated / (gyro_scale_factor / 1000);

        // WE'RE FINISHED EXECUTING OUR CORRECTIONS.
        // WE NOW HAVE EXPORTED CORRECTED VALUES FOR P,Q,R (ROLL,PITCH,YAW) as float corrected_rates in EULER.
        // WE WANT RADS
        data.GyroCorrectedRadians[i] = Angles.DegreesToRadians(data.GyroCorrectedRadians[i]);
    }

    if (data.AccelerationCorrectedRadians_X > maxX)
        data.AccelerationCorrectedRadians_X = maxX;
    else if (data.AccelerationCorrectedRadians_X < -maxX)
        data.AccelerationCorrectedRadians_X = -maxX;


    if (data.AccelerationCorrectedRadians_Y > maxY)
        data.AccelerationCorrectedRadians_Y = maxY;
    else if (data.AccelerationCorrectedRadians_Y < -maxY)
        data.AccelerationCorrectedRadians_Y = -maxY;

    if (data.AccelerationCorrectedRadians_Z > maxZ)
        data.AccelerationCorrectedRadians_Z = maxZ;
    else if (data.AccelerationCorrectedRadians_Z < -maxZ)
        data.AccelerationCorrectedRadians_Z = -maxZ;
}