mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-14 04:43:33 +08:00
fix: format
This commit is contained in:
+1
-1
@@ -116,7 +116,7 @@ void ScPositionControl::_positionControl(const float dt)
|
||||
_pos_int(1) = math::constrain(_vel_int(1), -_pos_int_lim, _pos_int_lim);
|
||||
_pos_int(2) = math::constrain(_vel_int(2), -_pos_int_lim, _pos_int_lim);
|
||||
|
||||
// P-position controller
|
||||
// P-position controller
|
||||
ControlMath::setZeroIfNanVector3f(_pos_sp);
|
||||
Vector3f pos_error = _pos_sp - _pos;
|
||||
Vector3f vel_sp_position = pos_error.emult(_gain_pos_p) + _pos_int;
|
||||
|
||||
+11
-10
@@ -84,11 +84,11 @@ public:
|
||||
|
||||
/**
|
||||
* @brief Set the Position Integral Limits object
|
||||
*
|
||||
*
|
||||
* @param lim float limit to be set (on all axis)
|
||||
*/
|
||||
void setPositionIntegralLimits(const float lim);
|
||||
|
||||
|
||||
/**
|
||||
* Set the velocity control gains
|
||||
* @param P 3D vector of proportional gains for x,y,z axis
|
||||
@@ -105,7 +105,7 @@ public:
|
||||
|
||||
/**
|
||||
* @brief Set the Velocity Integral Limits object
|
||||
*
|
||||
*
|
||||
* @param lim float limit to be set (on all axis)
|
||||
*/
|
||||
void setVelocityIntegralLimits(const float lim);
|
||||
@@ -145,12 +145,13 @@ public:
|
||||
* Set the integral term in xy to 0.
|
||||
* @see _vel_int
|
||||
*/
|
||||
void resetIntegral() {
|
||||
_pos_int.setZero();
|
||||
_vel_int.setZero();
|
||||
}
|
||||
void resetIntegral()
|
||||
{
|
||||
_pos_int.setZero();
|
||||
_vel_int.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
/**
|
||||
* Get the controllers output attitude setpoint
|
||||
* This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
|
||||
* It needs to be executed by the attitude controller to achieve velocity and position tracking.
|
||||
@@ -184,10 +185,10 @@ private:
|
||||
float _lim_vel{}; ///< Horizontal velocity limit with feed forward and position control
|
||||
float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9
|
||||
float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
|
||||
float _pos_int_lim{}; ///< Anti-windup for position control
|
||||
float _pos_int_lim{}; ///< Anti-windup for position control
|
||||
float _vel_int_lim{}; ///< Anti-windup for velocity control
|
||||
|
||||
// States
|
||||
// States
|
||||
matrix::Vector3f _pos; /**< current position */
|
||||
matrix::Vector3f _pos_int; /**< integral term of the position controller */
|
||||
matrix::Vector3f _vel; /**< current velocity */
|
||||
|
||||
@@ -353,7 +353,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt,
|
||||
!_vehicle_control_mode.flag_control_offboard_enabled) {
|
||||
|
||||
if (_vehicle_control_mode.flag_control_attitude_enabled &&
|
||||
_vehicle_control_mode.flag_control_position_enabled) {
|
||||
_vehicle_control_mode.flag_control_position_enabled) {
|
||||
// We are in Stabilized mode
|
||||
// Generate position setpoints
|
||||
if (!stabilized_pos_sp_initialized) {
|
||||
|
||||
@@ -169,9 +169,9 @@ private:
|
||||
* @brief publishes target setpoint.
|
||||
*
|
||||
*/
|
||||
void publishLocalPositionSetpoint(vehicle_attitude_setpoint_s &_att_sp);
|
||||
void publishLocalPositionSetpoint(vehicle_attitude_setpoint_s &_att_sp);
|
||||
|
||||
/**
|
||||
/**
|
||||
* Generate setpoint to bridge no executable setpoint being available.
|
||||
* Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid.
|
||||
* This should only happen briefly when transitioning and never during mode operation or by design.
|
||||
|
||||
Reference in New Issue
Block a user