diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp index 6d9ea12767..a2ef448755 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp @@ -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; diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp index d5a910e41c..9bb2e55f8b 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp @@ -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 */ diff --git a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp index 5dd63f23b5..8b04d6c0a5 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp @@ -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) { diff --git a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp index a58531689e..7058361be7 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp @@ -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.