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