[ARDrone] Add TCP Socket for owlps-sig
Communication witch Google Maps API, for Real-Time tracking. Port set to 8080. SIG -> Geo Information System.
This commit is contained in:
parent
80a6439205
commit
3b2823fef9
|
@ -16,8 +16,17 @@
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <pthread.h>
|
#include <pthread.h>
|
||||||
|
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
|
||||||
|
#define SEPARATOR ','
|
||||||
|
#define EOL '$'
|
||||||
|
|
||||||
/* Options */
|
/* Options */
|
||||||
|
|
||||||
|
gps datagps;
|
||||||
|
|
||||||
struct timespec timestamp ;
|
struct timespec timestamp ;
|
||||||
struct {
|
struct {
|
||||||
char dest_ip[INET_ADDRSTRLEN] ; // Destination IP of the packets
|
char dest_ip[INET_ADDRSTRLEN] ; // Destination IP of the packets
|
||||||
|
@ -54,7 +63,9 @@ struct sockaddr_in server ; // Server info
|
||||||
uint8_t *packet = NULL ; // Packet to send
|
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)
|
||||||
|
float share_x, share_y, share_z;
|
||||||
|
double share_lat, share_lon, share_alt=0;
|
||||||
|
|
||||||
int main(int argc, char *argv[])
|
int main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
|
@ -64,10 +75,16 @@ int main(int argc, char *argv[])
|
||||||
create_socket() ;
|
create_socket() ;
|
||||||
pthread_t send ;
|
pthread_t send ;
|
||||||
pthread_create(&send, NULL, thread_send, NULL) ;
|
pthread_create(&send, NULL, thread_send, NULL) ;
|
||||||
|
// Thread communication TCP OwlSIG
|
||||||
|
pthread_t control ;
|
||||||
|
pthread_create(&control, NULL, thread_control, NULL) ;
|
||||||
|
pthread_t gps ;
|
||||||
|
pthread_create(&gps, NULL, thread_gps, NULL) ;
|
||||||
while(1)
|
while(1)
|
||||||
{
|
{
|
||||||
if (options.listening_port > 0)
|
if (options.listening_port > 0)
|
||||||
receive_position() ;
|
receive_position() ;
|
||||||
|
else usleep(1000);
|
||||||
}
|
}
|
||||||
(void) close(socksendfd) ;
|
(void) close(socksendfd) ;
|
||||||
(void) close(sockreceivefd) ;
|
(void) close(sockreceivefd) ;
|
||||||
|
@ -89,6 +106,124 @@ void parse_command_line(int argc, char **argv)
|
||||||
#endif // DEBUG
|
#endif // DEBUG
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void traficGPS(int gps) {
|
||||||
|
int offset=0, c=1, i=0, l;
|
||||||
|
char buffer[4096], chtime[1024];
|
||||||
|
char *utc, *latitude, *longitude, *chv, *cmd, *ch;
|
||||||
|
float vitesse;
|
||||||
|
|
||||||
|
do {
|
||||||
|
c=read(gps, buffer, 4096);
|
||||||
|
printf("lecture intiale %d\n",c);
|
||||||
|
} while (c>99);
|
||||||
|
do {
|
||||||
|
c=read(gps, buffer+offset, 1);
|
||||||
|
offset=offset+c;
|
||||||
|
} while (buffer[offset-c]!='$');
|
||||||
|
//buffer[offset]='\0';
|
||||||
|
//printf("phrase x%sx\n",buffer);
|
||||||
|
// Debut de phrase GPS sans le dollar
|
||||||
|
while (1) {
|
||||||
|
i=0;
|
||||||
|
offset=0;
|
||||||
|
do {
|
||||||
|
c=read(gps, buffer+offset, 1);
|
||||||
|
offset=offset+c;
|
||||||
|
} while (buffer[offset-c]!='$');
|
||||||
|
//buffer[offset]='\0';
|
||||||
|
//printf("phrase s x%sx\n",buffer);
|
||||||
|
if (strncmp(buffer+i,"GPRMC",5)==0)
|
||||||
|
{
|
||||||
|
char data[15][20];
|
||||||
|
int offset_line, length, ct_string, y;
|
||||||
|
int z;
|
||||||
|
|
||||||
|
ct_string = offset_line = y = z = 0 ;
|
||||||
|
length = strlen(buffer+i) ;
|
||||||
|
memset(data,0,200);
|
||||||
|
while(y<=length)
|
||||||
|
{
|
||||||
|
if((buffer)[y+i]==EOL && z==0) z=1;
|
||||||
|
else if((buffer)[y+i]==EOL && z==1)
|
||||||
|
{
|
||||||
|
length=y-1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
y++;
|
||||||
|
}
|
||||||
|
printf("\nTaille finale : %d\n", length);
|
||||||
|
|
||||||
|
y=0;
|
||||||
|
while(y<=length)
|
||||||
|
{
|
||||||
|
if ((buffer)[y+i]==SEPARATOR)
|
||||||
|
{
|
||||||
|
|
||||||
|
if((y-offset_line)==0)
|
||||||
|
memcpy(data[ct_string], "\0", 1);
|
||||||
|
|
||||||
|
else
|
||||||
|
{
|
||||||
|
memcpy(data[ct_string], buffer+i+offset_line, y-offset_line);
|
||||||
|
data[ct_string][y-offset_line]=0;
|
||||||
|
}
|
||||||
|
ct_string++;
|
||||||
|
offset_line=y+1;
|
||||||
|
}
|
||||||
|
y++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!(ct_string==0 || offset_line==0))
|
||||||
|
memcpy(data[ct_string], buffer+i+offset_line, length-offset_line);
|
||||||
|
data[ct_string][length-offset_line]=0;
|
||||||
|
|
||||||
|
datagps.time = atof(data[1]);
|
||||||
|
datagps.state = data[2][0];
|
||||||
|
datagps.latitude = atof(strndup(data[3],2))+atof(strndup(data[3]+2,strlen(data[3])-2))/60.0 ;
|
||||||
|
datagps.orilat = data[4][0];
|
||||||
|
datagps.longitude = atof(strndup(data[5],3))+atof(strndup(data[5]+3,strlen(data[5])-3))/60.0 ;
|
||||||
|
datagps.orilon = data[6][0];
|
||||||
|
datagps.speed = atof(data[7])*1.852;
|
||||||
|
datagps.cap = atof(data[8]);
|
||||||
|
datagps.date = atoi(data[9]);
|
||||||
|
datagps.decmag = atof(data[10]);
|
||||||
|
|
||||||
|
share_lat=datagps.latitude;
|
||||||
|
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("Taille : %d | Message : %s\n",strlen(buffer+i),buffer+i);
|
||||||
|
cmd=strtok(buffer+i,",");
|
||||||
|
utc=strtok(NULL,",");
|
||||||
|
ch=strtok(NULL,",");
|
||||||
|
latitude=strtok(NULL,",");
|
||||||
|
ch=strtok(NULL,",");
|
||||||
|
longitude=strtok(NULL,",");
|
||||||
|
ch=strtok(NULL,",");
|
||||||
|
chv=strtok(NULL,",");
|
||||||
|
//printf("h %s lt %s lg %s v %s\n",utc, latitude, longitude,chv);
|
||||||
|
vitesse=atof(chv)*1.852;
|
||||||
|
share_lat=atof(strndup(latitude,2))+atof(strndup(latitude+2,strlen(latitude)-2))/60.0;
|
||||||
|
share_lon=atof(strndup(longitude,3))+atof(strndup(longitude+3,strlen(longitude)-3))/60.0;
|
||||||
|
printf("h %s lt %s lg %s v %f km/h share lat lon %f %f\n",utc, latitude, longitude,vitesse, share_lat, share_lon);
|
||||||
|
*/
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void* thread_gps(void* data)
|
||||||
|
{
|
||||||
|
int c=0;
|
||||||
|
char buffer[1024];
|
||||||
|
|
||||||
|
int gps_fd = open("/dev/ttyPA0",O_RDONLY);
|
||||||
|
|
||||||
|
read(gps_fd, buffer, 80);
|
||||||
|
//printf("%sxxx\n",buffer);
|
||||||
|
traficGPS(gps_fd);
|
||||||
|
}
|
||||||
|
|
||||||
void* thread_send(void* data)
|
void* thread_send(void* data)
|
||||||
{
|
{
|
||||||
while(1)
|
while(1)
|
||||||
|
@ -99,6 +234,114 @@ void* thread_send(void* data)
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// TCP Server OwlSIG
|
||||||
|
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;
|
||||||
|
socklen_t clilen;
|
||||||
|
char request[2048], response[2048], *c;
|
||||||
|
struct sockaddr_in serv_addr, cli_addr;
|
||||||
|
int n,i=4;
|
||||||
|
char *cmd, *sx, *sy, *sz, *st;
|
||||||
|
|
||||||
|
sockfd = socket(AF_INET, SOCK_STREAM, 0);
|
||||||
|
if (sockfd < 0)
|
||||||
|
perror("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)
|
||||||
|
perror("ERROR on binding");
|
||||||
|
listen(sockfd,1);
|
||||||
|
clilen = sizeof(cli_addr);
|
||||||
|
memset(request,0,1500);
|
||||||
|
cmd=malloc(256);
|
||||||
|
strcpy(cmd,"");
|
||||||
|
while ( strcmp(cmd,"end")!=0) {
|
||||||
|
newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
|
||||||
|
if (newsockfd < 0)
|
||||||
|
perror("ERROR on accept");
|
||||||
|
// while ( strcmp(request,"end")!=0) {
|
||||||
|
n = read(newsockfd,request,1500);
|
||||||
|
printf("\ntaille %d\n", n);
|
||||||
|
fflush(NULL);
|
||||||
|
if (n < 0) perror("ERROR reading from socket");
|
||||||
|
printf("%s", request);
|
||||||
|
// Add command processing
|
||||||
|
c=strtok(request,"=");
|
||||||
|
printf("val 1 %s\n",c);
|
||||||
|
c=strtok(NULL,"=");
|
||||||
|
printf("val 2 %s\n",c);
|
||||||
|
cmd=strtok(c,";");
|
||||||
|
printf("val 3 %s\n",cmd);
|
||||||
|
sx=strtok(NULL,";");
|
||||||
|
sy=strtok(NULL,";");
|
||||||
|
sz=strtok(NULL,";");
|
||||||
|
st=strtok(NULL,";");
|
||||||
|
|
||||||
|
strcpy(response,"HTTP/1.1 200 OK\nDate: Mon, 18 Apr 2011 19:51:52 GMT\nServer: Apache/ProXad [Aug 5 2010 16:17:11]\nAccess-Control-Allow-Origin: *\nX-Powered-By: PHP/4.4.3-dev\nConnection: close\nContent-Type: text/html\n\n");
|
||||||
|
if (strcmp(cmd, "ReadPosition")==0)
|
||||||
|
sprintf(response,"%sPosition;OK;%d.0;5.0;6.1\n",response,i++);
|
||||||
|
else if (strcmp(cmd, "ReadGeoPosition")==0)
|
||||||
|
sprintf(response,"%sGeoPosition;OK;%f;%f;%f\n",response,share_lat,share_lon,share_alt);
|
||||||
|
else if (strcmp(cmd, "SendGeoSetpoint")==0)
|
||||||
|
sprintf(response,"%sGeoSetpoint;OK;%d.0;5.0;6.1;0.0\n",response,i++);
|
||||||
|
else if (strcmp(cmd, "SendSetpoint")==0)
|
||||||
|
sprintf(response,"%sSetpoint;OK;%s;%s;%s;%s\n",response,sx,sy,sz,st);
|
||||||
|
else
|
||||||
|
strcat(response,"Not understand\n");
|
||||||
|
n = write(newsockfd,response,strlen(response));
|
||||||
|
if (n < 0) perror("ERROR writing to socket");
|
||||||
|
printf ("Write : %d %d\n", n, (int) strlen(response));
|
||||||
|
close(newsockfd);
|
||||||
|
|
||||||
|
// }
|
||||||
|
}
|
||||||
|
close(sockfd);
|
||||||
|
return NULL;
|
||||||
|
>>>>>>> ea33a1a... [ARDrone] Add support GPS NMEA
|
||||||
|
}
|
||||||
|
|
||||||
void parse_main_options(int argc, char **argv)
|
void parse_main_options(int argc, char **argv)
|
||||||
{
|
{
|
||||||
int opt ;
|
int opt ;
|
||||||
|
|
|
@ -35,11 +35,15 @@ void receive_position(void) ;
|
||||||
void print_usage(void) ;
|
void print_usage(void) ;
|
||||||
void send_labview(char*) ;
|
void send_labview(char*) ;
|
||||||
void string2data(char*) ;
|
void string2data(char*) ;
|
||||||
|
void traficGPS(int);
|
||||||
|
static int isWaiting(int);
|
||||||
|
void* thread_gps(void*) ;
|
||||||
void* thread_send(void*) ;
|
void* thread_send(void*) ;
|
||||||
|
void* thread_control(void*) ;
|
||||||
void print_error(char*) ;
|
void print_error(char*) ;
|
||||||
void print_version(void) ;
|
void print_version(void) ;
|
||||||
typedef struct result result;
|
typedef struct result result;
|
||||||
|
typedef struct gps gps;
|
||||||
/* Struct */
|
/* Struct */
|
||||||
|
|
||||||
struct result
|
struct result
|
||||||
|
@ -49,3 +53,17 @@ struct result
|
||||||
float y;
|
float y;
|
||||||
float z;
|
float z;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct gps
|
||||||
|
{
|
||||||
|
float time;
|
||||||
|
char state;
|
||||||
|
double latitude;
|
||||||
|
char orilat;
|
||||||
|
double longitude;
|
||||||
|
char orilon;
|
||||||
|
float speed;
|
||||||
|
float cap;
|
||||||
|
int date;
|
||||||
|
float decmag;
|
||||||
|
};
|
||||||
|
|
Loading…
Reference in New Issue