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; }