centripetal_compensation

/* Use the gps velocity information to calculate the estimated centripetal force on the UAV
 * REFER TO: http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20140002398.pdf by NASA for some centrifugal stuff.*/
void AHRS_centripetal_compensation(IMU_t * imuStruct, float longitudinalSpeed)
{


    // Firstly, lets get the vectors:
    sensor_vector_t angularRate = (sensor_vector_t) { 0 } ; // angular rate of the body (GYRO)
    sensor_vector_t bodyVelocity = (sensor_vector_t) {0}; // velocity vector of the body (UAV GPS)
    sensor_vector_t centripetalAcceleration = (sensor_vector_t) {0};

    angularRate = (sensor_vector_t){ imuStruct->gyro.x, imuStruct->gyro.y, imuStruct->gyro.z };
    bodyVelocity.x = longitudinalSpeed;

    // Now centripetal acceleration = angular rate x body velocity.
    crossProduct(&centripetalAcceleration, angularRate, bodyVelocity);

    // Finally, we can remove the centripetal acceleration from the body acceleration values:
    imuStruct->acceleration.x -= centripetalAcceleration.x;
    imuStruct->acceleration.y -= centripetalAcceleration.y;
    imuStruct->acceleration.z -= centripetalAcceleration.z;

}