void Viewer::loadView(int i) {
if (m_saved_views[i] == glm::mat4(0)) {
navigation()->loadView(navigation()->defaultView());
} else {
navigation()->loadView(m_saved_views[i]);
}
}
void Viewer::saveView(int i) {
m_saved_views[i] = navigation()->viewMatrix();
QSettings s;
s.beginGroup(SETTINGS_SAVED_VIEWS);
s.setValue("view_" + QString::number(i+1), mat2string(navigation()->viewMatrix()));
s.endGroup();
}
int main ()
{//coordonne de depart
char str_gps[] = "$GPRMC,220516,A,4851.600,N,00220.4000,W,173.8,231.8,130694,004.2,W*70";
struct gps_coordinate *depart;
depart = malloc(sizeof(struct gps_coordinate));
printf("%s\n", str_gps);
extract_coord(str_gps,depart);
//coordonnne de dest
struct gps_coordinate *dest;
dest = malloc(sizeof(struct gps_coordinate));
char str_gps_dest[] = "$GPRMC,220516,A,5038.400,N,00304.200,E,173.8,231.8,130694,004.2,W*70";
printf("%s\n", str_gps_dest);
extract_coord(str_gps_dest,dest);
/*depart->latitude = 43.570;
depart->longitude = 1.465 ;
dest->latitude = 43.570 ;
dest->longitude = 1.465;*/
double *distance, *angle;
angle = malloc(sizeof(double));
distance = malloc(sizeof(double));
navigation(depart, dest, distance, angle);
printf("angle distance %f %f \n", *angle , *distance);
return 0;
}
/*
All steps for reaching target
1 Take off and Calibration
2 Receive coordinates gps and calculate direction
3 Turn to direction, then move forward in 3s
4 Return to step 2, if distance < 4 m , Landing ...
*/
void go_target() {
//1 DONE, 0 not yet
static int landed = 1;
static int calibration = 0;
static int mission = 0;
comm_datas datas;
comm_datas_target datas_target;
struct gps_coordinate depart;
struct gps_coordinate relatif_error;
//gps_error error_dest, error_depart;
mov speed;
static double distance,angle;
if ( (landed == 1) && (calibration == 0) ){
printf("start calibration\n");
calibrate_magneto(NULL);
sleep(4);
calibration = 1;
printf("Calibration done\n");
}
if ( (landed == 1) && (calibration == 1) && (mission == 0) ) {
datas = get_comm_datas();
extract_coord(datas.gprmc_string,&depart);
//printf("COORD UAV %f %f\n",depart.longitude,depart.latitude);
//extract_error(datas.gpgga_string,&error_depart);
datas_target = get_comm_datas_target();
//printf("COORD TG %f %f\n",datas_target.dest.longitude,datas_target.dest.latitude);
//extract_coord(datas_target.gprmc_string,&dest);
//extract_error(datas_target.gpgga_string,&error_dest);
if ((check_gps_coord_struc(&depart) > 0) && (check_gps_coord_struc(&datas_target.dest) > 0)) {
navigation(&depart, &datas_target.dest, &distance, &angle, NULL); //&relatif_error
printf("DISTANCE %f\n\n\n\n",distance);
printf("turn angle %f, &angle",angle);
turn_angle2(angle ,5.0);
if (distance > 5.0){
printf("La distance restante est %f\n",distance);
fprintf(redir_sortie,"La distance restante est %f\n",distance);
fflush(redir_sortie);
//send_order(forward,NULL);
speed.power = 3;
//speed.distance = distance/2;
//speed.time = d2time(speed.power,speed.distance);
send_order(forward,(void *)&speed);
sleep(3);
//sleep(4);
} else {
send_order(land,NULL);
printf("LANDING \n");
mission = 1;
//exit(0);
}
}
}
}
void go_target2() {
//1 DONE, 0 not yet
static int landed = 1;
static int calibration = 0;
static int mission = 0;
static float last_distance = 1000.0;
comm_datas datas;
comm_datas_target datas_target;
struct gps_coordinate depart;
struct gps_coordinate relatif_error;
mov speed;
static double distance,angle;
if ( (landed == 1) && (calibration == 0) ){
printf("start calibration\n");
calibrate_magneto(NULL);
sleep(4);
calibration = 1;
printf("Calibration done\n");
}
if ( (landed == 1) && (calibration == 1) && (mission == 0) ) {
datas = get_comm_datas();
extract_coord(datas.gprmc_string,&depart);
datas_target = get_comm_datas_target();
if ((check_gps_coord_struc(&depart) > 0) && (check_gps_coord_struc(&datas_target.dest) > 0)) {
navigation(&depart, &datas_target.dest, &distance, &angle, NULL); //&relatif_error
printf("DISTANCE %f\n\n\n\n",distance);
fprintf(redir_sortie,"DISTANCE %f\n\n\n\n",distance);
printf("turn angle %f, &angle",angle);
fprintf(redir_sortie,"turn angle %f, &angle",angle);
turn_angle2(angle ,5.0);
while (last_distance > distance) {
printf("La distance restante est %f\n",distance);
fprintf(redir_sortie,"La distance restante est %f\n",distance);
fflush(redir_sortie);
speed.power = 3;
send_order(forward,(void *)&speed);
last_distance = distance;
sleep(1);
}
if (distance < 10.0){
send_order(land,NULL);
printf("LANDING \n");
mission = 1;
fflush(redir_sortie);
exit(0);
}
}
}
}
void test1_de_navigation() {
struct gps_coordinate *depart;
depart = malloc(sizeof(struct gps_coordinate));
depart->latitude = 90;//43.570546;
//depart->longitude = 2;//1.468281;
struct gps_coordinate *dest;
dest = malloc(sizeof(struct gps_coordinate));
dest->latitude = -90;//43.571157;
//dest->longitude = 2;//1.467671;
double distance;// = malloc(sizeof(double));
double var;
double angle; //= malloc(sizeof(double));
navigation(depart, dest, &distance, &angle);
//CU_ASSERT( (*distance - M_PI*6371*1000) < 1e-3) ;
//CU_ASSERT( (*angle - (180)) < 1e-3);
dest->longitude = -180.0;
depart->longitude = -180.0;
var = -180.0;
while (var < 180.0)
{
printf("test with longitude of %f \n", dest->longitude);
navigation(depart, dest, &distance, &angle);
var = var + 1.0;
depart->latitude = 90.0;
dest->latitude = -90.0;
depart->longitude = var;
dest->longitude = var;
CU_ASSERT( (distance - M_PI*6371*1000) < 1e-5) ;
CU_ASSERT( (angle - (180)) < 1e-5);
}
}
TuiNodeResponse TuiNodeActivate::handleKey(int key)
{
if (key==Qt::Key_Return)
{
emit(activate());
TuiNodeResponse response;
response.accepted = true;
response.newNode = this;
return response;
}
else
{
return navigation(key);
}
}
void test_target() {
comm_datas datas;
comm_datas_target datas_target;
struct gps_coordinate depart;
double distance,angle;
datas = get_comm_datas();
extract_coord(datas.gprmc_string,&depart);
printf("COORD UAV %f %f\n",depart.longitude,depart.latitude);
datas_target = get_comm_datas_target();
printf("COORD TG %f %f\n",datas_target.dest.longitude,datas_target.dest.latitude);
navigation(&depart, &datas_target.dest, &distance, &angle, NULL); //&relatif_error
printf("angle %f\n",angle);
sleep(3);
}
TuiNodeResponse TuiNode::handleKey(int key)
{
return navigation(key);
}
void Viewer::on_actionTopRightView_triggered() {
navigation()->loadView(navigation()->topRightView());
}
void Viewer::on_actionBottomView_triggered() {
navigation()->loadView(navigation()->bottomview());
}
void Viewer::on_actionBackView_triggered() {
navigation()->loadView(navigation()->backview());
}
void Viewer::on_actionLeftView_triggered() {
navigation()->loadView(navigation()->leftview());
}
void Viewer::on_actionFrontView_triggered() {
navigation()->loadView(navigation()->frontview());
}
void Viewer::keyReleaseEvent( QKeyEvent *event )
{
if(!event->isAutoRepeat()) {
navigation()->keyReleaseEvent(event);
}
}
