// set guided mode posvel target bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) { // check we are in velocity control mode if (guided_mode != Guided_PosVel) { guided_posvel_control_start(); } #if AC_FENCE == ENABLED // reject destination if outside the fence Location_Class dest_loc(destination); if (!fence.check_destination_within_fence(dest_loc)) { Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } #endif posvel_update_time_ms = AP_HAL::millis(); posvel_pos_target_cm = destination; posvel_vel_target_cms = velocity; pos_control.set_pos_target(posvel_pos_target_cm); // log target Log_Write_GuidedTarget(guided_mode, destination, velocity); return true; }
// set guided mode posvel target void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) { // check we are in velocity control mode if (guided_mode != Guided_PosVel) { guided_posvel_control_start(); } posvel_update_time_ms = millis(); posvel_pos_target_cm = destination; posvel_vel_target_cms = velocity; pos_control.set_pos_target(posvel_pos_target_cm); // log target Log_Write_GuidedTarget(guided_mode, destination, velocity); }