[ARDrone] Add algorythm for moving
Add calcul for drone command, and command function. Fix compilation warning.
This commit is contained in:
parent
e093dcd7e3
commit
6023295880
|
@ -14,12 +14,13 @@
|
||||||
#include <net/if.h>
|
#include <net/if.h>
|
||||||
|
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <pthread.h>
|
|
||||||
|
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
#include <sys/types.h>
|
#include <sys/types.h>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
|
|
||||||
|
#include <assert.h>
|
||||||
|
|
||||||
#define SEPARATOR ','
|
#define SEPARATOR ','
|
||||||
#define EOL '$'
|
#define EOL '$'
|
||||||
#define TYPE "GPS"
|
#define TYPE "GPS"
|
||||||
|
@ -66,10 +67,28 @@ 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)
|
||||||
double share_x, share_y, share_z;
|
//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;
|
struct point share_gps;
|
||||||
double share_x_cons=-999.0, share_y_cons=-999.00, share_z_cons=-999.0, share_t_cons=-999.0;
|
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[])
|
int main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
|
@ -100,7 +119,10 @@ int main(int argc, char *argv[])
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
while(1)
|
||||||
|
{
|
||||||
|
usleep(200000);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@ -217,10 +239,14 @@ void traficGPS(int gps) {
|
||||||
|
|
||||||
if (datagps.orilat=='S') datagps.latitude=-datagps.latitude;
|
if (datagps.orilat=='S') datagps.latitude=-datagps.latitude;
|
||||||
if (datagps.orilon=='W') datagps.longitude=-datagps.longitude;
|
if (datagps.orilon=='W') datagps.longitude=-datagps.longitude;
|
||||||
share_lat=datagps.latitude;
|
share_gps_retro.lat=share_gps.lat;
|
||||||
share_lon=datagps.longitude;
|
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("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)
|
if (strcmp(cmd, "ReadPosition")==0)
|
||||||
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_gps.lat,share_gps.lon,share_gps.alt);
|
||||||
else if (strcmp(cmd, "SendGeoSetpoint")==0) {
|
else if (strcmp(cmd, "SendGeoSetpoint")==0) {
|
||||||
share_lat_cons=atof(sx);
|
share_gps_cons.lat=atof(sx);
|
||||||
share_lon_cons=atof(sy);
|
share_gps_cons.lon=atof(sy);
|
||||||
share_alt_cons=atof(sz);
|
share_gps_cons.alt=atof(sz);
|
||||||
share_t_cons=atof(st);
|
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) {
|
else if (strcmp(cmd, "SendSetpoint")==0) {
|
||||||
share_x_cons=atof(sx);
|
share_relatif_cons.x=atof(sx);
|
||||||
share_y_cons=atof(sy);
|
share_relatif_cons.y=atof(sy);
|
||||||
share_z_cons=atof(sz);
|
share_relatif_cons.z=atof(sz);
|
||||||
share_t_cons=atof(st);
|
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
|
else
|
||||||
strcat(response,"Not understand\n");
|
strcat(response,"Not understand\n");
|
||||||
|
@ -810,3 +836,197 @@ void print_version()
|
||||||
#endif // OWLPS_VERSION:w
|
#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 <stdio.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
#include <math.h>
|
||||||
|
/* include OpenCoordinate */
|
||||||
|
#include "oc.c"
|
||||||
|
|
||||||
#define DEBUG
|
#define DEBUG
|
||||||
|
|
||||||
|
@ -18,6 +22,24 @@
|
||||||
/* Program arguments (getopt string) */
|
/* Program arguments (getopt string) */
|
||||||
#define OPTIONS "d:hi:l::n:p:t:V"
|
#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 */
|
/* Function headers */
|
||||||
void parse_command_line(int argc, char **argv) ;
|
void parse_command_line(int argc, char **argv) ;
|
||||||
|
@ -54,14 +76,34 @@ struct result
|
||||||
|
|
||||||
struct gps
|
struct gps
|
||||||
{
|
{
|
||||||
float time;
|
float time;
|
||||||
char state;
|
char state;
|
||||||
double latitude;
|
double latitude;
|
||||||
char orilat;
|
char orilat;
|
||||||
double longitude;
|
double longitude;
|
||||||
char orilon;
|
char orilon;
|
||||||
float speed;
|
float speed;
|
||||||
float cap;
|
float cap;
|
||||||
int date;
|
int date;
|
||||||
float decmag;
|
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