bool vector_field_waypoint_update(vector_field_waypoint_t* vector_field)
{
float tmp_vector[3];
float pos_obj[3];
// Re-init velocity command
vector_field->velocity_command->mode = VELOCITY_COMMAND_MODE_LOCAL;
vector_field->velocity_command->xyz[X] = 0.0f;
vector_field->velocity_command->xyz[Y] = 0.0f;
vector_field->velocity_command->xyz[Z] = 0.0f;
// Compute vector field for floor avoidance
vector_field_floor(vector_field->pos_est->local_position.pos,
20,
tmp_vector);
// Add floor vector field to the velocity command
vector_field->velocity_command->xyz[X] += tmp_vector[X];
vector_field->velocity_command->xyz[Y] += tmp_vector[Y];
vector_field->velocity_command->xyz[Z] += tmp_vector[Z];
// Go through waypoint list
for (uint16_t i = 0; i < vector_field->waypoint_handler->waypoint_count(); ++i)
{
// Get waypoint in NED coordinates
Mavlink_waypoint_handler::waypoint_struct_t waypoint = convert_waypoint_to_local_ned(&vector_field->waypoint_handler->waypoint_list[i],
&vector_field->pos_est->local_position.origin);
// Get object position
pos_obj[X] = waypoint.x;
pos_obj[Y] = waypoint.y;
pos_obj[Z] = waypoint.z;
switch (waypoint.command)
{
// Attractive object
case 22:
vector_field_attractor(vector_field->pos_est->local_position.pos,
pos_obj,
waypoint.param1, // attractiveness
tmp_vector);
break;
// Cylindrical repulsive object
case 17:
vector_field_repulsor_cylinder(vector_field->pos_est->local_position.pos,
pos_obj,
waypoint.param1, // repulsiveness
waypoint.param2, // max_range
waypoint.param3, // safety_radius
tmp_vector);
break;
// Spherical repulsive object
case 18:
vector_field_repulsor_sphere(vector_field->pos_est->local_position.pos,
pos_obj,
waypoint.param1, // repulsiveness
waypoint.param2, // max_range
waypoint.param3, // safety_radius
tmp_vector);
break;
// Circular waypoint
case 16:
vector_field_circular_waypoint(vector_field->pos_est->local_position.pos,
pos_obj,
waypoint.param1, // attractiveness
waypoint.param2, // cruise_speed
waypoint.param3, // radius
tmp_vector);
break;
// Unknown object
default:
tmp_vector[X] = 0.0f;
tmp_vector[Y] = 0.0f;
tmp_vector[Z] = 0.0f;
break;
}
// Add vector field to the velocity command
vector_field->velocity_command->xyz[X] += tmp_vector[X];
vector_field->velocity_command->xyz[Y] += tmp_vector[Y];
vector_field->velocity_command->xyz[Z] += tmp_vector[Z];
}
// Limit velocity
float vel_norm = vectors_norm(vector_field->velocity_command->xyz);
if (vel_norm > 5.0f)
{
vector_field->velocity_command->xyz[X] = 5.0f * vector_field->velocity_command->xyz[X] / vel_norm;
vector_field->velocity_command->xyz[Y] = 5.0f * vector_field->velocity_command->xyz[Y] / vel_norm;
vector_field->velocity_command->xyz[Z] = 5.0f * vector_field->velocity_command->xyz[Z] / vel_norm;
}
return true;
}
bool vector_field_waypoint_update(vector_field_waypoint_t* vector_field)
{
float tmp_vector[3];
float pos_obj[3];
// Re-init velocity command
vector_field->velocity_command->xyz[X] = 0.0f;
vector_field->velocity_command->xyz[Y] = 0.0f;
vector_field->velocity_command->xyz[Z] = 0.0f;
// Compute vector field for floor avoidance
vector_field_floor(vector_field->ins->position_lf().data(),
20,
tmp_vector);
// Add floor vector field to the velocity command
vector_field->velocity_command->xyz[X] += tmp_vector[X];
vector_field->velocity_command->xyz[Y] += tmp_vector[Y];
vector_field->velocity_command->xyz[Z] += tmp_vector[Z];
// Go through waypoint list
for (uint16_t i = 0; i < vector_field->waypoint_handler->waypoint_count(); ++i)
{
const Waypoint& waypoint = vector_field->waypoint_handler->waypoint_from_index(i);
local_position_t local_wpt = waypoint.local_pos();
// Get object position
pos_obj[X] = local_wpt[X];
pos_obj[Y] = local_wpt[Y];
pos_obj[Z] = local_wpt[Z];
switch (waypoint.command())
{
// Attractive object
case 22:
vector_field_attractor(vector_field->ins->position_lf().data(),
pos_obj,
waypoint.param1(), // attractiveness
tmp_vector);
break;
// Cylindrical repulsive object
case 17:
vector_field_repulsor_cylinder(vector_field->ins->position_lf().data(),
pos_obj,
waypoint.param1(), // repulsiveness
waypoint.param2(), // max_range
waypoint.param3(), // safety_radius
tmp_vector);
break;
// Spherical repulsive object
case 18:
vector_field_repulsor_sphere(vector_field->ins->position_lf().data(),
pos_obj,
waypoint.param1(), // repulsiveness
waypoint.param2(), // max_range
waypoint.param3(), // safety_radius
tmp_vector);
break;
// Circular waypoint
case 16:
vector_field_circular_waypoint(vector_field->ins->position_lf().data(),
pos_obj,
waypoint.param1(), // attractiveness
waypoint.param2(), // cruise_speed
waypoint.param3(), // radius
tmp_vector);
break;
// Unknown object
default:
tmp_vector[X] = 0.0f;
tmp_vector[Y] = 0.0f;
tmp_vector[Z] = 0.0f;
break;
}
// Add vector field to the velocity command
vector_field->velocity_command->xyz[X] += tmp_vector[X];
vector_field->velocity_command->xyz[Y] += tmp_vector[Y];
vector_field->velocity_command->xyz[Z] += tmp_vector[Z];
}
// Limit velocity
float vel_norm = vectors_norm(vector_field->velocity_command->xyz.data());
if (vel_norm > 5.0f)
{
vector_field->velocity_command->xyz[X] = 5.0f * vector_field->velocity_command->xyz[X] / vel_norm;
vector_field->velocity_command->xyz[Y] = 5.0f * vector_field->velocity_command->xyz[Y] / vel_norm;
vector_field->velocity_command->xyz[Z] = 5.0f * vector_field->velocity_command->xyz[Z] / vel_norm;
}
return true;
}