Just a couple of questions and comments:
1. Satellite GPS does not use the angle the signal is traveling at,
it uses the distance from the satellite to the device
2. I believe that you do not need to use your "firstrun" variable and loop to initialize the static variables, i believe you can just define statics when you declare them
Code:
static int myVar = 5;
and the definition will only happen the first time the function is called, so the variable won't be changed by the definition/declaration statement in successive function calls.
3. Have you run this code on a FRC? It seems like it, along with the neccesary parts of the default code, could start to cause "default_routine" to start missing cycles as some people have mentioned has happened, if this code is running in every cycle in "default_rountine".
I like the use of a 4th sensor for error correction, pretty cool. If you have used this system, how did it work, if not, post about it when you do.