/* * This file is part of the rtap localisation project. */ #include "owlps-drone.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /* Options */ gps datagps; point_consigne array_cons[30]; int i_consigne=0; int num_point=0; struct timespec timestamp ; struct { char dest_ip[INET_ADDRSTRLEN] ; // Destination IP of the packets uint_fast16_t dest_port ; char iface[IFNAMSIZ + 1] ; // Source network interface int_fast32_t delay ; // Time between two packet transmissions uint_fast16_t nb_pkt ; // Number of packets to send uint_fast16_t listening_port ; // Calibration data: owl_direction direction ; float x ; float y ; float z ; } options = { "", OWL_DEFAULT_REQUEST_PORT, "", -1, 0, 0, 0, 0, 0, 0 } ; char *program_name = NULL ; // True if the packet is a calibration request, false if it is a simple // positioning request: owl_bool is_calibration_request = owl_false ; int socksendfd ; // Sending socket descriptor (send positioning packets) int sockreceivefd ; // Receiving socket descriptor (Receive position) struct sockaddr_in server ; // Server info uint8_t *packet = NULL ; // Packet to send uint_fast16_t packet_size ; // Packet size struct point share_gps; struct point share_gps_retro; consigne* list_share_gps_cons = NULL; struct point share_gps_cons; struct point share_gps_origin; struct point_relatif share_relatif; struct point_relatif share_relatif_retro; struct point_relatif share_relatif_cons; int state_flying=0, i=-1, start=0, run=1; sem_t lock; /* Socket drone */ int sockcmdfd; struct sockaddr_in servcmd; char sendline[100]; char count[100]; //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; double share_t_cons=-999.00; int main(int argc, char *argv[]) { sem_init(&lock, 0, 1); FILE* log = NULL; float angle, distance, speed_pitch, distance_total; struct sigaction action; program_name = argv[0] ; parse_command_line(argc, argv) ; action.sa_flags = 0; sigemptyset(&action.sa_mask); action.sa_handler = drone_handler_sigint; sigaction(SIGINT, &action, NULL); // Reset value share variables memset(&share_gps,0,sizeof(share_gps)); memset(&share_gps_cons,0,sizeof(share_gps_cons)); memset(&share_gps_retro,0,sizeof(share_gps_retro)); memset(&share_gps_origin,0,sizeof(share_gps_origin)); memset(&share_relatif,0,sizeof(share_relatif)); memset(&share_relatif_cons,0,sizeof(share_relatif_cons)); memset(&share_relatif_retro,0,sizeof(share_relatif_retro)); //Set unknow value gps share_gps_retro.lat=-999.00; share_gps_retro.lon=-999.00; share_gps_cons.lat = -999.00; share_gps_cons.lon = -999.00; // Thread communication TCP OwlSIG pthread_t control ; pthread_create(&control, NULL, thread_control, NULL) ; //Launch thread or wifi depend of TYPE if(!strcmp(TYPE,"WIFI")) { pthread_t send ; pthread_t receive ; create_socket() ; pthread_create(&send, NULL, thread_send, NULL) ; if (options.listening_port > 0) pthread_create(&receive, NULL, receive_position, NULL) ; } else if(!strcmp(TYPE,"GPS")) { pthread_t gps ; pthread_create(&gps, NULL, thread_gps, NULL) ; } else { printf("TYPE is not correct, change value of TYPE\n"); return -1; } drone_socket_create() ; pthread_t drone_watchdog ; pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ; log = fopen("vol.log", "w"); fprintf(log, "Date;Heure;Latitude;Longitude;Latitude_cons;Longitude_cons;" "DistanceTotal;Distance;SpeedPitch;angle;" "x;y;x_retro;y_retro;x_cons;y_cons\n") ; while(run) { i_consigne=0; while(start==0) usleep(200000); //Wait GPS Signal if(share_gps.lat==0||share_gps.lon==0) printf("[GPS] GPS not fix! Wait...\n"); while(share_gps.lat==0||share_gps.lon==0); /* Log File */ /* - */ printf("[GPS] GPS fix OK!\n"); speed_pitch=-1; if(read_position(TYPE)<0) drone_err(1); if(cmd_drone_take_off()<0) drone_err(2); while(share_gps_cons.lat==-999.00||share_gps_cons.lon==-999.00); sleep(8); while(i_consigne<=num_point) { share_gps_cons.lat = array_cons[i_consigne].lat; share_gps_cons.lon = array_cons[i_consigne].lon; share_gps_cons.alt = array_cons[i_consigne].alt; distance_total = fabs(oc_distance_between(share_gps, share_gps_cons)) ; while(start==1&&!check_destination(share_gps, share_gps_cons)) { if(read_position(TYPE)<0) drone_err(1); //calcul_trajectory(&angle, &distance_old); angle = calcul_angle(); distance = fabs(oc_distance_between(share_gps, share_gps_cons)) ; if(speed_pitch<-(distance/distance_total)) speed_pitch=-(distance/distance_total)-0.2; if(speed_pitch < -1) speed_pitch = -1; else if(speed_pitch > 1) speed_pitch = 1; cmd_drone_move((speed_pitch), (angle)); usleep(10000); printf("Vitesse : %f Angle : %f\n", speed_pitch, angle); fprintf(log, "%d;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f\n", share_gps.date, share_gps.time, share_gps.lat, share_gps.lon, share_gps_cons.lat, share_gps_cons.lon, distance_total, distance, speed_pitch, angle, share_relatif.x, share_relatif.y, share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y) ; //printf("[DEBUG] Vitesse %f Angle %f state %d\n",speed_pitch, angle,state_flying); } ++i_consigne ; } fclose(log); cmd_drone_landing(); } sem_destroy(&lock); close(socksendfd) ; close(sockreceivefd) ; return 0 ; } void parse_command_line(int argc, char **argv) { parse_main_options(argc, argv) ; if(!strcmp(TYPE,"WIFI")) { check_destination_ip() ; parse_calibration_data(argc, argv) ; check_configuration() ; #ifdef DEBUG print_configuration() ; #endif // DEBUG } } void traficGPS(int gps) { int offset=0, c=1, i=0; char buffer[4096]; 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 (run) { 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]); if (datagps.orilat=='S') datagps.latitude=-datagps.latitude; if (datagps.orilon=='W') datagps.longitude=-datagps.longitude; if(share_gps.lat!=datagps.latitude||share_gps.lon!=datagps.longitude) { share_gps_retro.lat=share_gps.lat; share_gps_retro.lon=share_gps.lon; share_gps.lat=datagps.latitude; share_gps.lon=datagps.longitude; share_gps.cap=datagps.cap; } share_gps.date=datagps.date; share_gps.time=datagps.time; //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_gps.lat, share_gps.lon); } } } void* thread_gps(void* NULL_value) { char buffer[1024]; int gps_fd = open("/dev/ttyPA0",O_RDONLY); read(gps_fd, buffer, 80); traficGPS(gps_fd); pthread_exit(NULL_value); } void* thread_send(void* NULL_value) { while(run) { make_packet() ; send_request() ; sleep(1) ; } pthread_exit(NULL_value); } // TCP Server OwlSIG void* thread_control(void* NULL_value) { 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\n" "Date: Mon, 18 Apr 2011 19:51:52 GMT\n" "Server: Apache/ProXad [Aug 5 2010 16:17:11]\n" "Access-Control-Allow-Origin: *\n" "X-Powered-By: PHP/4.4.3-dev\n" "Connection: close\n" "Content-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_gps.lat, share_gps. lon,share_gps.alt) ; else if (strcmp(cmd, "SendGeoSetpoint")==0) { share_gps_cons.lat=atof(sx); share_gps_cons.lon=atof(sy); share_gps_cons.alt=atof(sz); share_t_cons=atof(st); sprintf(response, "%sGeoSetpoint;OK;%.15f;%.15f;%f;%f\n", response, share_gps_cons.lat, share_gps_cons.lon, share_gps_cons.alt, share_t_cons) ; printf("[Consigne] Latitude %f Lontitude %f défini!\n", share_gps_cons.lat, share_gps_cons.lon) ; array_cons[num_point].lat = share_gps_cons.lat; array_cons[num_point].lon = share_gps_cons.lon; array_cons[num_point].alt = share_gps_cons.alt; ++num_point ; printf("[Consigne %d point défini]\n", num_point); } else if (strcmp(cmd, "SendSetpoint")==0) { share_relatif_cons.x=atof(sx); share_relatif_cons.y=atof(sy); share_relatif_cons.z=atof(sz); share_t_cons=atof(st); sprintf(response, "%sSetpoint;OK;%f;%f;%f;%f\n", response, share_relatif_cons.x, share_relatif_cons.y, share_relatif_cons.z, share_t_cons) ; } else if(strcmp(cmd, "SendStart")==0) { start=1; sprintf(response,"%sStarted;OK;",response); } else if(strcmp(cmd, "SendLanding")==0) { start=0; cmd_drone_landing(); sprintf(response,"%sLanding;OK;",response); } else if(strcmp(cmd, "SendEraseTrace")==0) { num_point=0; sprintf(response,"%sSendEraseTrace;OK;",response); } 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); pthread_exit(NULL_value); } void parse_main_options(int argc, char **argv) { int opt ; while ((opt = getopt(argc, argv, OPTIONS)) != -1) { switch (opt) { case 'd' : strncpy(options.dest_ip, optarg, INET_ADDRSTRLEN) ; break ; case 'h' : print_usage() ; exit(0) ; case 'i' : strncpy(options.iface, optarg, IFNAMSIZ + 1) ; break ; case 'l' : /* Facultative getopt options do not handle separated values * (like -l ), so we have to test separately. */ if (optarg) // We got an option like -l, it's OK options.listening_port = strtoul(optarg, NULL, 0) ; else // We got -l alone or -l { /* If we are at the end of the string, or the next optind * is an option, we have -l without a port number */ if (argv[optind] == NULL || argv[optind][0] == '-') // Take the default value: options.listening_port = OWL_DEFAULT_RESULT_PORT ; else { // Take the optind value: options.listening_port = strtoul(argv[optind], NULL, 0) ; ++optind ; } } break ; case 'n' : options.nb_pkt = strtoul(optarg, NULL, 0) ; break ; case 'p' : options.dest_port = strtoul(optarg, NULL, 0) ; break ; case 't' : options.delay = strtol(optarg, NULL, 0) ; break ; case 'V' : print_version() ; exit(0) ; default : print_usage() ; exit(ERR_BAD_USAGE) ; } } } void check_destination_ip() { /* Check if we got a destination IP address */ if (options.dest_ip[0] == '\0') { fprintf(stderr, "Error! You must specify a destination IP address" " (-d).\n") ; print_usage() ; exit(ERR_BAD_USAGE) ; } } void parse_calibration_data(int argc, char **argv) { /* Parse remaining arguments (possible calibration data) */ if (argc - optind != 0) { if (argc - optind == 4) { is_calibration_request = owl_true ; options.direction = strtoul(argv[optind++], NULL, 0) ; options.x = strtod(argv[optind++], NULL) ; options.y = strtod(argv[optind++], NULL) ; options.z = strtod(argv[optind], NULL) ; } else // Bad number of arguments { print_usage() ; exit(ERR_BAD_USAGE) ; } } } void check_configuration() { // Delay not specified (or bad delay): if (options.delay < 0) { #ifdef DEBUG fprintf(stderr, "Warning! delay: failing back to default value.\n") ; #endif // DEBUG if (is_calibration_request) options.delay = DEFAULT_DELAY_CALIB ; else options.delay = DEFAULT_DELAY_NORMAL ; } // Number of packet not specified (or bad number) if (options.nb_pkt < 1) { #ifdef DEBUG fprintf(stderr, "Warning! nb_pkt: failing back to default value.\n") ; #endif // DEBUG if (is_calibration_request) options.nb_pkt = DEFAULT_NBPKT_CALIB ; else options.nb_pkt = DEFAULT_NBPKT_NORMAL ; } // Calibration request but bad direction if (is_calibration_request) if (options.direction < OWL_DIRECTION_MIN || options.direction > OWL_DIRECTION_MAX) { fprintf(stderr, "Error! « %"PRIu8" » is not a valid" " direction.\n", options.direction) ; exit(ERR_BAD_USAGE) ; } // Check port numbers if (options.dest_port < 1 || options.dest_port > 65535) { #ifdef DEBUG fprintf(stderr, "Warning! Bad dest_port:" " failing back to default value.\n") ; options.dest_port = OWL_DEFAULT_REQUEST_PORT ; #endif // DEBUG } if (options.listening_port > 65535) { #ifdef DEBUG fprintf(stderr, "Warning! listening_port too high: ignored.\n") ; options.listening_port = 0 ; #endif // DEBUG } // We want to send a calibration request AND to be located, which is // not allowed: if (is_calibration_request && options.listening_port > 0) { #ifdef DEBUG fprintf(stderr, "Warning! You cannot wait for a server answer when" " you calibrate. Option -l ignored…\n") ; #endif // DEBUG options.listening_port = 0 ; } } #ifdef DEBUG void print_configuration() { fprintf(stderr, "Options:\n" "\tDestination IP: %s\n" "\tDestination port: %"PRIuFAST16"\n" "\tInterface: %s\n" "\tDelay: %"PRIuFAST32"\n" "\tNumber of packets: %"PRIuFAST16"\n" "\tListening port: %"PRIuFAST16"\n" "\tDirection: %"PRIu8"\n" "\tX: %f\n" "\tY: %f\n" "\tZ: %f\n" , options.dest_ip, options.dest_port, options.iface, options.delay, options.nb_pkt, options.listening_port, options.direction, options.x, options.y, options.z ) ; } #endif // DEBUG void create_socket() { socksendfd = owl_create_trx_socket(options.dest_ip, options.dest_port, &server, options.iface) ; sockreceivefd = owl_create_udp_listening_socket(options.listening_port) ; } /* Creates the packet to send. */ void make_packet() { uint_fast16_t offset ; // Index used to create the packet owl_timestamp request_time ; char request_time_str[OWL_TIMESTAMP_STRLEN] ; // Get the current time and copy it as a string before to switch it to // network endianess: owl_timestamp_now(&request_time) ; owl_timestamp_to_string(request_time_str, request_time) ; request_time = owl_hton_timestamp(request_time) ; if (is_calibration_request) // Calibration packet { printf("Preparing calibration request packet…\n") ; offset = 0 ; packet_size = sizeof(uint8_t) * 2 + sizeof(owl_timestamp) + sizeof(float) * 3 ; packet = malloc(packet_size) ; memset(&packet[offset], OWL_REQUEST_CALIBRATION, 1) ; // Packet type ++offset ; memcpy(&packet[offset], &request_time, sizeof(request_time)) ; offset += sizeof(request_time) ; packet[offset++] = options.direction ; // Direction #ifdef DEBUG printf("Direction = %d, X = %f, Y = %f, Z = %f\n", packet[offset - 1], options.x, options.y, options.z) ; #endif // DEBUG options.x = owl_htonf(options.x) ; options.y = owl_htonf(options.y) ; options.z = owl_htonf(options.z) ; memcpy(&packet[offset], &options.x, sizeof(float)) ; offset += sizeof(float) ; memcpy(&packet[offset], &options.y, sizeof(float)) ; offset += sizeof(float) ; memcpy(&packet[offset], &options.z, sizeof(float)) ; } else // Standard packet { printf("Preparing request packet…\n") ; packet_size = sizeof(uint8_t) + sizeof(owl_timestamp) ; packet = malloc(packet_size) ; memset(&packet[0], OWL_REQUEST_NORMAL, 1) ; // Packet type memcpy(&packet[1], &request_time, sizeof(request_time)) ; } printf("Packet timestamp: %s\n", request_time_str) ; } void send_request() { owl_send_request(socksendfd, &server, packet, packet_size, options.nb_pkt, options.delay) ; } void* receive_position(void* NULL_value) { // Position of the mobile as computed by the infrastructure: owl_result *result ; while(run) { result = owl_receive_position(sockreceivefd) ; if (result) { owl_print_result(result) ; owl_free_result(result) ; } usleep(100000) ; } pthread_exit(NULL_value) ; } void print_error(char* merror) { if (! strcmp(merror,"trame")) printf("Impossible de lire la trame : Abandon") ; else if (! strcmp(merror,"algo")) printf("Impossible de lire les coordonnées de l'algo : Abandon") ; else printf("Erreur inconnue : Abandon") ; } void print_usage() { printf("Usage:\n" "Localisation request:\n" "\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]" " [-n nb_packets] [-l [port]]\n" "Calibration request:\n" "\t%s -d dest_ip [-p dest_port] [-i iface] [-t delay]" " [-n nb_packets] direction x y z\n" "\n" "Options:\n" "\t-h\t\tPrint this help.\n" "\t-V\t\tPrint version information.\n" "\t-d dest_ip\tDestination IP address of the localisation request.\n" "\t-p dest_port\tDestination port of the localisation request" " (default: %d).\n" "\t-t delay\tTime between each packet transmission (default: %d" " µs for a normal request, %d µs for a calibration request).\n" "\t-n nb_packets\tNumber of packet transmitted for the request" " (default: %d for a normal request, %d for a calibration" " request).\n" "\t-i iface\tName of the network interface used to transmit the" " request (e.g. \"eth2\"). If this option is absent, interface" " is selected automatically. You must be root to use this" " option.\n" "\t-l [port]\tWait for the computed position and display it." " Optional argument 'port' allows to specify the listening" " port (default: %d).\n" , program_name, program_name, OWL_DEFAULT_REQUEST_PORT, DEFAULT_DELAY_NORMAL, DEFAULT_DELAY_CALIB, DEFAULT_NBPKT_NORMAL, DEFAULT_NBPKT_CALIB, OWL_DEFAULT_RESULT_PORT ) ; } void print_version() { printf("This is OwlPS Client for AR.Drone, part of the Open Wireless" " Positioning System project.\n" "Version: %s.\n", #ifdef OWLPS_VERSION OWLPS_VERSION #else // OWLPS_VERSION "unknown version" #endif // OWLPS_VERSION:w ) ; } /* *** Commands for drone *** */ void calcul_trajectory(float *distance, float *angle) { int a, b; float dist_av, dist_ar, dep ; (*distance) = dist_av = dist_ar = dep = (*angle) = 0 ; dist_av = sqrt(pow(share_relatif_cons.x - share_relatif.x, 2) + pow(share_relatif_cons.y - share_relatif.y, 2)) ; dist_ar = sqrt(pow(share_relatif_cons.x - share_relatif_retro.x, 2) + pow(share_relatif_cons.x - share_relatif_retro.y, 2)) ; dep = sqrt(pow(share_relatif.x - share_relatif_retro.x, 2) + pow(share_relatif.y - share_relatif_retro.y, 2)) ; *angle = acos((pow(dist_ar, 2) + pow(dep, 2) - pow(dist_av, 2)) / (2 * dist_ar * dep)) ; *angle = (*angle)*180/M_PI; *distance = dist_av; a=share_relatif_cons.x-share_relatif.x; b=share_relatif_cons.y-share_relatif.y; /* ********** */ /* a b | R */ /* F F | F */ /* F T | T */ /* T F | T */ /* T T | F */ /* ********** */ if(a*b>=0) (*angle) = -(*angle); } int check_destination(struct point drone, struct point target) { if(oc_distance_between(drone, target)<3) return 1; else return 0; } float calcul_angle() { float captarget = oc_cap(share_gps, share_gps_cons); if((share_gps.cap-captarget-180)<0) return ((int)(360 - share_gps.cap + (captarget-180))%360)-180; else return ((int)(360 - share_gps.cap + (180 - captarget))%360)-180; } int read_position(char *type) { if(!strcmp(type,"WIFI")) printf("Localisation by WIFI\n"); else if (!strcmp(type,"GPS")) { printf("Localisation by GPS\n"); while((share_gps.lat == 0 || share_gps.lon == 0) && (share_gps_origin.lat == 0 || share_gps_origin.lon == 0)) { if(share_gps.lat!=0 && share_gps.lon!=0) { share_gps_origin.lat = share_gps.lat; share_gps_origin.lon = share_gps.lon; } usleep(100000); } share_relatif = oc_convert(share_gps_origin, share_gps); if(share_gps_retro.lat!=-999.00&&share_gps_retro.lon!=-999.00) share_relatif_retro = oc_convert(share_gps_origin, share_gps_retro) ; share_relatif_cons = oc_convert(share_gps_origin, share_gps_cons); //printf("X current : %f Y current : %f X old : %f Y old : %f X target : %f Y target : %f\n",share_relatif.x, share_relatif.y, -share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y); } else return -1; return 0; } int cmd_drone_take_off() { printf("Take-off...\n"); state_flying=1; return 0; } int cmd_drone_landing() { printf("Landing....\n"); state_flying=0; return 0; } int cmd_drone_init_direction() { //int boucle; float speed; assert(sizeof(float) == 4) ; if(state_flying==0) return -1; //Implemented socket communication printf("Initialisation...\n"); speed=-0.5; sem_wait(&lock); //for(boucle=0;boucle<3;boucle++) while(share_gps_retro.lat == -999.00 || share_gps_retro.lon == -999.00) { int *floatint_speed = (int32_t*) &speed ; sprintf(sendline,"AT*PCMD=%d,1,0,%d,0,0\r", i, *floatint_speed); sendto(sockcmdfd,sendline,strlen(sendline),0, (struct sockaddr *)&servcmd,sizeof(servcmd)); usleep(100000); } sprintf(sendline,"AT*PCMD=%d,1,0,0,0,0\r", i); sendto(sockcmdfd,sendline,strlen(sendline),0, (struct sockaddr *)&servcmd,sizeof(servcmd)); sem_post(&lock); return 1; } int cmd_drone_move(float speed_pitch, int angle) { float speed_yaw; //if(state_flying==0) //return -1; //else //{ speed_yaw=((float)angle/180); sem_wait(&lock); int *floatint_speed_pitch = (int32_t*) &speed_pitch; int *floatint_speed_yaw = (int32_t*) &speed_yaw; sprintf(sendline, "AT*PCMD=%d,1,0,%d,0,%d\r", i, *floatint_speed_pitch, *floatint_speed_yaw) ; //printf("%s\n", sendline); ++i ; sendto(sockcmdfd,sendline,strlen(sendline),0, (struct sockaddr *)&servcmd,sizeof(servcmd)); //printf("[ATPCMD] pitch : %f yaw : %f\n", speed_pitch, speed_yaw); //} sem_post(&lock); return 0; } void drone_err(int err) { char msg[100]; switch(err) { case 1 : strcpy(msg, "Undetermined position or invalid localisation type," " canceled\n") ; break; case 2 : strcpy(msg,"Taking-off failed, canceled\n"); break; case 3 : strcpy(msg,"Calibration failed, canceled\n"); break; case 4 : strcpy(msg,"Can not move drone, it's not flying, canceled\n"); break; } printf("%s",msg); //drone_handler_sigint(SIGINT); } int drone_socket_create(void) { sockcmdfd=socket(AF_INET,SOCK_DGRAM,0); bzero(&servcmd,sizeof(servcmd)); servcmd.sin_family = AF_INET; servcmd.sin_addr.s_addr=inet_addr(IP); servcmd.sin_port=htons(PORT); return sockcmdfd; } int drone_socket_close(void) { return close(sockcmdfd); } void* drone_socket_watchdog(void* NULL_value) { struct timeval start, end; while(run) { gettimeofday(&start, NULL); usleep(8000); memset(sendline, 0, sizeof(sendline)); if(i==-1) { strncpy(sendline, "AT*CONFIG=1,\"control:outdoor\",\"TRUE\"\r" "AT*CONFIG=2,\"general:navdata_demo\",\"TRUE\"\r" "AT*CONFIG=3,\"control:control_yaw\",\"2.5\"\r" "AT*PMODE=4,2\rAT*MISC=5,2,20,2000,3000\r", LENSTR) ; i=6; } else { sem_wait(&lock); snprintf(count, 100, "%d", i); strncat(sendline, ATREF, LENSTR); strncat(sendline, count, LENSTR); if(state_flying==0) strncat(sendline, LANDING, LENSTR); else if(state_flying==1) strncat(sendline, TAKEOFF, LENSTR); else strncat(sendline, LANDING, LENSTR); strncat(sendline, "\r", LENSTR); } ++i ; if(i>=101) { strncat(sendline, "AT*COMWDG=", LENSTR); snprintf(count, 100, "%d", i); strncat(sendline, count, LENSTR); strncat(sendline, "\r", LENSTR); i=1; } sendto(sockcmdfd,sendline,strlen(sendline),0, (struct sockaddr *)&servcmd,sizeof(servcmd)); //printf("[WATCHDOG] %s\n", sendline); sem_post(&lock); gettimeofday(&end, NULL); //long calc = // (end.tv_sec * 1000000 + end.tv_usec) - // (start.tv_sec * 1000000 + start.tv_usec) ; //printf("[WATCHDOG] Time usec : %ld\n", calc); } pthread_exit(NULL_value); } void drone_handler_sigint(const int num) { if(num==SIGINT) { cmd_drone_landing(); sleep(2); run=0; close(socksendfd) ; close(sockreceivefd) ; drone_socket_close() ; exit(0); } } /* *** Linked list functions *** */ consigne* list_add_front(consigne* list, double latitude, double longitude, double altitude) { consigne* new_consigne = malloc(sizeof(consigne)); new_consigne->lat = latitude; new_consigne->lon = longitude; new_consigne->alt = altitude; new_consigne->next = list; return new_consigne; } consigne* list_add_back(consigne* list, double lat, double lon, double alt) { consigne* new_consigne = malloc(sizeof(consigne)); new_consigne->lat = lat; new_consigne->lon = lon; new_consigne->alt = alt; new_consigne->next = NULL; if(list == NULL) return new_consigne ; else { consigne* tmp=list; while(tmp->next != NULL) { tmp = tmp->next; } tmp->next = new_consigne; return list; } }