inline Iterator next_point(Range const& range, Iterator it) const
{
Iterator result = it;
move_to_next_point(range, result);
// TODO: we could use either piece-boundaries, or comparison with
// robust points, to check if the point equals the last one
while(geometry::equals(*it, *result))
{
move_to_next_point(range, result);
}
return result;
}
void calcul_mission()
{
struct coordinates_ C_blue; //coordinates of drone
struct coordinates_ nextPoint;
struct coordinates_ startPoint;
mission_state_t state;
int indice = 0;
int index = 0;
int bad_move = 0;
float angle_actuel, calcul_x, calcul_y, angle_desire;
node_t **path = NULL;
C_blue.x = 0.0;
C_blue.y = 0.0;
// First of all, check if the given destination is valid
if(find_point(graph, destination.x, destination.y) == -1)
{
printf("Invalid destination: (%.2f, %.2f)\n", destination.x, destination.y);
state = FINISHED;
}
else
state = INIT;
while(state != FINISHED)
{
if(inC.flag_control_s == STATE_MANUAL)
{
state = FINISHED;
}
switch(state)
{
case INIT:
printf("[Mission state: INIT]\n");
// Wait for the data to stabilize
sleep(5);
read_data_bluetooth(&C_blue.x,&C_blue.y);
printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y);
printf("Destination point: (%f, %f)\n", destination.x, destination.y);
path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph);
if (path == NULL)
state = LOST;
else
{
printf("Path to follow\n");
int i;
for(i = 0 ; path[i] != NULL ; i++)
{
indice = i;
printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y);
}
state = RUNNING;
}
break;
case LOST:
printf("[Mission state: LOST]\n");
sleep(3);
read_data_bluetooth(&C_blue.x,&C_blue.y);
index = find_closest_node(graph, C_blue.x, C_blue.y);
startPoint.x = graph->nodes[index].x;
startPoint.y = graph->nodes[index].y;
Main_Nav = return_navdata();
angle_actuel = Main_Nav.magneto.heading_fusion_unwrapped;
angle_desire = computeDesiredAngle(C_blue, startPoint);
computeOffsetMag(&angle_desire, nav_prec, nav_suiv);
yaw_power = computeDirection(angle_actuel, angle_desire, 0.3, &yaw_move);
if(rotate_to_desired_angle(angle_desire) == -1)
{
fprintf(stderr, "[%s:%d] Error: Rotation to desired angle failed\n", __FILE__, __LINE__);
break_drone();
stop_mission();
free(graph);
return;
}
// Let the drone stabilize itself
sleep(1);
bad_move = move_to(startPoint, &C_blue);
break_drone();
// If we find a start point, we can switch to state RUNNING
if(!bad_move)
{
printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y);
printf("Destination point: (%f, %f)\n", destination.x, destination.y);
path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph);
if(path == NULL)
{
fprintf(stderr, "[%s:%d] Error: Bluetooth Data too unstable, aborting\n", __FILE__, __LINE__);
break_drone();
stop_mission();
free(graph);
return;
}
printf("Path to follow\n");
int i;
for(i = 0 ; path[i] != NULL ; i++)
{
indice = i;
printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y);
}
state = RUNNING;
}
break;
case DRIFTED:
printf("[Mission state: DRIFTED]\n");
// Pretty much the same that LOST, at the difference that we think that we know where we are
sleep(3);
read_data_bluetooth(&C_blue.x,&C_blue.y);
printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y);
printf("Destination point: (%f, %f)\n", destination.x, destination.y);
path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph);
if (path == NULL)
{
// Mmh, Bluetooth data unstable, going to LOST
state = LOST;
}
else
{
printf("New path to follow\n");
int i;
for(i = 0 ; path[i] != NULL ; i++)
{
indice = i;
printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y);
}
state = RUNNING;
}
break;
case RUNNING:
printf("[Mission state: RUNNING]\n");
sleep(3);
read_data_bluetooth(&C_blue.x,&C_blue.y);
printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y);
printf("Destination point: (%f, %f)\n", destination.x, destination.y);
// Let's start
indice--;
if (indice <= 0)
{
state = ARRIVED;
}
else
{
// FIXME: Cast path[indice] from node_t to coordinates_ since we don't use the samee structure...
nextPoint.x = path[indice]->x;
nextPoint.y = path[indice]->y;
Main_Nav = return_navdata();
angle_actuel = Main_Nav.magneto.heading_fusion_unwrapped;
angle_desire = computeDesiredAngle(C_blue, nextPoint);
/*
* angle désiré
*/
//calcul the new angle_desire
computeOffsetMag(&angle_desire, nav_prec, nav_suiv);
//printf("Calcul angle desire final v1 : %2.f\n", angle_desire);
//calculating the direction of rotation of the Z-axis for optimum positioning.
yaw_power = computeDirection(angle_actuel, angle_desire, 0.3, &yaw_move);
//printf("Valeur de la puissance mise : %1.f, Valeur de l'angle souhaité = %2.f, valeur de l'angle actuel = %2.f sens de rotation = %d\n", yaw_power, angle_desire, angle_actuel, yaw_move);
if (rotate_to_desired_angle(angle_desire) == -1)
{
break_drone();
stop_mission();
free(graph);
return;
}
// Let the drone stabilize itself
sleep(1);
// Let's move to next point
bad_move = move_to_next_point(nextPoint, (const node_t **) path, &C_blue);
// Stop the drone movement
break_drone();
if(bad_move == 1)
state = LOST;
else if(bad_move == 2)
state = DRIFTED;
else
printf("One step made\n");
}
break;
case ARRIVED:
printf("[Mission state: ARRIVED]\n");
printf("You're arrived to destination\n");
state = FINISHED;
break;
case FINISHED:
// if stop mission called
break;
default:
fprintf(stderr, "[%s:%d] Error: Unknown mission state\n", __FILE__, __LINE__);
break_drone();
stop_mission();
free(graph);
return;
break;
}
}
printf("[Mission state: FINISHED]\n");
stop_mission();
printf("fin mission\n");
free(path);
}
