File content as of revision 48:597738b77f77:

#include "turn_sensor.h"
#include "l3g.h"

void TurnSensor::reset()
{
    angleUnsigned = 0;
}

void TurnSensor::start()
{
    timer.start(); 
    rate = 0;  
    angleUnsigned = 0;
    gyroLastUpdate = timer.read_us();
}

void TurnSensor::update()
{
    if (l3gZAvailable() == 1)
    {
        int32_t gz = l3gZReadCalibrated();
        if (gz < -500000)
        {
            // error
            return;
        }        

        // The gyro on this robot is mounted upside down; account for that here so that
        // we can have counter-clockwise be a positive rotation.
        gz = -gz;        
       
        rate = gz;
        
        // First figure out how much time has passed since the last update (dt)
        uint16_t m = timer.read_us();
        uint16_t dt = m - gyroLastUpdate;
        gyroLastUpdate = m;

        // Multiply dt by turnRate in order to get an estimation of how
        // much the robot has turned since the last update.
        // (angular change = angular velocity * time)
        int32_t d = (int32_t)rate * dt;

        // The units of d are gyro digits times microseconds.  We need
        // to convert those to the units of turnAngle, where 2^29 units
        // represents 45 degrees.  The conversion from gyro digits to
        // degrees per second (dps) is determined by the sensitivity of
        // the gyro: 0.07 degrees per second per digit.
        //
        // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree)
        // = 14680064/17578125 unit/(digit*us)
        
        if (rate > 0)
        {
          // Counter-clockwise scale factors from
          // log_190730_2.csv and log_190801_4.csv.
          const double scale = 1.012394298 * 0.998207092;
          angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125;
        }
        else
        {
          // Clockwise scale factor calculated from
          // log_190730_3.csv and then log_190801_3.csv.
          const double scale = 0.992376154 * 0.999892525;
          angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125;
        }
    }
}