软件集成和轨迹预测开源分享

2513001 2023-02-09 | zip | 0.11 MB | 次下载 | 免费

资料介绍

描述

现在将所有硬件连接到相应的引脚后，我们开始集成软件

1. 获取 MPU 读数（之前讨论过）

2. 获取 gps 读数——使用之前讨论的代码行并使用 smartDelay() 进行更新，我还在引脚 46 处连接了一个开关以从正常模式更改为轨迹模式：

if (gps.location.isValid())
{ Lat = gps.location.lat();
Long = gps.location.lng();
}
else
{
Lat = 12.82; /*my location in case there is no gps readings*/
Long = 80.04;
}
yy = gps.date.year();
//yy=2016;
//  Serial.println(yy);
mu = gps.date.month();
//mu=5;
// Serial.println(mu);
dd = gps.date.day();
// dd=4;
//  Serial.println(dd);
if (digitalRead(46) == 1)
{
if(gps.time.isValid())
{
hh = gps.time.hour();
//Serial.println(hh);
mm = gps.time.minute();
}
else
{
hh=3;
mm=0;
}
}
else if((digitalRead(46) == 0))
{
int ss=trajec();
hh = gps.time.hour();
mm = gps.time.minute();
}
//smartDelay function
static void smartDelay(unsigned long ms)
{
//Serial.println(“C”);
unsigned long start = millis();
do
{
while (Serial2.available())
gps.encode(Serial2.read());
} while (millis() – start < ms);
//Serial.println(“D”);
}

3.方位角-高度的计算：这个已经讨论过了，这里是合并的代码片段（密切元素在下面单独提到）

double ipart(double xx)
{
//Serial.println(“IPART”);
double sgn;
if (xx < 0) { sgn = -1.0; } else if (xx == 0) { sgn = 0.0; } else if (xx > 0)
{
sgn = 1.0;
}
double ret = sgn * ((int)fabs(xx));
return ret;
}
double FNdegmin(double xx)
{
//Serial.println(“DEGMIN”);
double a = ipart(xx) ;
double b = xx – a ;
double e = ipart(60 * b) ;
//   deal with carry on minutes
if ( e >= 60 )
{
e = 0 ;
a = a + 1 ;
}
return (a + (e / 100) );
}
double dayno(int dx, int mx, int yx, double fx)
{
//Serial.println(“DAY NO”);
//dno=(367 * yx) –  (int)(7*(yx + (int)((mx + 9) / 12)) / 4) + (int)(275 * mx / 9) + dx – 730531.5 + fx;
dno = 987 + dx + (fx / 24);
//Serial.print(“\ndays:”);
//Serial.println(dno);
return dno;
}
double frange(double x)
{
//Serial.println(“FRANGE”);
x = x / (2 * pi);
x = (2 * pi) * (x – ipart(x));
if (x < 0) x = x + (2 * pi); return x; } double fkep( double m, double ecc) { //  Serial.println(m);  Serial.println(ecc); //Serial.println(“FKEPA”); // Serial.println(m);  Serial.println(ecc); double e = ecc; //  do //  { dyo = e – (ecc * sin(e)) – m; //    e = e – (dyo / (1 – (ecc * cos(e)))); //    delay(1); ////Serial.print(“fabs”);Serial.print((fabs(dyo)*pow(10, 6)+1),8);Serial.print(”              “);Serial.print(“pow”);Serial.println((pow(10, -6)+1),8); //  } while (fabs(dyo)>= pow(10, -12));
//      //Serial.println(“fkepB”);
//   double v = 2 * atan(sqrt((1 + ecc) / (1 – ecc)) * tan(e / 2));
double v = m + (2 * e – 0.25 *pow(e,3) + 5/96 * pow(e,5)) * sin(m) + (1.25 * pow(e,2) – 11/24 * pow(e,4)) * sin(2*m) + (13/12 * pow(e,3) – 43/64 * pow(e,5)) * sin(3*m) + 103/96 * pow(e,4) * sin(4*m) + 1097/960 * pow(e,5) * sin(5*m);
if (v < 0)
v = v + (2 * pi);
return v;
}
double fnatan(double x, double y)
{
//Serial.println(“ATAN”);
double a = atan(y / x);
if (x < 0)
a = a + pi;
if ((y < 0) && (x > 0))
a = a + (2 * pi);
return a;
}
void AltAzCalculate(double RA, double Dec, double Lat, double Long, double hrs, double minut, double dy)
{
//Serial.println(“G”);
// Day offset and Local Siderial Time
dy = dy + 4975.5;
double LST = (100.46 + 0.985647 * dy + Long + 15 * (hrs + minut / 60) + 360) – (((int)((100.46 + 0.985647 * dy + Long + 15 * (hrs + minut / 60) + 360) / 360)) * 360);
// Hour Angle
double HA = (LST – RA + 360) – ((int)((LST – RA + 360) / 360)) * 360 ;
// HA, DEC, Lat to Alt, AZ
double x = cos(HA * (pi / 180)) * cos(Dec * (pi / 180));
double y = sin(HA * (pi / 180)) * cos(Dec * (pi / 180));
double z = sin(Dec * (pi / 180));
double xhor = x * cos((90 – Lat) * (pi / 180)) – z * sin((90 – Lat) * (pi / 180));
double yhor = y;
double zhor = x * sin((90 – Lat) * (pi / 180)) + z * cos((90 – Lat) * (pi / 180));
Azimuth = atan2(yhor, xhor) * (180 / pi) + 180;
Elevation = asin(zhor) * (180 / pi);
}
void earth()
{
//Serial.println(“B”);
M[3] = ((n[3] * rads) * d) + (L[3] – p[3]) * rads;
M[3] = frange(M[3]);
v[3] = fkep(M[3], e[3]);
r[3] = a[3] * ((1 – (pow(e[3], 2))) / (1 + (e[3] * cos(v[3]))));
x[3] = r[3] * cos(v[3] + p[3] * rads);
y[3] = r[3] * sin(v[3] + p[3] * rads);
z[3] = 0;
}
void mainCalculations()
{
dfrac = hh + (mm / 60);
d = dayno(dd, mu, yy, dfrac);
//Serial.println(“E”);
earth();
//Serial.println(“F”);
//Serial.println(“E”);
int j;
for (j = 0; j <=9; j++)
{
if (j == 3)
continue;
if(j==9);
// Serial.println(“A”);
M[j] = ((n[j] * rads) * d) + (L[j] – p[j]) * rads;
if(j==9);
//   Serial.println(“B”);
M[j] = frange(M[j]);
if(j==9);
// Serial.println(“C”);
v[j] = fkep(M[j], e[j]);
if(j==9);
// Serial.println(“D”);
r[j] = a[j] * ((1 – pow(e[j], 2)) / (1 + (e[j] * cos(v[j]))));
x[j] = r[j] * (cos(o[j] * rads) * cos(v[j] + p[j] * rads – o[j] * rads) – sin(o[j] * rads) * sin(v[j] + p[j] * rads – o[j] * rads) * cos(i[j] * rads));
y[j] = r[j] * (sin(o[j] * rads) * cos(v[j] + p[j] * rads – o[j] * rads) + cos(o[j] * rads) * sin(v[j] + p[j] * rads – o[j] * rads) * cos(i[j] * rads));
z[j] = r[j] * (sin(v[j] + p[j] * rads – o[j] * rads)) * sin(i[j] * rads);
Xi[j] = x[j] – x[3];
Yi[j] = y[j] – y[3];
Zi[j] = z[j];
Xq[j] = Xi[j];
Yq[j] = (Yi[j] * cos(ec)) – (Zi[j] * sin(ec));
Zq[j] = (Yi[j] * sin(ec)) + (Zi[j] * cos(ec));
ra[j] = fnatan(Xq[j], Yq[j]);
dec[j] = atan(Zq[j] / sqrt(pow(Xq[j], 2.0) + pow(Yq[j], 2.0)));
// Serial.println(j);
}
//Serial.println(“H”);
double alpha = FNdegmin((ra[pno] * degs) / 15);
double delta = FNdegmin(dec[pno] * degs);
//Serial.println(“G”);
AltAzCalculate((alpha * 15), delta, Lat, Long, hh, mm, d);
}

这是 2013 年 8 月 16 日的密切元素

double i[10] = {0.0, 7.0052, 3.3949, 0.0, 1.8496, 1.3033, 2.4869, 0.7728, 1.7692, 17.1695};
double o[10] = {0.0, 48.493, 76.804, 0.0, 49.668, 100.629, 113.732, 73.989, 131.946, 110.469};
double p[10] = {0.0, 77.669, 131.99, 103.147, 336.322, 14.556, 91.500, 169.602, 6.152, 223.486};
double a[10] = {0.0, 0.387098, 0.723327, 1.0000, 1.523762, 5.20245, 9.52450, 19.1882, 29.9987, 39.2766};
double n[10] = {0.0, 4.09235, 1.60215, 0.985611, 0.523998, 0.083099, 0.033551, 0.011733, 0.006002, 0.004006};
double e[10] = {0.0, 0.205645 , 0.006769, 0.016679, 0.093346, 0.048892, 0.055724, 0.047874, 0.009816, 0.246211};
double L[10] = {0.0, 93.8725, 233.5729, 324.5489, 82.9625, 87.9728, 216.6279, 11.9756, 335.0233, 258.8717};

4.伺服映射和伺服进给（之前讨论过）

5. 行星选择——我用一个小电位器来选择不同的行星：

int planetInput(int potval)
{
if ((potval >= 0) && (potval <= 90)) { Serial.print(“Mercury”); return 1; } if ((potval >= 91) && (potval <= 191)) { Serial.print(“Venus”); return 2; } if ((potval >= 192) && (potval <= 292)) { Serial.print(“Mars”); return 4; } if ((potval >= 293) && (potval <= 393)) { Serial.print(“Jupiter”); return 5; } if ((potval >= 394) && (potval <= 494)) { Serial.print(“Saturn”); return 6; } if ((potval >= 495) && (potval <= 595)) { Serial.print(“Uranus”); return 7; } if ((potval >= 596) && (potval <= 696)) { Serial.print(“Neptune”); return 8; } if ((potval >= 697) && (potval <= 1023))
{
Serial.print(“Pluto”);
return 9;
}
}

6 . 轨迹预测——我用一个开关从正常模式切换到轨迹模式在这种模式下，伺服系统移动得有点快，给出了行星在一整天的未来位置。这是片段：

int trajec()
{
hh=0;
mm=0;
while(hh<24&& (digitalRead(46) == 0))
{
while(mm<60&& (digitalRead(46) == 0))
{
mainCalculations();
nyaw = 360 – yaw;
Azim = Azimuth – nyaw;
Azim -= 90;
while (Azim < 0)
Azim = 360.0 – abs(Azim);
Azi = map(Azim, 0, 360, 5, 29);
Az = (int)Azi;
Elev = map(Elevation, -90, 90, 2, 178);
El = (int)Elev;
myservoAz.write(Az);
myservoEl.write(El);
mm++;
Serial.print(hh); Serial.print(“:”); Serial.println(mm);
}
mm=0;
hh++;
while(hh==19)
hh=20;
Serial.println(“good”);
}
return 5;
}