void neighbors_collision_log(neighbors_t *neighbors)
{
uint8_t ind, i;
float relative_position[3];
float dist;
for (ind = 0; ind < neighbors->number_of_neighbors; ind++)
{
for (i = 0; i < 3; i++)
{
relative_position[i] = neighbors->position_estimator->local_position.pos[i] - neighbors->neighbors_list[ind].extrapolated_position[i];
}
dist = vectors_norm_sqr(relative_position);
if (dist < neighbors->collision_dist_sqr && !neighbors->collision_log.collision_flag[ind])
{
neighbors->collision_log.count_collision++;
if (neighbors->collision_log.count_near_miss != 0)
{
neighbors->collision_log.count_near_miss--;
}
neighbors->collision_log.collision_flag[ind] = true;
print_util_dbg_print("Collision with neighbor:");
print_util_dbg_print_num(ind,10);
print_util_dbg_print(", nb of collisions:");
print_util_dbg_print_num(neighbors->collision_log.count_collision,10);
print_util_dbg_print("\r\n");
}
else if (dist < neighbors->near_miss_dist_sqr && !neighbors->collision_log.near_miss_flag[ind])
{
neighbors->collision_log.count_near_miss++;
neighbors->collision_log.near_miss_flag[ind] = true;
print_util_dbg_print("Near miss with neighbor:");
print_util_dbg_print_num(ind,10);
print_util_dbg_print(", nb of near miss:");
print_util_dbg_print_num(neighbors->collision_log.count_near_miss,10);
print_util_dbg_print("\r\n");
}
else if (dist > neighbors->near_miss_dist_sqr)
{
neighbors->collision_log.collision_flag[ind] = false;
neighbors->collision_log.near_miss_flag[ind] = false;
}
}
}
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;
}
