static void next_waypoint(void)
{
if (extended_range == 0)
{
#if (USE_MAVLINK == 1)
mavlink_waypoint_reached(waypointIndex);
#endif
waypointIndex++;
if (waypointIndex >= numPointsInCurrentSet) waypointIndex = 0;
DPRINT("next_waypoint(%u)\r\n", waypointIndex);
set_waypoint(waypointIndex);
/*
#if (USE_MAVLINK == 1)
mavlink_waypoint_changed(waypointIndex);
#endif
if (waypointIndex == 0)
{
if (numPointsInCurrentSet > 1)
{
struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[numPointsInCurrentSet-1]);
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(previous_waypoint.loc, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
}
else
{
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(GPSlocation, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
}
setBehavior(currentWaypointSet[0].flags);
}
else
{
struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[waypointIndex-1]);
current_waypoint = wp_to_relative(currentWaypointSet[waypointIndex]);
navigate_set_goal(previous_waypoint.loc, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
setBehavior(current_waypoint.flags);
}
*/
}
else
{
navigate_set_goal(GPSlocation, current_waypoint.loc);
#if (DEADRECKONING == 0)
navigate_compute_bearing_to_goal();
#endif
}
}
static inline void MissionSetCurrent(mavlink_message_t* handle_msg)
{
mavlink_mission_set_current_t packet;
//send_text((uint8_t*)"waypoint set current\r\n");
DPRINT("mission set current\r\n");
// decode
mavlink_msg_mission_set_current_decode(handle_msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) return;
DPRINT("mission set current: %u\r\n", packet.seq);
// set current waypoint
set(PARAM_WP_INDEX, packet.seq);
set_waypoint(packet.seq);
{
//Location temp; // XXX this is gross
//temp = get_wp_with_index(packet.seq);
//set_next_WP(&temp);
}
mavlink_msg_mission_current_send(MAVLINK_COMM_0, get(PARAM_WP_INDEX));
}