PX4 Position_Control RC_Remoter引入

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;
}

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

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.