Is the any good idea to achieve accurate time contorl with offboard api, like goTo(x,y,z,time), or other operation like takeoff(height, time), land(height,time). now i use the velocity, and i use the distance divide the time. but i do need to check the goal is touched, and this alway have error. I am inspired by the crazyflie with high level contorl. can i move it to the controller ?
if (command.getCommandId() == CommandId::Takeoff) {
const TakeOffCommand& takeOffCmd = static_cast<const TakeOffCommand&>(command);
if (takeOffCmd.params.height <= 0 || takeOffCmd.params.duration <= 0) {
m_logger.warning("Ignore command : Invalid takeoff parameters: height and duration must be positive.");
return;
}
auto takeoff_speed = takeOffCmd.params.height / takeOffCmd.params.duration;
auto get_speed_result = action.get_maximum_speed();
if (get_speed_result.first == Action::Result::Success)
{
action.set_maximum_speed(std::min(takeoff_speed, get_speed_result.second));
}
action.set_takeoff_altitude(takeOffCmd.params.height);
const Action::Result takeoff_result = action.takeoff();
if (takeoff_result != Action::Result::Success) {
std::stringstream sstr;
sstr << "Takeoff failed: " << takeoff_result;
m_logger.error(sstr.str());
return;
}
{
// check position reached
auto in_air_promise = std::promise<void>{};
auto in_air_future = in_air_promise.get_future();
Telemetry::PositionVelocityNedHandle handle = telemetry.subscribe_position_velocity_ned(
[&telemetry, &in_air_promise, &handle, takeOffCmd](Telemetry::PositionVelocityNed position_velocity_ned) {
if (fabs(position_velocity_ned.position.down_m + takeOffCmd.params.height) < 0.05)
{
std::cout << "Taking off has finished\n.";
telemetry.unsubscribe_position_velocity_ned(handle);
in_air_promise.set_value();
}
});
in_air_future.wait_for(std::chrono::duration<float>(DeltaSeconds));
if (in_air_future.wait_for(std::chrono::duration<float>(takeOffCmd.params.duration)) == std::future_status::timeout) {
std::cerr << "Takeoff timed out.\n";
return;
}
}
// or just sleep
//sleep_for(std::chrono::duration<float>(takeOffCmd.params.duration));