PX4飞控位置环控制中如何引入遥控器的控制量, 本文基于PX4 1.12版本, 相比于之前的版本, 多引入了Flight Task这么一个模块.

省流总结

1. 遥控器的值通过_flight_tasks.update() —>_velocity_setpoint 通过 _flight_tasks.getPositionSetpoint() —> setpoint 通过_control.updateSetpoint(setpoint) —> vel_sp 通过 PositionControl::_positionController() —> 前馈项
2. 发布话题 trajectory_setpoint 为其中的setpoint数值
   发布话题vehicle_local_position_setpoint为P-PID计算后的的setpoint速度数值

主要的程序位于mc_pos_control_main.cpp中, 毕竟这个是位置环的主体部分. 每次循环下面的Run()函数

void
MulticopterPositionControl::Run()
{
    if (should_exit()) {
        _local_pos_sub.unregisterCallback();
        exit_and_cleanup();
        return;
    }

    perf_begin(_cycle_perf);

    if (_local_pos_sub.update(&_local_pos)) {

        poll_subscriptions();
        parameters_update(false);

        // set _dt in controllib Block - the time difference since the last loop iteration in seconds
        const hrt_abstime time_stamp_current = hrt_absolute_time();
        setDt((time_stamp_current - _time_stamp_last_loop) / 1e6f);
        _time_stamp_last_loop = time_stamp_current;

        const bool was_in_failsafe = _in_failsafe;
        _in_failsafe = false;

        // activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
        // that turns the nose of the vehicle into the wind
        if (_wv_controller != nullptr) {

            // in manual mode we just want to use weathervane if position is controlled as well
            // in mission, enabling wv is done in flight task
            if (_control_mode.flag_control_manual_enabled) {
                if (_control_mode.flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
                    _wv_controller->activate();

                } else {
                    _wv_controller->deactivate();
                }
            }

            _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
        }

        // an update is necessary here because otherwise the takeoff state doesn’t get skiped with non-altitude-controlled modes
        _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f,
                        !_control_mode.flag_control_climb_rate_enabled, time_stamp_current);

        // takeoff delay for motors to reach idle speed
        if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) {
            // when vehicle is ready switch to the required flighttask
            start_flight_task();

        } else {
            // stop flighttask while disarmed
            _flight_tasks.switchTask(FlightTaskIndex::None);
        }

        // check if any task is active
        if (_flight_tasks.isAnyTaskActive()) {
            // setpoints and constraints for the position controller from flighttask or failsafe
            vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
            vehicle_constraints_s constraints = FlightTask::empty_constraints;

            _flight_tasks.setYawHandler(_wv_controller);

            // update task
            if (!_flight_tasks.update()) {
                // FAILSAFE
                // Task was not able to update correctly. Do Failsafe.
                failsafe(setpoint, _states, false, !was_in_failsafe);

            } else {
                setpoint = _flight_tasks.getPositionSetpoint();
                constraints = _flight_tasks.getConstraints();

                _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_current);

                // Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid
                if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) &&
                    !(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) &&
                    !(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) {
                    failsafe(setpoint, _states, true, !was_in_failsafe);
                }

                // Check if altitude, climbrate or thrust in D-direction are valid -> trigger failsafe if none
                // of these setpoints are valid
                if (!PX4_ISFINITE(setpoint.z) && !PX4_ISFINITE(setpoint.vz) && !PX4_ISFINITE(setpoint.thrust[2])) {
                    failsafe(setpoint, _states, true, !was_in_failsafe);
                }
            }

            // publish trajectory setpoint
            _traj_sp_pub.publish(setpoint);

            landing_gear_s gear = _flight_tasks.getGear();

            // check if all local states are valid and map accordingly
            set_vehicle_states(setpoint.vz);

            // handle smooth takeoff
            _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff,
                            constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_current);
            constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up);

            if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
                // we are not flying yet and need to avoid any corrections
                reset_setpoint_to_nan(setpoint);
                setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
                // set yaw-sp to current yaw
                // TODO: we need a clean way to disable yaw control
                setpoint.yaw = _states.yaw;
                setpoint.yawspeed = 0.f;
                // prevent any integrator windup
                _control.resetIntegralXY();
                _control.resetIntegralZ();
                // reactivate the task which will reset the setpoint to current state
                _flight_tasks.reActivate();
            }

            if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
                constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
            }

            // limit altitude only if local position is valid
            if (PX4_ISFINITE(_states.position(2))) {
                limit_altitude(setpoint);
            }

            // Update states, setpoints and constraints.
            _control.updateConstraints(constraints);
            _control.updateState(_states);

            // update position controller setpoints
            if (!_control.updateSetpoint(setpoint)) {
                warn_rate_limited(“Position-Control Setpoint-Update failed”);
                failsafe(setpoint, _states, true, !was_in_failsafe);
                _control.updateSetpoint(setpoint);
                constraints = FlightTask::empty_constraints;
            }

            // Generate desired thrust and yaw.
            _control.generateThrustYawSetpoint(_dt);

            // Fill local position, velocity and thrust setpoint.
            // This message contains setpoints where each type of setpoint is either the input to the PositionController
            // or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID).
            // Example:
            // If the desired setpoint is position-setpoint, _local_pos_sp will contain
            // position-, velocity- and thrust-setpoint where the velocity- and thrust-setpoint were generated by the PositionControlller.
            // If the desired setpoint has a velocity-setpoint only, then _local_pos_sp will contain valid velocity- and thrust-setpoint, but the position-setpoint
            // will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the
            // PositionController.
            vehicle_local_position_setpoint_s local_pos_sp{};
            local_pos_sp.timestamp = hrt_absolute_time();
            local_pos_sp.x = setpoint.x;
            local_pos_sp.y = setpoint.y;
            local_pos_sp.z = setpoint.z;
            local_pos_sp.yaw = _control.getYawSetpoint();
            local_pos_sp.yawspeed = _control.getYawspeedSetpoint();
            local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx;
            local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy;
            local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
            _control.getThrustSetpoint().copyTo(local_pos_sp.thrust);

            // Publish local position setpoint
            // This message will be used by other modules (such as Landdetector) to determine
            // vehicle intention.
            _local_pos_sp_pub.publish(local_pos_sp);

            // Inform FlightTask about the input and output of the velocity controller
            // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock)
            _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), Vector3f(local_pos_sp.thrust));

            // Part of landing logic: if ground-contact/maybe landed was detected, turn off
            // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.
            // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction.
            if (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
                limit_thrust_during_landing(local_pos_sp);
            }

            // Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
            _att_sp = ControlMath::thrustToAttitude(matrix::Vector3f(local_pos_sp.thrust), local_pos_sp.yaw);
            _att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint();
            _att_sp.fw_control_yaw = false;
            _att_sp.apply_flaps = false;

            // publish attitude setpoint
            // Note: this requires review. The reason for not sending
            // an attitude setpoint is because for non-flighttask modes
            // the attitude septoint should come from another source, otherwise
            // they might conflict with each other such as in offboard attitude control.
            publish_attitude();

            // if there’s any change in landing gear setpoint publish it
            if (gear.landing_gear != _old_landing_gear_position
                && gear.landing_gear != landing_gear_s::GEAR_KEEP) {

                _landing_gear.landing_gear = gear.landing_gear;
                _landing_gear.timestamp = hrt_absolute_time();

                _landing_gear_pub.publish(_landing_gear);
            }

            _old_landing_gear_position = gear.landing_gear;

        } else {
            // no flighttask is active: set attitude setpoint to idle
            _att_sp.roll_body = _att_sp.pitch_body = 0.0f;
            _att_sp.yaw_body = _states.yaw;
            _att_sp.yaw_sp_move_rate = 0.0f;
            _att_sp.fw_control_yaw = false;
            _att_sp.apply_flaps = false;
            matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
            q_sp.copyTo(_att_sp.q_d);
            _att_sp.q_d_valid = true;
            _att_sp.thrust_body[2] = 0.0f;

            // reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation
            _vel_x_deriv.reset();
            _vel_y_deriv.reset();
            _vel_z_deriv.reset();
        }
    }

    perf_end(_cycle_perf);
}

在Position模式中, 程序会在其中的 _flight_tasks.update() 跳转到 对应的子模式 该部分是新增的
程序跳转到FlightTask.cpp中的 bool FlightTasks::update()

bool FlightTasks::update()
{
    _updateCommand();

    if (isAnyTaskActive()) {
        return _current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize();
    }

    return false;
}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10

然后会进入到子模式中的更新函数, 比如下面的

bool FlightTaskManualAltitude::updateInitialize()
{
    bool ret = FlightTaskManual::updateInitialize();

    _sub_home_position.update();

    // in addition to manual require valid position and velocity in D-direction and valid yaw
    return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
}

继续跳转就可以得到

bool FlightTaskManual::updateInitialize()
{
    bool ret = FlightTask::updateInitialize();

    _sub_manual_control_setpoint.update();

    const bool sticks_available = _evaluateSticks();

    if (_sticks_data_required) {
        ret = ret && sticks_available;
    }

    return ret;
}

以及下面的函数.

bool FlightTaskManual::_evaluateSticks()
{
    hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() _ 1.5f) _ 1_s;

    /_ Sticks are rescaled linearly and exponentially to [-1,1] _/
    if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {

        /_ Linear scale  _/
        _sticks(0) = _sub_manual_control_setpoint.get().x; /_ NED x, “pitch” [-1,1] _/
        _sticks(1) = _sub_manual_control_setpoint.get().y; /_ NED y, “roll” [-1,1] _/
        _sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) _ 2.f; /_ NED z, “thrust” resacaled from [0,1] to [-1,1] _/
        _sticks(3) = _sub_manual_control_setpoint.get().r; /_ “yaw” [-1,1] _/

        /_ Exponential scale _/
        _sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
        _sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
        _sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
        _sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());

        // Only switch the landing gear up if the user switched from gear down to gear up.
        // If the user had the switch in the gear up position and took off ignore it
        // until he toggles the switch to avoid retracting the gear immediately on takeoff.
        int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;

        if (_gear_switch_old != gear_switch) {
            _applyGearSwitch(gear_switch);
        }

        _gear_switch_old = gear_switch;

        // valid stick inputs are required
        const bool valid_sticks =  PX4_ISFINITE(_sticks(0))
                       && PX4_ISFINITE(_sticks(1))
                       && PX4_ISFINITE(_sticks(2))
                       && PX4_ISFINITE(_sticks(3));

        return valid_sticks;

    } else {
        /_ Timeout: set all sticks to zero _/
        _sticks.zero();
        _sticks_expo.zero();
        _gear.landing_gear = landing_gear_s::GEAR_KEEP;
        return false;
    }
}

其中上面的函数中可以看出,是对遥控器数据进行读入. 而对应的下面的update函数中含有一个_scaleSticks()则完成数据的转移

bool FlightTaskManualAltitude::update()
{
    _scaleSticks();
    _updateSetpoints();
    _constraints.want_takeoff = _checkTakeoff();

    return true;
}

将上面的_scaleSticks()展开得到

void FlightTaskManualPosition::_scaleSticks()
{
    /_ Use same scaling as for FlightTaskManualAltitude _/
    FlightTaskManualAltitude::_scaleSticks();

    /_ Constrain length of stick inputs to 1 for xy_/
    Vector2f stick_xy(&_sticks_expo(0));

    float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);

    if (mag > FLT_EPSILON) {
        stick_xy = stick_xy.normalized() _ mag;
    }

    // scale the stick inputs
    if (PX4_ISFINITE(_sub_vehicle_local_position.get().vxy_max)) {
        // estimator provides vehicle specific max

        // use the minimum of the estimator and user specified limit
        _velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position.get().vxy_max);
        // Allow for a minimum of 0.3 m/s for repositioning
        _velocity_scale = fmaxf(_velocity_scale, 0.3f);

    } else if (stick_xy.length() > 0.5f) {
        // raise the limit at a constant rate up to the user specified value

        if (_velocity_scale < _constraints.speed_xy) {
            _velocity_scale += _deltatime _ _param_mpc_acc_hor_estm.get();

        } else {
            _velocity_scale = _constraints.speed_xy;

        }
    }

    // scale velocity to its maximum limits
    Vector2f vel_sp_xy = stick_xy _ _velocity_scale;

    /_ Rotate setpoint into local frame. _/
    _rotateIntoHeadingFrame(vel_sp_xy);

    // collision prevention
    if (_collision_prevention.is_active()) {
        _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, Vector2f(_position),
                             Vector2f(_velocity));
    }

    _velocity_setpoint(0) = vel_sp_xy(0);
    _velocity_setpoint(1) = vel_sp_xy(1);
}

至此我们稍微做个总结, 相当于遥控器的值在 FlightTaks::update()中完成读入, 并转化成了_velocity_setpoint()

紧接着 后面有一个函数setpoint = _flight_tasks.getPositionSetpoint(); 我们将其展开得到

const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
{
    /_ fill position setpoint message _/
    vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
    vehicle_local_position_setpoint.timestamp = hrt_absolute_time();

    vehicle_local_position_setpoint.x = _position_setpoint(0);
    vehicle_local_position_setpoint.y = _position_setpoint(1);
    vehicle_local_position_setpoint.z = _position_setpoint(2);

    vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
    vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
    vehicle_local_position_setpoint.vz = _velocity_setpoint(2);

    vehicle_local_position_setpoint.acc_x = _acceleration_setpoint(0);
    vehicle_local_position_setpoint.acc_y = _acceleration_setpoint(1);
    vehicle_local_position_setpoint.acc_z = _acceleration_setpoint(2);

    vehicle_local_position_setpoint.jerk_x = _jerk_setpoint(0);
    vehicle_local_position_setpoint.jerk_y = _jerk_setpoint(1);
    vehicle_local_position_setpoint.jerk_z = _jerk_setpoint(2);

    _thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
    vehicle_local_position_setpoint.yaw = _yaw_setpoint;
    vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;

    return vehicle_local_position_setpoint;
}

可见随后_velocity_setpoint() 进入到setpoint变量中

——————————-! 分割线 !——————————-

随后通过_traj_sp_pub.publish(setpoint);将setpoint以话题的形式发布出去; 话题为trajectory_setpoint

随后 我们注意到下面有一个_control.updateSetpoint(setpoint)函数, 我们将其展开. 来看一下是如何进入到控制中的

bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{
    // by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose)
    _setCtrlFlag(true);

    _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
    _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
    _acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z);
    _thr_sp = Vector3f(setpoint.thrust);
    _yaw_sp = setpoint.yaw;
    _yawspeed_sp = setpoint.yawspeed;
    bool mapping_succeeded = _interfaceMapping();

    // If full manual is required (thrust already generated), don’t run position/velocity
    // controller and just return thrust.
    _skip_controller = PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))
               && PX4_ISFINITE(_thr_sp(2));

    return mapping_succeeded;
}

此处又将setpoint赋值给 _vel_sp.

位置环P-PID形式在下面函数中完成
_control.generateThrustYawSetpoint(_dt);

void PositionControl::generateThrustYawSetpoint(const float dt)
{
    if (_skip_controller) {

        // Already received a valid thrust set-point.
        // Limit the thrust vector.
        float thr_mag = _thr_sp.length();

        if (thr_mag > _param_mpc_thr_max.get()) {
            _thr_sp = _thr_sp.normalized() _ _param_mpc_thr_max.get();

        } else if (thr_mag < _param_mpc_manthr_min.get() && thr_mag > FLT_EPSILON) {
            _thr_sp = _thr_sp.normalized() _ _param_mpc_manthr_min.get();
        }

        // Just set the set-points equal to the current vehicle state.
        _pos_sp = _pos;
        _vel_sp = _vel;
        _acc_sp = _acc;

    } else {
        _positionController();
        _velocityController(dt);
    }
}

我们将_positionController()展开

void PositionControl::_positionController()
{
    // P-position controller
    const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(),
                     _param_mpc_z_p.get()));
    _vel_sp = vel_sp_position + _vel_sp;

    // Constrain horizontal velocity by prioritizing the velocity component along the
    // the desired position setpoint over the feed-forward term.
    const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
                   Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get());
    _vel_sp(0) = vel_sp_xy(0);
    _vel_sp(1) = vel_sp_xy(1);
    // Constrain velocity in z-direction.
    _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

可以看到_vel_sp 作为前馈项进入到 速度设定值中

然后通过速度控制PID算出来Thrust 以及 Yaw的方向设定值
发布话题vehicle_local_position_setpoint. 其中速度为P-PID计算后的的setpoint速度数值_vel_sp变量

最后再将三维的Thrust转换为给定的Attitude.