[ARDrone] Add algorythm for moving
Add calcul for drone command, and command function. Fix compilation warning.
This commit is contained in:
Florian Taillard
parent
e093dcd7e3
commit
6023295880
|
|
@ -14,12 +14,13 @@
|
|||
#include <net/if.h>
|
||||
|
||||
#include <time.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
#define SEPARATOR ','
|
||||
#define EOL '$'
|
||||
#define TYPE "GPS"
|
||||
|
|
@ -66,10 +67,28 @@ uint8_t *packet = NULL ; // Packet to send
|
|||
uint_fast16_t packet_size ; // Packet size
|
||||
|
||||
// Shared variables for OwlSIG (Geo Information System)
|
||||
double share_x, share_y, share_z;
|
||||
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;
|
||||
//double share_x, share_y, share_z;
|
||||
|
||||
struct point share_gps;
|
||||
struct point share_gps_retro;
|
||||
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, lock=0, i=-1;
|
||||
|
||||
/* 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[])
|
||||
{
|
||||
|
|
@ -100,7 +119,10 @@ int main(int argc, char *argv[])
|
|||
return -1;
|
||||
}
|
||||
|
||||
|
||||
while(1)
|
||||
{
|
||||
usleep(200000);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -217,10 +239,14 @@ void traficGPS(int gps) {
|
|||
|
||||
if (datagps.orilat=='S') datagps.latitude=-datagps.latitude;
|
||||
if (datagps.orilon=='W') datagps.longitude=-datagps.longitude;
|
||||
share_lat=datagps.latitude;
|
||||
share_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;
|
||||
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("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);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -297,20 +323,20 @@ void* thread_control(void* NULL_value)
|
|||
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);
|
||||
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_lat_cons=atof(sx);
|
||||
share_lon_cons=atof(sy);
|
||||
share_alt_cons=atof(sz);
|
||||
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_lat_cons,share_lon_cons,share_alt_cons,share_t_cons);
|
||||
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);
|
||||
}
|
||||
else if (strcmp(cmd, "SendSetpoint")==0) {
|
||||
share_x_cons=atof(sx);
|
||||
share_y_cons=atof(sy);
|
||||
share_z_cons=atof(sz);
|
||||
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_x_cons,share_y_cons,share_z_cons,share_t_cons);
|
||||
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
|
||||
strcat(response,"Not understand\n");
|
||||
|
|
@ -810,3 +836,197 @@ void print_version()
|
|||
#endif // OWLPS_VERSION:w
|
||||
) ;
|
||||
}
|
||||
|
||||
|
||||
/* Commands for drone */
|
||||
|
||||
|
||||
|
||||
float calcul_trajectory(void)
|
||||
{
|
||||
float dist_av, dist_ar, dep ;
|
||||
float angle ;
|
||||
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;
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
float calcul_speed(float angle)
|
||||
{
|
||||
float time;
|
||||
time = angle/180;
|
||||
return time;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
sleep(1);
|
||||
}
|
||||
share_relatif = oc_convert(share_gps_origin, share_gps);
|
||||
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);
|
||||
double angle=oc_cap(share_gps_retro, share_gps);
|
||||
printf("Angle : %f\n",angle);
|
||||
|
||||
}
|
||||
else return -1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int cmd_drone_take_off(void)
|
||||
{
|
||||
printf("Take-off...\n");
|
||||
state_flying=1;
|
||||
return 0;
|
||||
}
|
||||
int cmd_drone_landing(void)
|
||||
{
|
||||
printf("Landing....\n");
|
||||
state_flying=0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int cmd_drone_init_direction(void)
|
||||
{
|
||||
int boucle;
|
||||
float speed;
|
||||
assert(sizeof(float) == 4) ;
|
||||
|
||||
if(state_flying==0)
|
||||
return -1;
|
||||
//Implemented socket communication
|
||||
printf("Calibration...\n");
|
||||
speed=-0.5;
|
||||
while(lock==1);
|
||||
lock=1;
|
||||
for(boucle=0;boucle<3;boucle++)
|
||||
{
|
||||
int *floatint_speed = (int32_t*) &speed ;
|
||||
sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,0\r", i, *floatint_speed);
|
||||
}
|
||||
lock=0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
int cmd_drone_move(float speed_pitch, float speed_yaw)
|
||||
{
|
||||
if(state_flying==0)
|
||||
return -1;
|
||||
else
|
||||
{
|
||||
while(lock==1);
|
||||
lock=1;
|
||||
int *floatint_speed_pitch = (int32_t*) &speed_pitch;
|
||||
int *floatint_speed_yaw = (int32_t*) &speed_yaw;
|
||||
sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,%d\r",i,*floatint_speed_pitch,*floatint_speed_yaw);
|
||||
i++;
|
||||
sendto(sockcmdfd,sendline,strlen(sendline),0,
|
||||
(struct sockaddr *)&servcmd,sizeof(servcmd));
|
||||
|
||||
}
|
||||
lock=0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void drone_err(int err)
|
||||
{
|
||||
char msg[100];
|
||||
|
||||
switch(err)
|
||||
{
|
||||
case 1 :
|
||||
strcpy(msg,"Position is not determined or type of localisation is invalid, 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);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
while(1)
|
||||
{
|
||||
usleep(50000);
|
||||
memset(sendline, 0, sizeof(sendline));
|
||||
if(i==-1)
|
||||
{
|
||||
strncpy(sendline,"AT*CONFIG=1,\"general:navdata_demo\",\"TRUE\"\rAT*CONFIG=2,\"control:control_yaw\",\"3.14\"\rAT*PMODE=3,2\rAT*MISC=4,2,20,2000,3000\r", LENSTR);
|
||||
i=4;
|
||||
}
|
||||
else
|
||||
{
|
||||
while(lock==1);
|
||||
lock=1;
|
||||
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));
|
||||
lock=0;
|
||||
}
|
||||
pthread_exit(NULL_value);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,4 +1,8 @@
|
|||
#include <stdio.h>
|
||||
#include <pthread.h>
|
||||
#include <math.h>
|
||||
/* include OpenCoordinate */
|
||||
#include "oc.c"
|
||||
|
||||
#define DEBUG
|
||||
|
||||
|
|
@ -18,6 +22,24 @@
|
|||
/* Program arguments (getopt string) */
|
||||
#define OPTIONS "d:hi:l::n:p:t:V"
|
||||
|
||||
/* Type of position (WIFI or GPS) */
|
||||
#define TYPE "GPS"
|
||||
|
||||
/* 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) ;
|
||||
|
|
@ -54,14 +76,34 @@ struct result
|
|||
|
||||
struct gps
|
||||
{
|
||||
float time;
|
||||
char state;
|
||||
double latitude;
|
||||
char orilat;
|
||||
double longitude;
|
||||
char orilon;
|
||||
float speed;
|
||||
float cap;
|
||||
int date;
|
||||
float decmag;
|
||||
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
|
||||
float calcul_trajectory(void);
|
||||
float calcul_speed(float);
|
||||
|
|
|
|||
Loading…
Reference in New Issue