static void send_reached_waypoint(void)
{
if(waypoint_info.reached_waypoint.is_update == true) {
mavlink_message_t msg;
/* Notice the ground station that the vehicle is reached at the
waypoint */
mavlink_msg_mission_item_reached_pack(1, 0, &msg,
waypoint_info.reached_waypoint.number);
send_package(&msg);
waypoint_info.reached_waypoint.is_update = false;
}
}
void navigation_mission_item_reached()
{
//send MISSION_ITEM_REACHED ( #46 )
COMM_FRAME msg_frame;
mavlink_msg_mission_item_reached_pack(mavlink_system.sysid, MAV_COMP_ID_MISSIONPLANNER, &(msg_frame.mavlink_message),
mission_items.current_index);
comm_mavlink_broadcast(&(msg_frame));
mission_items.item[mission_items.current_index].current = 0;
if(mission_items.item[mission_items.current_index].autocontinue)
{
if(navigation_next_mission_item(NULL, NULL, &(msg_frame.mavlink_message)) == REPLY_TO_SENDER)
{
comm_mavlink_broadcast(&(msg_frame));
}
}
}
Mission_handler::update_status_t Mission_handler_navigating::update()
{
/**********************************
Determine if arrived for first time
**********************************/
// Find distance to waypoint
local_position_t wpt_pos = waypoint_.local_pos();
float rel_pos[3];
for (int i = 0; i < 3; i++)
{
rel_pos[i] = wpt_pos[i] - ins_.position_lf()[i];
}
float dist2wp_sqr = vectors_norm_sqr(rel_pos);
float vel_sqr = vectors_norm_sqr(ins_.velocity_lf().data());
// Check if radius is available
float radius = 0.0f;
waypoint_.radius(radius);
if (radius == 0.0f)
{
// TODO use a configuration for this default radius
radius = 2.0f;
}
// Check if we reached the waypoint
bool is_arrived = (dist2wp_sqr < (radius * radius)) && (vel_sqr < 1.0f); // TODO use a config for this speed threshold
if (is_arrived)
{
// If we are near the waypoint but the flag has not been set, do this once ...
if (!waypoint_reached_)
{
// Send debug log ...
print_util_dbg_print("Waypoint reached, distance: ");
print_util_dbg_putfloat(sqrt(dist2wp_sqr), 3);
print_util_dbg_print(" m. Less than acceptable radius:");
print_util_dbg_putfloat(sqrt(radius * radius), 3);
print_util_dbg_print(" m.\r\n");
// ... as well as a mavlink message ...
mavlink_message_t msg;
mavlink_msg_mission_item_reached_pack(mavlink_stream_.sysid(),
mavlink_stream_.compid(),
&msg,
waypoint_handler_.current_waypoint_index());
mavlink_stream_.send(&msg);
// ... and record the travel time ...
travel_time_ = time_keeper_get_ms() - start_time_;
// ... and set to waiting at waypoint ...
waypoint_reached_ = true;
// If the waypoint is not autocontinue, use its heading
if (waypoint_.autocontinue() == false)
{
// Use waypoint's heading
waypoint_.heading(position_command_.heading);
}
}
}
/*******************
Determine status code
********************/
// First check if we have reached the waypoint
if (waypoint_reached_)
{
// Then ensure that we are in autocontinue
if ((waypoint_.autocontinue() == 1) && (waypoint_handler_.waypoint_count() > 1))
{
// Differentiate between dubin and direct to
return MISSION_FINISHED;
}
}
return MISSION_IN_PROGRESS;
}
