111 lines
2.4 KiB
C
111 lines
2.4 KiB
C
#include <stdio.h>
|
|
#include <pthread.h>
|
|
#include <math.h>
|
|
/* include OpenCoordinate */
|
|
#include "oc.c"
|
|
|
|
#define DEBUG
|
|
|
|
/* Lenght of string algo */
|
|
#define ALGO_STRLEN 15
|
|
/* Error codes */
|
|
#define ERR_BAD_USAGE 1 // Bad program call (bad number of arguments)
|
|
|
|
/* Number of packets to send */
|
|
#define DEFAULT_NBPKT_CALIB 20 // 20 packets when calibrating
|
|
#define DEFAULT_NBPKT_NORMAL 10 // 10 packets when requesting the position
|
|
|
|
/* Delay between two packet transmissions (in microseconds) */
|
|
#define DEFAULT_DELAY_CALIB 50000 // Calibration request
|
|
#define DEFAULT_DELAY_NORMAL 25000 // Localisation request
|
|
|
|
/* Program arguments (getopt string) */
|
|
#define OPTIONS "d:hi:l::n:p:t:V"
|
|
|
|
/* Define M_PI if is not define */
|
|
#ifndef M_PI
|
|
#define M_PI 3.14159265358979323846
|
|
#endif
|
|
|
|
//Define send command
|
|
#define LENSTR 1000
|
|
#define ATREF "AT*REF="
|
|
#define LANDING ",290717696"
|
|
#define TAKEOFF ",290718208"
|
|
//Define socket
|
|
//#define IP "172.20.105.4"
|
|
#define IP "192.168.1.1"
|
|
#define PORT 5556
|
|
|
|
|
|
/* Function headers */
|
|
void parse_command_line(int argc, char **argv) ;
|
|
void parse_main_options(int argc, char **argv) ;
|
|
void check_destination_ip(void) ;
|
|
void parse_calibration_data(int argc, char **argv) ;
|
|
void check_configuration(void) ;
|
|
#ifdef DEBUG
|
|
void print_configuration(void) ;
|
|
#endif // DEBUG
|
|
void create_socket(void) ;
|
|
void make_packet(void) ;
|
|
void send_request(void) ;
|
|
void* receive_position(void*) ;
|
|
void print_usage(void) ;
|
|
void string2data(char*) ;
|
|
void traficGPS(int);
|
|
void* thread_gps(void*) ;
|
|
void* thread_send(void*) ;
|
|
void* thread_control(void*) ;
|
|
void print_error(char*) ;
|
|
void print_version(void) ;
|
|
void drone_handler_sigint(const int);
|
|
typedef struct result result;
|
|
typedef struct gps gps;
|
|
/* Struct */
|
|
|
|
struct result
|
|
{
|
|
char algo[ALGO_STRLEN];
|
|
float x;
|
|
float y;
|
|
float z;
|
|
char *check;
|
|
float err;
|
|
};
|
|
|
|
struct gps
|
|
{
|
|
float time;
|
|
char state;
|
|
double latitude;
|
|
char orilat;
|
|
double longitude;
|
|
char orilon;
|
|
float speed;
|
|
float cap;
|
|
int date;
|
|
float decmag;
|
|
};
|
|
|
|
|
|
|
|
/* Prototype drone */
|
|
|
|
void drone_err(int);
|
|
//Position
|
|
int read_position(char*);
|
|
//Socket
|
|
int drone_socket_create(void);
|
|
void* drone_socket_watchdog(void*);
|
|
int drone_socket_close(void);
|
|
//Commands drone
|
|
int cmd_drone_take_off(void); //Add socket
|
|
int cmd_drone_landing(void); //Add socket
|
|
int cmd_drone_move(float, float);
|
|
int cmd_drone_init_direction(void); //Add socket
|
|
//Algo
|
|
void calcul_trajectory(float*, float*);
|
|
float calcul_speed(float);
|
|
int check_destination(struct point, struct point);
|