Sorry for the lack of context.This is currently the script i am using for the move function: the variable move_as_goto is set to false.
I tried to add the for loop in the stopping command to be sure that i still send commands after arriving destination for at least 10 seconds.
After this change, i am able to perform a single move command, if i try a second one i immediately get the same failsafe error as you can see below:
void CommandHandler::move(Json::Value root)
{
Json::Value coords;
// Calculate latitude and longitude; this is calculated even if using position_ned as the actual command since it is
// useful to publish an approximate target coordinate
std::tuple<double, double> lat_lon =
calcTranslatedCoords(telem->attitude_euler().yaw_deg,
telem->position().latitude_deg,
telem->position().longitude_deg,
root[“x”].asDouble(), root[“y”].asDouble());
coords[“lat”] = std::get<0>(lat_lon);
coords[“lon”] = std::get<1>(lat_lon);
// Calculate altitude
coords[“alt”] = telem->position().absolute_altitude_m + root[“z”].asFloat();
coords[“height”] = telem->position().relative_altitude_m + root[“z”].asFloat();
if (!can_move(“move”, coords))
return;
if (move_as_goto)
{
coords[“yaw”] = root[“yaw”];
coords[“speed”] = root[“speed”];
// Send command
go_to(coords, true);
}
else
{
// Calculate new position
std::tuple<double, double, double> new_pos =
calcTranslatedPosition(telem->attitude_euler().yaw_deg,
telem->position_velocity_ned().position.north_m,
telem->position_velocity_ned().position.east_m,
root[“y”].asDouble(), root[“x”].asDouble());
coords[“north”] = std::get<0>(new_pos);
coords[“east”] = std::get<1>(new_pos);
// Calculate altitude
coords[“down”] = telem->position_velocity_ned().position.down_m - root[“z”].asFloat();
// If no yaw angle is provided or it is null, face the next waypoint
if (!root.isMember(“yaw”) || root[“yaw”] == Json::nullValue)
coords[“yaw”] = std::get<2>(new_pos);
// Monitor distance and adjust velocity dynamically
float acceleration = 1.0f; // Aceleração constante (m/s²)
float min_speed = 0.1f; // Velocidade mínima (m/s)
float max_speed = root.isMember(“speed”) ? root[“speed”].asFloat() : this->max_speed();
while (true)
{
// Get current position
float current_north = telem->position_velocity_ned().position.north_m;
float current_east = telem->position_velocity_ned().position.east_m;
float current_down = telem->position_velocity_ned().position.down_m;
// Calculate distance to target
float distance = std::sqrt(
std::pow(coords[“north”].asFloat() - current_north, 2) +
std::pow(coords[“east”].asFloat() - current_east, 2) +
std::pow(coords[“down”].asFloat() - current_down, 2));
// Stop the drone when close enough
if (distance <= 0.2f)
{
Json::Value stop_command;
stop_command[“north”] = 0.0f;
stop_command[“east”] = 0.0f;
stop_command[“down”] = 0.0f;
stop_command[“yaw”] = coords[“yaw”];
velocity_ned(stop_command, true);
break;
// keep sending zero velocity for 10 seconds
for (int i = 0; i < 100; i++) {
velocity_ned(stop_command, true);
usleep(100000); // 100 ms
}
}
// Calculate velocity using cinematics: v = sqrt(2 \* a \* d)
float speed = std::sqrt(2 * acceleration * distance);
// Clamp speed between min_speed and max_speed
speed = std::max(min_speed, std::min(speed, max_speed));
// Calculate velocity vector
Json::Value velocity_command;
velocity_command[“north”] = speed * (coords[“north”].asFloat() - current_north) / distance;
velocity_command[“east”] = speed * (coords[“east”].asFloat() - current_east) / distance;
velocity_command[“down”] = speed * (coords[“down”].asFloat() - current_down) / distance;
velocity_command[“yaw”] = coords[“yaw”];
// Send velocity command
velocity_ned(velocity_command, true);
// Dynamically adjust delay based on distance (clamped between 10ms and 100ms)
float delay = std::max(0.01f, std::min(0.1f, distance / 10.0f)); // Delay between 10ms and 100ms
usleep(static_cast<useconds_t>(delay * 1e6)); // Convert seconds to microseconds
}
}
stop_offb(true);
tracker->track_move(root[“y”].asDouble(), root[“x”].asDouble(), root[“z”].asDouble(), coords, move_as_goto);
}
I think this is explain why your first move command working but just after arriving the desired location offboard failsafe action triggered.