Ejemplo n.º 1
0
// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
    // convert back to location
    Location_Class target_loc(cmd.content.location);

    // use current location if not provided
    if (target_loc.lat == 0 && target_loc.lng == 0) {
        // To-Do: make this simpler
        Vector3f temp_pos;
        wp_nav.get_wp_stopping_point_xy(temp_pos);
        Location_Class temp_loc(temp_pos);
        target_loc.lat = temp_loc.lat;
        target_loc.lng = temp_loc.lng;
    }

    // use current altitude if not provided
    // To-Do: use z-axis stopping point instead of current alt
    if (target_loc.alt == 0) {
        // set to current altitude but in command's alt frame
        int32_t curr_alt;
        if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
            target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
        } else {
            // default to current altitude as alt-above-home
            target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
        }
    }

    // start way point navigator and provide it the desired location
    auto_wp_start(target_loc);
}
Ejemplo n.º 2
0
// do_loiter_time - initiate loitering at a point for a given time period
// note: caller should set yaw_mode
void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
    Vector3f target_pos;

    // get current position
    Vector3f curr_pos = inertial_nav.get_position();

    // default to use position provided
    target_pos = pv_location_to_vector(cmd.content.location);

    // use current location if not provided
    if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
        wp_nav.get_wp_stopping_point_xy(target_pos);
    }

    // use current altitude if not provided
    if( cmd.content.location.alt == 0 ) {
        target_pos.z = curr_pos.z;
    }

    // start way point navigator and provide it the desired location
    auto_wp_start(target_pos);

    // setup loiter timer
    loiter_time     = 0;
    loiter_time_max = cmd.p1;     // units are (seconds)
}
Ejemplo n.º 3
0
// do_nav_wp - initiate move to next waypoint
void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
    Location_Class target_loc(cmd.content.location);
    // use current lat, lon if zero
    if (target_loc.lat == 0 && target_loc.lng == 0) {
        target_loc.lat = current_loc.lat;
        target_loc.lng = current_loc.lng;
    }
    // use current altitude if not provided
    if (target_loc.alt == 0) {
        // set to current altitude but in command's alt frame
        int32_t curr_alt;
        if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
            target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
        } else {
            // default to current altitude as alt-above-home
            target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
        }
    }
    
    // this will be used to remember the time in millis after we reach or pass the WP.
    loiter_time = 0;
    // this is the delay, stored in seconds
    loiter_time_max = cmd.p1;

    // Set wp navigation target
    auto_wp_start(target_loc);

    // if no delay set the waypoint as "fast"
    if (loiter_time_max == 0 ) {
        wp_nav.set_fast_waypoint(true);
    }
}
Ejemplo n.º 4
0
// do_land - initiate landing procedure
void Copter::do_land(const AP_Mission::Mission_Command& cmd)
{
    // To-Do: check if we have already landed

    // if location provided we fly to that location at current altitude
    if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
        // set state to fly to location
        land_state = LAND_STATE_FLY_TO_LOCATION;

        // convert to location class
        Location_Class target_loc(cmd.content.location);

        // decide if we will use terrain following
        int32_t curr_terr_alt_cm, target_terr_alt_cm;
        if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
            target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
            curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
            // if using terrain, set target altitude to current altitude above terrain
            target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
        } else {
            // set target altitude to current altitude above home
            target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
        }
        auto_wp_start(target_loc);
    }else{
        // set landing state
        land_state = LAND_STATE_DESCENDING;

        // initialise landing controller
        auto_land_start();
    }
}
Ejemplo n.º 5
0
// verify_surface - returns true if surface procedure has been completed
bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
{
    bool retval = false;

    switch (auto_surface_state) {
        case AUTO_SURFACE_STATE_GO_TO_LOCATION:
            // check if we've reached the location
            if (wp_nav.reached_wp_destination()) {
                // Set target to current xy and zero depth
                // TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination
                Location_Class target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);

                auto_wp_start(target_location);

                // advance to next state
                auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
            }
            break;

        case AUTO_SURFACE_STATE_ASCEND:
            if (wp_nav.reached_wp_destination()) {
                retval = true;
            }
            break;

        default:
            // this should never happen
            // TO-DO: log an error
            retval = true;
            break;
    }

    // true is returned if we've successfully surfaced
    return retval;
}
Ejemplo n.º 6
0
// do_surface - initiate surface procedure
void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
{
    Location_Class target_location;

    // if location provided we fly to that location at current altitude
    if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
        // set state to go to location
        auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION;

        // calculate and set desired location below surface target
        // convert to location class
        target_location = Location_Class(cmd.content.location);

        // decide if we will use terrain following
        int32_t curr_terr_alt_cm, target_terr_alt_cm;
        if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
                target_location.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
            // if using terrain, set target altitude to current altitude above terrain
            target_location.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
        } else {
            // set target altitude to current altitude above home
            target_location.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
        }
    } else {
        // set surface state to ascend
        auto_surface_state = AUTO_SURFACE_STATE_ASCEND;

        // Set waypoint destination to current location at zero depth
        target_location = Location_Class(current_loc.lat, current_loc.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);
    }

    // Go to wp location
    auto_wp_start(target_location);
}
Ejemplo n.º 7
0
// do_nav_wp - initiate move to next waypoint
void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
    // this will be used to remember the time in millis after we reach or pass the WP.
    loiter_time = 0;
    // this is the delay, stored in seconds
    loiter_time_max = cmd.p1;

    // Set wp navigation target
    auto_wp_start(Location_Class(cmd.content.location));

    // if no delay set the waypoint as "fast"
    if (loiter_time_max == 0 ) {
        wp_nav.set_fast_waypoint(true);
    }
}
Ejemplo n.º 8
0
// do_nav_wp - initiate move to next waypoint
void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
    const Vector3f &curr_pos = inertial_nav.get_position();
    const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);

    // this will be used to remember the time in millis after we reach or pass the WP.
    loiter_time = 0;
    // this is the delay, stored in seconds
    loiter_time_max = abs(cmd.p1);

    // Set wp navigation target
    auto_wp_start(local_pos);
    // if no delay set the waypoint as "fast"
    if (loiter_time_max == 0 ) {
        wp_nav.set_fast_waypoint(true);
    }
}
Ejemplo n.º 9
0
// do_payload_place - initiate placing procedure
void Copter::do_payload_place(const AP_Mission::Mission_Command& cmd)
{
    // if location provided we fly to that location at current altitude
    if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
        // set state to fly to location
        nav_payload_place.state = PayloadPlaceStateType_FlyToLocation;

        Location_Class target_loc = terrain_adjusted_location(cmd);

        auto_wp_start(target_loc);
    } else {
        nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;

        // initialise placing controller
        auto_payload_place_start();
    }
    nav_payload_place.descend_max = cmd.p1;
}
Ejemplo n.º 10
0
// do_land - initiate landing procedure
void Copter::do_land(const AP_Mission::Mission_Command& cmd)
{
    // To-Do: check if we have already landed

    // if location provided we fly to that location at current altitude
    if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
        // set state to fly to location
        land_state = LandStateType_FlyToLocation;

        Location_Class target_loc = terrain_adjusted_location(cmd);

        auto_wp_start(target_loc);
    }else{
        // set landing state
        land_state = LandStateType_Descending;

        // initialise landing controller
        auto_land_start();
    }
}
Ejemplo n.º 11
0
// do_land - initiate landing procedure
void Copter::do_land(const AP_Mission::Mission_Command& cmd)
{
    // To-Do: check if we have already landed

    // if location provided we fly to that location at current altitude
    if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
        // set state to fly to location
        land_state = LAND_STATE_FLY_TO_LOCATION;

        // calculate and set desired location above landing target
        Vector3f pos = pv_location_to_vector(cmd.content.location);
        pos.z = inertial_nav.get_altitude();
        auto_wp_start(pos);
    }else{
        // set landing state
        land_state = LAND_STATE_DESCENDING;

        // initialise landing controller
        auto_land_start();
    }
}
Ejemplo n.º 12
0
// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
    Vector3f target_pos;

    // get current position
    Vector3f curr_pos = inertial_nav.get_position();

    // default to use position provided
    target_pos = pv_location_to_vector(cmd.content.location);

    // use current location if not provided
    if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
        wp_nav.get_wp_stopping_point_xy(target_pos);
    }

    // use current altitude if not provided
    // To-Do: use z-axis stopping point instead of current alt
    if( cmd.content.location.alt == 0 ) {
        target_pos.z = curr_pos.z;
    }

    // start way point navigator and provide it the desired location
    auto_wp_start(target_pos);
}
Ejemplo n.º 13
0
// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
    // convert back to location
    Location_Class target_loc(cmd.content.location);

    // use current location if not provided
    if (target_loc.lat == 0 && target_loc.lng == 0) {
        // To-Do: make this simpler
        Vector3f temp_pos;
        wp_nav.get_wp_stopping_point_xy(temp_pos);
        Location_Class temp_loc(temp_pos);
        target_loc.lat = temp_loc.lat;
        target_loc.lng = temp_loc.lng;
    }

    // In mavproxy misseditor: Abs = 0 = ALT_FRAME_ABSOLUTE
    //                         Rel = 1 = ALT_FRAME_ABOVE_HOME
    //                         AGL = 3 = ALT_FRAME_ABOVE_TERRAIN
    //    2 = ALT_FRAME_ABOVE_ORIGIN, not an option in mavproxy misseditor

    // use current altitude if not provided
    // To-Do: use z-axis stopping point instead of current alt
    if (target_loc.alt == 0) {
        // set to current altitude but in command's alt frame
        int32_t curr_alt;
        if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
            target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
        } else {
            // default to current altitude as alt-above-home
            target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
        }
    }

    // start way point navigator and provide it the desired location
    auto_wp_start(target_loc);
}
Ejemplo n.º 14
0
// verify_payload_place - returns true if placing has been completed
bool Copter::verify_payload_place()
{
    const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds
    const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds
    const float hover_throttle_placed_fraction = 0.8; // i.e. if throttle is less than 80% of hover we have placed
    const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed
    const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed

    const float current_throttle_level = motors.get_throttle();
    const uint32_t now =  AP_HAL::millis();

    // if we discover we've landed then immediately release the load:
    if (ap.land_complete) {
        switch (nav_payload_place.state) {
        case PayloadPlaceStateType_FlyToLocation:
        case PayloadPlaceStateType_Calibrating_Hover_Start:
        case PayloadPlaceStateType_Calibrating_Hover:
        case PayloadPlaceStateType_Descending_Start:
        case PayloadPlaceStateType_Descending:
            gcs_send_text_fmt(MAV_SEVERITY_INFO, "NAV_PLACE: landed");
            nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
            break;
        case PayloadPlaceStateType_Releasing_Start:
        case PayloadPlaceStateType_Releasing:
        case PayloadPlaceStateType_Released:
        case PayloadPlaceStateType_Ascending_Start:
        case PayloadPlaceStateType_Ascending:
        case PayloadPlaceStateType_Done:
            break;
        }
    }

    switch (nav_payload_place.state) {
    case PayloadPlaceStateType_FlyToLocation:
        if (!wp_nav.reached_wp_destination()) {
            return false;
        }
        // we're there; set loiter target
        auto_payload_place_start(wp_nav.get_wp_destination());
        nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
        // no break
    case PayloadPlaceStateType_Calibrating_Hover_Start:
        // hover for 1 second to get an idea of what our hover
        // throttle looks like
        debug("Calibrate start");
        nav_payload_place.hover_start_timestamp = now;
        nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover;
        // no break
    case PayloadPlaceStateType_Calibrating_Hover: {
        if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) {
            // still calibrating...
            debug("Calibrate Timer: %d", now - nav_payload_place.hover_start_timestamp);
            return false;
        }
        // we have a valid calibration.  Hopefully.
        nav_payload_place.hover_throttle_level = current_throttle_level;
        const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors.get_throttle_hover());
        gcs_send_text_fmt(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast<double>(hover_throttle_delta));
        nav_payload_place.state = PayloadPlaceStateType_Descending_Start;
        }
        // no break
    case PayloadPlaceStateType_Descending_Start:
        nav_payload_place.descend_start_timestamp = now;
        nav_payload_place.descend_start_altitude = inertial_nav.get_altitude();
        nav_payload_place.descend_throttle_level = 0;
        nav_payload_place.state = PayloadPlaceStateType_Descending;
        // no break
    case PayloadPlaceStateType_Descending:
        // make sure we don't descend too far:
        debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max);
        if (!is_zero(nav_payload_place.descend_max) &&
            nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()  > nav_payload_place.descend_max) {
            nav_payload_place.state = PayloadPlaceStateType_Ascending;
            gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Reached maximum descent");
            return false; // we'll do any cleanups required next time through the loop
        }
        // see if we've been descending long enough to calibrate a descend-throttle-level:
        if (is_zero(nav_payload_place.descend_throttle_level) &&
            now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) {
            nav_payload_place.descend_throttle_level = current_throttle_level;
        }
        // watch the throttle to determine whether the load has been placed
        // debug("hover ratio: %f   descend ratio: %f\n", current_throttle_level/nav_payload_place.hover_throttle_level, ((nav_payload_place.descend_throttle_level == 0) ? -1.0f : current_throttle_level/nav_payload_place.descend_throttle_level));
        if (current_throttle_level/nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction &&
            (is_zero(nav_payload_place.descend_throttle_level) ||
             current_throttle_level/nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) {
            // throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid)
            nav_payload_place.place_start_timestamp = 0;
            return false;
        }
        if (nav_payload_place.place_start_timestamp == 0) {
            // we've only just now hit the correct throttle level
            nav_payload_place.place_start_timestamp = now;
            return false;
        } else if (now - nav_payload_place.place_start_timestamp < placed_time) {
            // keep going down....
            debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp);
            return false;
        }
        nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
        // no break
    case PayloadPlaceStateType_Releasing_Start:
        if (g2.gripper.valid()) {
            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Releasing the gripper");
            g2.gripper.release();
        } else {
            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Gripper not valid");
        }
        nav_payload_place.state = PayloadPlaceStateType_Releasing;
        // no break
    case PayloadPlaceStateType_Releasing:
        if (g2.gripper.valid() && !g2.gripper.released()) {
            return false;
        }
        nav_payload_place.state = PayloadPlaceStateType_Released;
        // no break
    case PayloadPlaceStateType_Released: {
        nav_payload_place.state = PayloadPlaceStateType_Ascending_Start;
        }
        // no break
    case PayloadPlaceStateType_Ascending_Start: {
        Location_Class target_loc = inertial_nav.get_position();
        target_loc.alt = nav_payload_place.descend_start_altitude;
        auto_wp_start(target_loc);
        nav_payload_place.state = PayloadPlaceStateType_Ascending;
        }
        // no break
    case PayloadPlaceStateType_Ascending:
        if (!wp_nav.reached_wp_destination()) {
            return false;
        }
        nav_payload_place.state = PayloadPlaceStateType_Done;
        // no break
    case PayloadPlaceStateType_Done:
        return true;
    default:
        // this should never happen
        // TO-DO: log an error
        return true;
    }
    // should never get here
    return true;
}
Ejemplo n.º 15
0
void Sub::do_RTL()
{
    auto_wp_start(ahrs.get_home());
}