Exemple #1
0
// do_user_takeoff_start - initialises waypoint controller to implement take-off
bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home)
{
    guided_mode = Guided_TakeOff;

    // initialise wpnav destination
    Location target_loc = copter.current_loc;
    target_loc.set_alt_cm(final_alt_above_home, Location::ALT_FRAME_ABOVE_HOME);

    if (!wp_nav->set_wp_destination(target_loc)) {
        // failure to set destination can only be because of missing terrain data
        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
        // failure is propagated to GCS with NAK
        return false;
    }

    // initialise yaw
    auto_yaw.set_mode(AUTO_YAW_HOLD);

    // clear i term when we're taking off
    set_throttle_takeoff();

    // get initial alt for WP_NAVALT_MIN
    auto_takeoff_set_start_alt();

    return true;
}
Exemple #2
0
// auto_takeoff_start - initialises waypoint controller to implement take-off
void Copter::auto_takeoff_start(const Location& dest_loc)
{
    auto_mode = Auto_TakeOff;

    // convert location to class
    Location_Class dest(dest_loc);

    // set horizontal target
    dest.lat = current_loc.lat;
    dest.lng = current_loc.lng;

    // get altitude target
    int32_t alt_target;
    if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) {
        // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
        Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
        // fall back to altitude above current altitude
        alt_target = current_loc.alt + dest.alt;
    }

    // sanity check target
    if (alt_target < current_loc.alt) {
        dest.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
    }
    // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude
    if (alt_target < 100) {
        dest.set_alt_cm(100, Location_Class::ALT_FRAME_ABOVE_HOME);
    }

    // set waypoint controller target
    if (!wp_nav.set_wp_destination(dest)) {
        // failure to set destination can only be because of missing terrain data
        failsafe_terrain_on_event();
        return;
    }

    // initialise yaw
    set_auto_yaw_mode(AUTO_YAW_HOLD);

    // clear i term when we're taking off
    set_throttle_takeoff();

    // get initial alt for TKOFF_NAV_ALT
    auto_takeoff_set_start_alt();
}