Sharing your project, please try to follow the plan:
- Name of your project or research should be reflected in the topic title.
AAT(Auto Antenna Tracker) by using RTK GPS (M+ , RS+) in fixed UAV for formation flight - Background and motivation, where are you from.
Capstone design in university , South-Korea - Project steps.
We are using arduino for ATT, and we are stopping in LLA -> NED step.
( M+, We coverted NMEA->LLA)
We don’t know the way and results that we used is collect.
This is our code in arduino and this results are so splatting in experiment.
Also, in the experiment with stepping moter, they did not rotate correctly.
Is this code that changes from LLA to NED(XYZ) correct?
If it is incorrect, can you recommend other idea?
#include <SoftwareSerial.h>
#include <math.h>
// Arduino’s RXPIN & TXPIN which is pin 10 & 11.
#define RXPIN 10
#define TXPIN 11
// Set this value equal to the baud rate of your GPS
#define GPSBAUD 38400
// Initialize the NewSoftSerial library to the pins you defined above
SoftwareSerial uart_gps(RXPIN, TXPIN);
char temp = “”;
String str = “”;
String TargetStr = “$GNGGA”;
float home_LLA[3] = {37.6005476877, 126.864858245, 54.060};
float my_LLA[3] = {0, 0, 0};
float result[3]= {0, 0, 0};
float *my_NED;
void setup()
{
Serial.begin(38400);
// Sets baud rate of your GPS
uart_gps.begin(GPSBAUD);
Serial.println(“GPS Data Logginig Start”);
}
void loop()
{
// Gps Sensor Available
if(uart_gps.available())
{
// Read Sensor Data
temp = uart_gps.read();
// If One Sentence End
if(temp==’\n’)
{
// GNGGA?
if(TargetStr.equals(str.substring(0,6)))
{
Serial.println(str);
// Parsing about ','
int one = str.indexOf(",");
int two = str.indexOf(",", one+1);
int three = str.indexOf(",", two+1);
int four = str.indexOf(",", three+1);
int five = str.indexOf(",", four+1);
int six = str.indexOf(",", five+1);
int seven = str.indexOf(",",six+1);
int eight = str.indexOf(",", seven+1);
int nine = str.indexOf(",", eight+1);
int ten = str.indexOf(",", nine+1);
int eleven = str.indexOf(",", ten+1);
int twelve = str.indexOf(",", eleven+1);
int thirteen = str.indexOf(",", twelve+1);
int fourteen = str.indexOf(",", thirteen+1);
// Abstract Time & Lattitude & Longitude
String Time = str.substring(one+1, two);
String Hour = Time.substring(0,2);
String Minute = Time.substring(2,4);
String Second1 = Time.substring(4,6);
String Second2 = Time.substring(6);
String Lat = str.substring(two+1, three);
String Long = str.substring(four+1, five);
String Lat1 = Lat.substring(0,2);
String Lat2 = Lat.substring(2);
String Long1 = Long.substring(0,3);
String Long2 = Long.substring(3);
String NS = str.substring(three+1,four);
String EW = str.substring(five+1,six);
String Status = str.substring(six+1,seven);
String Satell = str.substring(seven+1, eight);
String HDOP = str.substring(eight+1, nine);
String Altitude1 = str.substring(nine+1, ten);
String Altitude2 = str.substring(ten+1, eleven);
String Altitude3 = str.substring(eleven+1, twelve);
String Altitude4 = str.substring(twelve+1, thirteen);
String DGPS = str.substring(thirteen+1, fourteen);
String Checksum = str.substring(fourteen+2);
String Check_String = str.substring(1,fourteen+1);
int Check_String_Leng = Check_String.length() + 1;
int crc = 0;
char Check_StringF[Check_String_Leng];
Check_String.toCharArray(Check_StringF, Check_String_Leng);
for(int i=0; i<(Check_String_Leng);i++)
{
crc ^= Check_StringF[i];
}
int Checksum_Leng = Checksum.length() + 1;
int ChecksumF = 0;
char ChecksumF_char[Checksum_Leng];
Checksum.toCharArray(ChecksumF_char, Checksum_Leng);
int a = ChecksumF_char[0];
if(ChecksumF_char[0]<60)
{
ChecksumF += (ChecksumF_char[0] - 48)*16;
}
else
{
ChecksumF += (ChecksumF_char[0] - 55)*16;
}
if(ChecksumF_char[1]<60)
{
ChecksumF += ChecksumF_char[1] - 48;
}
else
{
ChecksumF += ChecksumF_char[1] - 55;
}
if(ChecksumF==crc)
{
Serial.println("Communication Status ( Checksum ) : Good");
// time Calculate
int HourF = Hour.toInt() + 9; // Korea is UTC + 09:00
int MinuteF = Minute.toInt();
double SecondF = Second1.toDouble() + Second2.toDouble();
// position Calculate
float LatF = Lat1.toFloat() + Lat2.toFloat()/60;
float LongF = Long1.toFloat() + Long2.toFloat()/60;
int SatellF = Satell.toInt();
double HDOPF = HDOP.toDouble()*10.0;
double AltitudeF = Altitude1.toDouble() + Altitude3.toDouble();
Serial.print("Time : ");
Serial.print(HourF);
Serial.print(":");
Serial.print(MinuteF);
Serial.print(":");
Serial.println(SecondF,3);
Serial.print("Lattitude : ");
Serial.print(LatF, 15);
if(NS.equals("N"))
{
Serial.println(" North");
}
else if(NS.equals("S"))
{
Serial.println(" South");
}
Serial.print("Longitude : ");
Serial.print(LongF, 15);
if(EW.equals("E"))
{
Serial.println(" East");
}
else if(EW.equals("W"))
{
Serial.println(" West");
}
Serial.print("Altitude : ");
Serial.print(AltitudeF,3);
Serial.println(Altitude2);
Serial.print("GPS Status : ");
if(Status.equals("1"))
{
Serial.println("Average Single");
}
else if(Status.equals("0"))
{
Serial.println("Not Connected");
}
else
{
Serial.println("DGPS Fix");
}
Serial.print("Connected Satellite : ");
Serial.println(SatellF);
Serial.print("Horizontal Dilution of Precision : ");
Serial.println(HDOPF);
Serial.print("DGPS Status : ");
Serial.println(DGPS);
my_LLA[0] = LatF;
my_LLA[1] = LongF;
my_LLA[2] = AltitudeF;
my_NED = LLA2NED(home_LLA, my_LLA);
Serial.println("");
Serial.print("N : ");
Serial.print(my_NED[0]);
Serial.print(" , E : ");
Serial.print(my_NED[1]);
Serial.print(" , D : ");
Serial.println(my_NED[2]);
}
else
{
Serial.println("Communication Status ( Checksum ) : Bad");
}
}
// Initialize
str="";
}
else
{
// Logging String
str += temp;
}
}
}
float *LLA2NED(float *home_pos_LLA, float *cur_pos_LLA)
{
float exp = 0.08181919f;
float Ne, Ner;
float xe, ye, ze;
float xer, yer, zer;
//double _position_NED[3] = {0.0};
// LLA[0] = Latitude, LLA[1] = Longitude, LLA[2] = height
Ne = 6378137.0f / sqrtf(1.0f-expexpsinf(cur_pos_LLA[0])sinf(cur_pos_LLA[0]));
Ner = 6378137.0f / sqrtf(1.0f-expexp*sinf(home_pos_LLA[0])*sinf(home_pos_LLA[0]));
xe = (Ne + cur_pos_LLA[2])*cosf(cur_pos_LLA[0])*cosf(cur_pos_LLA[1]);
ye = (Ne + cur_pos_LLA[2])cosf(cur_pos_LLA[0])sinf(cur_pos_LLA[1]);
ze = (Ne(1.0f-expexp) + cur_pos_LLA[2])*sinf(cur_pos_LLA[0]);
xer = (Ner + home_pos_LLA[2])*cosf(home_pos_LLA[0])*cosf(home_pos_LLA[1]);
yer = (Ner + home_pos_LLA[2])cosf(home_pos_LLA[0])sinf(home_pos_LLA[1]);
zer = (Ner(1.0f-expexp) + home_pos_LLA[2])*sinf(home_pos_LLA[0]);
result[0] = -sinf(home_pos_LLA[0])cosf(home_pos_LLA[1])(xe-xer) -sinf(home_pos_LLA[0])sinf(home_pos_LLA[1])(ye-yer) + cosf(home_pos_LLA[0])(ze-zer);
result[1] = -sinf(home_pos_LLA[1])(xe-xer) + cosf(home_pos_LLA[1])(ye-yer);
result[2] = -cosf(home_pos_LLA[0])cosf(home_pos_LLA[1])(xe-xer) - cosf(home_pos_LLA[0])sinf(home_pos_LLA[1])(ye-yer) - sinf(home_pos_LLA[0])(ze-zer);
return result;
}
4) Main components.
RS+, two M+, Arduino, two Stepping moter, absolute encoder, Antennas ,mesh modems, Lora and so on.
5) Photos and Videos.
- Step by step documentation of building process (optional).