mc_pos_control.cpp 之 control_auto(dt)

• control_auto(dt)

void 
MulticopterPositionControl::control_auto(float dt)
{
    /* reset position setpoint on AUTO mode activation or if we are not in MC mode
     * 如果AUTO模式激活或者我们不在MC模式下，重置位置设定值 */
    if (!_mode_auto || !_vehicle_status.is_rotary_wing) {
        if (!_mode_auto) {
            _mode_auto = true;
        }

        _reset_pos_sp = true;
        _reset_alt_sp = true;
    }

    /* Always check reset state of altitude and position control flags in auto
     * 在AUTO模式下，一直检高度和位置控制的查复位状态 */
    reset_pos_sp();
    reset_alt_sp();

    bool current_setpoint_valid = false;
    bool previous_setpoint_valid = false;

    math::Vector<3> prev_sp;
    math::Vector<3> curr_sp;

    if (_pos_sp_triplet.current.valid) {

        /* project setpoint to local frame
         * 将设定值转化到机体坐标系 */
        map_projection_project(&_ref_pos,
                       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
                       &curr_sp.data[0], &curr_sp.data[1]);
        curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);

        if (PX4_ISFINITE(curr_sp(0)) &&
            PX4_ISFINITE(curr_sp(1)) &&
            PX4_ISFINITE(curr_sp(2))) {
            current_setpoint_valid = true;
        }
    }

    if (_pos_sp_triplet.previous.valid) {
        map_projection_project(&_ref_pos,
                       _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
                       &prev_sp.data[0], &prev_sp.data[1]);
        prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);

        if (PX4_ISFINITE(prev_sp(0)) &&
            PX4_ISFINITE(prev_sp(1)) &&
            PX4_ISFINITE(prev_sp(2))) {
            previous_setpoint_valid = true;
        }
    }

    if (current_setpoint_valid &&
        (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) {

        /* scaled space: 1 == position error resulting max allowed speed
         * 尺度空间：1==位置错误导致的最大的允许速度 */

        math::Vector<3> cruising_speed = _params.vel_cruise;

        if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
            _pos_sp_triplet.current.cruising_speed > 0.1f) {
            cruising_speed(0) = _pos_sp_triplet.current.cruising_speed;
            cruising_speed(1) = _pos_sp_triplet.current.cruising_speed;
        }

        math::Vector<3> scale = _params.pos_p.edivide(cruising_speed);

        /* convert current setpoint to scaled space */
        math::Vector<3> curr_sp_s = curr_sp.emult(scale);

        /* by default use current setpoint as is */
        math::Vector<3> pos_sp_s = curr_sp_s;

        if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION  ||
             _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) &&
            previous_setpoint_valid) {

            /* follow "previous - current" line */

            if ((curr_sp - prev_sp).length() > MIN_DIST) {

                /* find X - cross point of unit sphere and trajectory */
                math::Vector<3> pos_s = _pos.emult(scale);
                math::Vector<3> prev_sp_s = prev_sp.emult(scale);
                math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
                math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
                float curr_pos_s_len = curr_pos_s.length();

                if (curr_pos_s_len < 1.0f) {
                    /* copter is closer to waypoint than unit radius */
                    /* check next waypoint and use it to avoid slowing down when passing via waypoint */
                    if (_pos_sp_triplet.next.valid) {
                        math::Vector<3> next_sp;
                        map_projection_project(&_ref_pos,
                                       _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
                                       &next_sp.data[0], &next_sp.data[1]);
                        next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);

                        if ((next_sp - curr_sp).length() > MIN_DIST) {
                            math::Vector<3> next_sp_s = next_sp.emult(scale);

                            /* calculate angle prev - curr - next */
                            math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
                            math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();

                            /* cos(a) * curr_next, a = angle between current and next trajectory segments */
                            float cos_a_curr_next = prev_curr_s_norm * curr_next_s;

                            /* cos(b), b = angle pos - curr_sp - prev_sp */
                            float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;

                            if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
                                float curr_next_s_len = curr_next_s.length();

                                /* if curr - next distance is larger than unit radius, limit it */
                                if (curr_next_s_len > 1.0f) {
                                    cos_a_curr_next /= curr_next_s_len;
                                }

                                /* feed forward position setpoint offset */
                                math::Vector<3> pos_ff = prev_curr_s_norm *
                                             cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
                                             (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
                                pos_sp_s += pos_ff;
                            }
                        }
                    }

                } else {
                    bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);

                    if (!near) {
                        /* we're far away from trajectory, pos_sp_s is set to the nearest point on the trajectory */
                        pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
                    }
                }
            }
        }

        /* move setpoint not faster than max allowed speed */
        math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);

        /* difference between current and desired position setpoints, 1 = max speed */
        math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
        float d_pos_m_len = d_pos_m.length();

        if (d_pos_m_len > dt) {
            pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
        }

        /* scale result back to normal space */
        _pos_sp = pos_sp_s.edivide(scale);

        /* update yaw setpoint if needed */

        if (_pos_sp_triplet.current.yawspeed_valid
            && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
            _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;

        } else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
            _att_sp.yaw_body = _pos_sp_triplet.current.yaw;
        }

        /*
         * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
         * this makes the takeoff finish smoothly.
         */
        if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
             || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)
            && _pos_sp_triplet.current.acceptance_radius > 0.0f
            /* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */
            && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) {
            _do_reset_alt_pos_flag = false;

            /* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */

        } else {
            _do_reset_alt_pos_flag = true;
        }

        // During a mission or in loiter it's safe to retract the landing gear.
        if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
             _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) &&
            !_vehicle_land_detected.landed) {
            _att_sp.landing_gear = 1.0f;

            // During takeoff and landing, we better put it down again.

        } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF ||
               _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
            _att_sp.landing_gear = -1.0f;

        } else {
            // For the rest of the setpoint types, just leave it as is.
        }

    } else {
        /* no waypoint, do nothing, setpoint was already reset */
    }
}