[ARDrone] Fix GPS orientation
Add condition South and East for négative latitude/longitude.
This commit is contained in:
parent
3b2823fef9
commit
46292cfe8c
|
@ -64,8 +64,10 @@ uint8_t *packet = NULL ; // Packet to send
|
||||||
uint_fast16_t packet_size ; // Packet size
|
uint_fast16_t packet_size ; // Packet size
|
||||||
|
|
||||||
// Shared variables for OwlSIG (Geo Information System)
|
// Shared variables for OwlSIG (Geo Information System)
|
||||||
float share_x, share_y, share_z;
|
double share_x, share_y, share_z;
|
||||||
double share_lat, share_lon, share_alt=0;
|
double share_lat, share_lon, share_alt=0;
|
||||||
|
double share_lat_cons=-999.0, share_lon_cons=-999.00, share_alt_cons=-999.0;
|
||||||
|
double share_x_cons=-999.0, share_y_cons=-999.00, share_z_cons=-999.0, share_t_cons=-999.0;
|
||||||
|
|
||||||
int main(int argc, char *argv[])
|
int main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
|
@ -106,30 +108,37 @@ void parse_command_line(int argc, char **argv)
|
||||||
#endif // DEBUG
|
#endif // DEBUG
|
||||||
}
|
}
|
||||||
|
|
||||||
void traficGPS(int gps) {
|
void traficGPS(int gps)
|
||||||
|
{
|
||||||
int offset=0, c=1, i=0, l;
|
int offset=0, c=1, i=0, l;
|
||||||
char buffer[4096], chtime[1024];
|
char buffer[4096], chtime[1024];
|
||||||
char *utc, *latitude, *longitude, *chv, *cmd, *ch;
|
char *utc, *latitude, *longitude, *chv, *cmd, *ch;
|
||||||
float vitesse;
|
float vitesse;
|
||||||
|
|
||||||
do {
|
do
|
||||||
|
{
|
||||||
c=read(gps, buffer, 4096);
|
c=read(gps, buffer, 4096);
|
||||||
printf("lecture intiale %d\n",c);
|
printf("lecture intiale %d\n",c);
|
||||||
} while (c>99);
|
}
|
||||||
do {
|
while (c>99);
|
||||||
|
do
|
||||||
|
{
|
||||||
c=read(gps, buffer+offset, 1);
|
c=read(gps, buffer+offset, 1);
|
||||||
offset=offset+c;
|
offset=offset+c;
|
||||||
} while (buffer[offset-c]!='$');
|
}
|
||||||
|
while (buffer[offset-c]!='$');
|
||||||
//buffer[offset]='\0';
|
//buffer[offset]='\0';
|
||||||
//printf("phrase x%sx\n",buffer);
|
//printf("phrase x%sx\n",buffer);
|
||||||
// Debut de phrase GPS sans le dollar
|
// Debut de phrase GPS sans le dollar
|
||||||
while (1) {
|
while (1) {
|
||||||
i=0;
|
i=0;
|
||||||
offset=0;
|
offset=0;
|
||||||
do {
|
do
|
||||||
|
{
|
||||||
c=read(gps, buffer+offset, 1);
|
c=read(gps, buffer+offset, 1);
|
||||||
offset=offset+c;
|
offset=offset+c;
|
||||||
} while (buffer[offset-c]!='$');
|
}
|
||||||
|
while (buffer[offset-c]!='$');
|
||||||
//buffer[offset]='\0';
|
//buffer[offset]='\0';
|
||||||
//printf("phrase s x%sx\n",buffer);
|
//printf("phrase s x%sx\n",buffer);
|
||||||
if (strncmp(buffer+i,"GPRMC",5)==0)
|
if (strncmp(buffer+i,"GPRMC",5)==0)
|
||||||
|
@ -188,9 +197,12 @@ void traficGPS(int gps) {
|
||||||
datagps.date = atoi(data[9]);
|
datagps.date = atoi(data[9]);
|
||||||
datagps.decmag = atof(data[10]);
|
datagps.decmag = atof(data[10]);
|
||||||
|
|
||||||
|
if (datagps.orilat=='S') datagps.latitude=-datagps.latitude;
|
||||||
|
if (datagps.orilon=='W') datagps.longitude=-datagps.longitude;
|
||||||
share_lat=datagps.latitude;
|
share_lat=datagps.latitude;
|
||||||
share_lon=datagps.longitude;
|
share_lon=datagps.longitude;
|
||||||
printf("Time : %f | State : %c | Latitude : %f%c | Longitude %f%c | Speed : %f km/h | Cap : %f° | Date : %d | DecMagn : %f \n",datagps.time, datagps.state, datagps.latitude, datagps.orilat, datagps.longitude, datagps.orilon, datagps.speed, datagps.cap, datagps.date, datagps.decmag);
|
printf("lat %s lon %s\n",data[3], data[5]);
|
||||||
|
printf("Time : %f | State : %c | Latitude : %.10f%c | Longitude %.10f%c | Speed : %f km/h | Cap : %f° | Date : %d | DecMagn : %f maps %.10f %.10f\n",datagps.time, datagps.state, datagps.latitude, datagps.orilat, datagps.longitude, datagps.orilon, datagps.speed, datagps.cap, datagps.date, datagps.decmag, share_lat, share_lon);
|
||||||
|
|
||||||
|
|
||||||
/* printf("Taille : %d | Message : %s\n",strlen(buffer+i),buffer+i);
|
/* printf("Taille : %d | Message : %s\n",strlen(buffer+i),buffer+i);
|
||||||
|
@ -230,6 +242,7 @@ void* thread_send(void* data)
|
||||||
{
|
{
|
||||||
make_packet() ;
|
make_packet() ;
|
||||||
send_request() ;
|
send_request() ;
|
||||||
|
sleep(1) ;
|
||||||
}
|
}
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
@ -237,45 +250,6 @@ void* thread_send(void* data)
|
||||||
// TCP Server OwlSIG
|
// TCP Server OwlSIG
|
||||||
void* thread_control(void* data)
|
void* thread_control(void* data)
|
||||||
{
|
{
|
||||||
<<<<<<< HEAD
|
|
||||||
int sockfd, newsockfd, portno=8080;
|
|
||||||
socklen_t clilen;
|
|
||||||
char request[1024], response[1204];
|
|
||||||
struct sockaddr_in serv_addr, cli_addr;
|
|
||||||
int n;
|
|
||||||
|
|
||||||
sockfd = socket(AF_INET, SOCK_STREAM, 0);
|
|
||||||
if (sockfd < 0)
|
|
||||||
error("ERROR opening socket TCP");
|
|
||||||
bzero((char *) &serv_addr, sizeof(serv_addr));
|
|
||||||
serv_addr.sin_family = AF_INET;
|
|
||||||
serv_addr.sin_addr.s_addr = INADDR_ANY;
|
|
||||||
serv_addr.sin_port = htons(portno);
|
|
||||||
if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
|
|
||||||
error("ERROR on binding");
|
|
||||||
listen(sockfd,1);
|
|
||||||
clilen = sizeof(cli_addr);
|
|
||||||
while (1)
|
|
||||||
{
|
|
||||||
newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
|
|
||||||
if (newsockfd < 0)
|
|
||||||
error("ERROR on accept");
|
|
||||||
bzero(request,1024);
|
|
||||||
while ( strcmp(request,"end")!=0) {
|
|
||||||
n = read(newsockfd,request,1024);
|
|
||||||
if (n < 0) perror("ERROR reading from socket");
|
|
||||||
printf("%s", request);
|
|
||||||
// Add command processing
|
|
||||||
sprintf(response,"Position;OK;1.2;3.4;0.0");
|
|
||||||
//sprintf(response,"Position;OK;%f;%f;%f",share_x, share_y, share_z);
|
|
||||||
n = write(newsockfd,response,strlen(response));
|
|
||||||
if (n < 0) perror("ERROR writing to socket");
|
|
||||||
}
|
|
||||||
close(newsockfd);
|
|
||||||
close(sockfd);
|
|
||||||
}
|
|
||||||
return NULL;
|
|
||||||
=======
|
|
||||||
int sockfd, newsockfd, portno=8080;
|
int sockfd, newsockfd, portno=8080;
|
||||||
socklen_t clilen;
|
socklen_t clilen;
|
||||||
char request[2048], response[2048], *c;
|
char request[2048], response[2048], *c;
|
||||||
|
@ -324,10 +298,20 @@ strcpy(response,"HTTP/1.1 200 OK\nDate: Mon, 18 Apr 2011 19:51:52 GMT\nServer: A
|
||||||
sprintf(response,"%sPosition;OK;%d.0;5.0;6.1\n",response,i++);
|
sprintf(response,"%sPosition;OK;%d.0;5.0;6.1\n",response,i++);
|
||||||
else if (strcmp(cmd, "ReadGeoPosition")==0)
|
else if (strcmp(cmd, "ReadGeoPosition")==0)
|
||||||
sprintf(response,"%sGeoPosition;OK;%f;%f;%f\n",response,share_lat,share_lon,share_alt);
|
sprintf(response,"%sGeoPosition;OK;%f;%f;%f\n",response,share_lat,share_lon,share_alt);
|
||||||
else if (strcmp(cmd, "SendGeoSetpoint")==0)
|
else if (strcmp(cmd, "SendGeoSetpoint")==0) {
|
||||||
sprintf(response,"%sGeoSetpoint;OK;%d.0;5.0;6.1;0.0\n",response,i++);
|
share_lat_cons=atof(sx);
|
||||||
else if (strcmp(cmd, "SendSetpoint")==0)
|
share_lon_cons=atof(sy);
|
||||||
sprintf(response,"%sSetpoint;OK;%s;%s;%s;%s\n",response,sx,sy,sz,st);
|
share_alt_cons=atof(sz);
|
||||||
|
share_t_cons=atof(st);
|
||||||
|
sprintf(response,"%sGeoSetpoint;OK;%.15f;%.15f;%f;%f\n",response,share_lat_cons,share_lon_cons,share_alt_cons,share_t_cons);
|
||||||
|
}
|
||||||
|
else if (strcmp(cmd, "SendSetpoint")==0) {
|
||||||
|
share_x_cons=atof(sx);
|
||||||
|
share_y_cons=atof(sy);
|
||||||
|
share_z_cons=atof(sz);
|
||||||
|
share_t_cons=atof(st);
|
||||||
|
sprintf(response,"%sSetpoint;OK;%f;%f;%f;%f\n",response,share_x_cons,share_y_cons,share_z_cons,share_t_cons);
|
||||||
|
}
|
||||||
else
|
else
|
||||||
strcat(response,"Not understand\n");
|
strcat(response,"Not understand\n");
|
||||||
n = write(newsockfd,response,strlen(response));
|
n = write(newsockfd,response,strlen(response));
|
||||||
|
@ -339,7 +323,6 @@ strcpy(response,"HTTP/1.1 200 OK\nDate: Mon, 18 Apr 2011 19:51:52 GMT\nServer: A
|
||||||
}
|
}
|
||||||
close(sockfd);
|
close(sockfd);
|
||||||
return NULL;
|
return NULL;
|
||||||
>>>>>>> ea33a1a... [ARDrone] Add support GPS NMEA
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void parse_main_options(int argc, char **argv)
|
void parse_main_options(int argc, char **argv)
|
||||||
|
|
Loading…
Reference in New Issue