C++ (Cpp) guided_posvel_control_start Beispiele

Beispiel #1

Datei: control_guided.cpp Projekt: xrj3000/ardupilot

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

Beispiel #2

Datei: control_guided.cpp Projekt: 10man/ardupilot

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