Well, I’m lost. I can’t find a good way to plot the COP from the first three satellites used in the fix. My functions from something I wrote 5 years ago need GHA, Declination, Altitude. I’m using azimuth for GHA, altitude is altitude, and I’m using altitude again for declination. I figure I can transform these if I can get something working first. I tried to shoe horn some code and it ran all night endlessly, yikes. I tried something different(hey, not usually insane, not time to go there)and I got an array out of bounds.
I used @sterretje 's and @glv 's code to draw the azimuth altitude points on a Pimage after reading from saved GPS data. I saved that image and then used code to draw a sphere with that image draped.
I added my COP functions to that code and it draws the nice solid green circle. What I was hoping was that I could parse for satellites used in the fix and also get altitude and azimuth for those satellites. I know this much, it will involve combining data from GPGSV and GPGSA, but how?
Here are the functions needed to draw a circle based on an assumed latitude and longitude, and altitude and direction. Then convert that to cartesian coordinates and plot curvevertex points.
//original code from navigationalalgorithms.com and Andres Ruiz
boolean getPoints(){
released = false;
Mz = Rz(Math.toRadians(360.0) - GHA, Mz);
// Mz = Rz(Math.toRadians(360.0) - Math.toRadians(az0), Mz);
My = Ry(Math.toRadians(90.0) - dec, My);
// My = Ry( Math.toRadians(el0), My);
// My = Rx(Math.toRadians(90.0) - dec, My);
int w = 0;
for( double L0 = -180.0; L0 <= 180.0; L0 += .1 )
{
// released = false;
// println("top of For LooP; " + released);
vv = VectorSpherical2Cartesian((alt),Math.toRadians(L0) );
vy = MatrixVecProd( My, vv, vy );
vyz = MatrixVecProd( Mz, vy, vyz );
wpt[w] = C2ELat( vyz[0], vyz[1], vyz[2]);
wpt[w+1] = C2ELon( vyz[0], vyz[1], vyz[2]);
WPT = toFloatArray(wpt);
x[w] = map(WPT[w+1],radians(-180) ,radians(180),texture.width, texture.width - texture.width);
y[w] = map(WPT[w],radians(-90),(radians(90)),texture.height,texture.height - texture.height);
texture.beginDraw();
texture.point(x[w],y[w]);
texture.noFill();
texture.stroke(0,255,0);
texture.strokeWeight(3);
texture.beginShape();
texture.curveVertex(x[w],y[w]);
texture.curveVertex(x[w],y[w]);
texture.endShape();
texture.endDraw();
released = true;
}
w++;
// }
println(released);
println("Before return statement: " + released);
return released;
}
//Funcition to convert double[] to float[]
float[] toFloatArray(double[] arr) {
if (arr == null) return null;
int n = arr.length;
float[] ret = new float[n];
for (int i = 0; i < n; i++) {
ret[i] = (float)arr[i];
}
return ret;
}
// end of function to convert double[] to float[]
public double C2ELat( double x, double y, double z )
{
double[]res = new double[3];
res[0] = Math.sqrt( x*x+y*y+z*z); //R
//*B = ASIN(z/(*R));
res[1] = Math.atan2( z, Math.sqrt(x*x+y*y) ); //B
res[2] = Math.atan2( y, x ); //L
return (res[1]);
}
public double C2ELon( double x, double y, double z )
{
double[]res = new double[3];
res[0] = Math.sqrt( x*x+y*y+z*z); //R
res[1] = Math.atan2( z, Math.sqrt(x*x+y*y) ); //B
res[2] = Math.atan2( y, x ); //L
return (res[2]);
}
public double[] E2C( double B, double L, double R )
{
double[]res = new double[3];
res[0] = R*Math.cos((B))*Math.cos((L));
res[1] = R*Math.cos((B))*Math.sin((L));
res[2] = R*Math.sin((B));
return(res);
}
public double[][] Rx( double a, double[][] M ){
M[0][0] = 1.0;
M[1][0] = 0.0;
M[2][0] = 0.0;
M[0][1] = 0.0;
M[1][1] = Math.cos(a); //Math.cos(Math.toRadians(a));
M[2][1] = Math.sin(a); //Math.sin(Math.toRadians(a));
M[0][2] = 0.0;
M[1][2] = -Math.sin(a); //-Math.sin(Math.toRadians(a));
M[2][2] = Math.cos(a); //Math.cos(Math.toRadians(a));
return(M);
}
public double[][] Ry( double a, double[][] M ){
M[0][0] = Math.cos(a);
M[1][0] = 0.0;
M[2][0] = -Math.sin(a);
M[0][1] = 0.0;
M[1][1] = 1.0;
M[2][1] = 0.0;
M[0][2] = Math.sin(a);
M[1][2] = 0.0;
M[2][2] = Math.cos(a);
return(M);
}
public double[][] Rz( double a, double[][] M ){
M[0][0] = Math.cos(a); //Math.cos(a);
M[1][0] = Math.sin(a);
M[2][0] = 0.0;
M[0][1] = -Math.sin(a);
M[1][1] = Math.cos(a);
M[2][1] = 0.0;
M[0][2] = 0.0;
M[1][2] = 0.0;
M[2][2] = 1.0;
return(M);
}
public double[] MatrixVecProd( double[][] A, double[] v, double[] res ) {
int i,j;
int n = 3;
for( i=0; i<n; i++ ) {
res[i] = 0.0;
for( j=0; j<n; j++ ) {
res[i] += A[i][j]*v[j];
}
}
return (res);
}
double[] C2E( double x, double y, double z ){
double B;
double L;
double R = Math.sqrt( x*x+y*y+z*z);
B = Math.asin(z/(R));
// B = Math.atan2( z, Math.sqrt(x*x+y*y) );
L = Math.atan2( y, x );
double res[] = new double[2];
res[0] = Math.toDegrees(B);
res[1] = Math.toDegrees(L);
// println(res);
return res;
}
double[] VectorSpherical2Cartesian(double B, double L){
double v[] = new double[3];
v[0] = Math.cos(B) * Math.cos(L);
v[1] = Math.cos(B) * Math.sin(L);
v[2] = Math.sin(B);
// println(Math.toDegrees(B));
// println(Math.toDegrees(L));
return(v);
}
double[] Unit(double x[]) {
return (aVector(1.0/Mod(x),x));
}
double Mod(double x[]) {
return(Math.sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]));
}
double Mod4(double x[]) {
return((x[0] * x[0] + x[1] * x[1] + x[2] * x[2]));
}
double[] Add(double x[], double y[]) {
double z[] = new double[3];
for (int i = 0; i < 3; i++) {
z[i] = x[i] + y[i];
}
return z;
}
double[] Subtract(double x[], double y[]) {
double z[] = new double[3];
for (int i = 0; i < 3; i++) {
z[i] = x[i] - y[i];
}
return z;
}
double[] aVector(double a, double x[]) {
double z[] = new double[3];
z[0] = a * x[0];
z[1] = a * x[1];
z[2] = a * x[2];
// z[3] = a * x[3];
return (z);
}
double Dot(double x[], double y[])
{
return (x[0]*y[0]+x[1]*y[1]+x[2]*y[2]);
}
double[] Cross(double x[], double y[]){
double z[] = new double[3];
z[0] = (x[1]*y[2]) - (x[2]*y[1]);
z[1] = (x[2]*y[0]) - (x[0]*y[2]);
z[2] = (x[0]*y[1]) - (x[1]*y[0]);
return (z);
}
I appreciate any help sorting out a good approach.
