diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp index d2228d5597..b13acd5122 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp @@ -111,12 +111,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo // copy quaternion setpoint to attitude setpoint topic const Quatf q_sp{R_sp}; q_sp.copyTo(att_sp.q_d); - - // calculate euler angles, for logging only, must not be used for control - const Eulerf euler{R_sp}; - att_sp.roll_body = euler.phi(); - att_sp.pitch_body = euler.theta(); - att_sp.yaw_body = euler.psi(); } Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) @@ -257,4 +251,4 @@ void setZeroIfNanVector3f(Vector3f &vector) // Adding zero vector overwrites elements that are NaN with zero addIfNotNanVector3f(vector, Vector3f()); } -} \ No newline at end of file +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp index a2ef448755..c1ed93896a 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp @@ -45,7 +45,7 @@ using namespace matrix; -const trajectory_setpoint_s ScPositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; +const trajectory_setpoint6dof_s ScPositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}}; void ScPositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { @@ -88,12 +88,12 @@ void ScPositionControl::setState(const PositionControlStates &states) _att_q = states.quaternion; } -void ScPositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) +void ScPositionControl::setInputSetpoint(const trajectory_setpoint6dof_s &setpoint) { _pos_sp = Vector3f(setpoint.position); _vel_sp = Vector3f(setpoint.velocity); _acc_sp = Vector3f(setpoint.acceleration); - _quat_sp = Quatf(setpoint.attitude); + _quat_sp = Quatf(setpoint.quaternion); } bool ScPositionControl::update(const float dt) diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp index 9bb2e55f8b..a4dc628bb0 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include #include @@ -128,7 +128,7 @@ public: * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. * @param setpoint setpoints including feed-forwards to execute in update() */ - void setInputSetpoint(const trajectory_setpoint_s &setpoint); + void setInputSetpoint(const trajectory_setpoint6dof_s &setpoint); /** * Apply P-position and PID-velocity controller that updates the member @@ -162,7 +162,7 @@ public: /** * All setpoints are set to NAN (uncontrolled). Timestampt zero. */ - static const trajectory_setpoint_s empty_trajectory_setpoint; + static const trajectory_setpoint6dof_s empty_trajectory_setpoint; private: // The range limits of the hover thrust configuration/estimate diff --git a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp index 8b04d6c0a5..eccf2bb945 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp @@ -40,10 +40,7 @@ using namespace matrix; SpacecraftPositionControl::SpacecraftPositionControl(ModuleParams *parent) : ModuleParams(parent), - _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)), - _vel_x_deriv(this, "VELD"), - _vel_y_deriv(this, "VELD"), - _vel_z_deriv(this, "VELD") + _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)) { updateParams(); } @@ -58,7 +55,6 @@ void SpacecraftPositionControl::updateParams() // update parameters from storage ModuleParams::updateParams(); - SuperBlock::updateParams(); int num_changed = 0; @@ -156,28 +152,16 @@ PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicl if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) { states.velocity.xy() = velocity_xy; - states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0)); - states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1)); } else { states.velocity(0) = states.velocity(1) = NAN; - states.acceleration(0) = states.acceleration(1) = NAN; - - // reset derivatives to prevent acceleration spikes when regaining velocity - _vel_x_deriv.reset(); - _vel_y_deriv.reset(); } if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) { states.velocity(2) = vehicle_local_position.vz; - states.acceleration(2) = _vel_z_deriv.update(states.velocity(2)); } else { states.velocity(2) = NAN; - states.acceleration(2) = NAN; - - // reset derivative to prevent acceleration spikes when regaining velocity - _vel_z_deriv.reset(); } if (PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) @@ -201,9 +185,6 @@ void SpacecraftPositionControl::updatePositionControl() math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); _time_stamp_last_loop = vehicle_local_position.timestamp_sample; - // set _dt in controllib Block for BlockDerivative - setDt(dt); - if (_vehicle_control_mode_sub.updated()) { const bool previous_position_control_enabled = _vehicle_control_mode.flag_control_position_enabled; @@ -244,19 +225,11 @@ void SpacecraftPositionControl::updatePositionControl() } if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) { - _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading); + // Set proper attitude setpoint with quaternion + // _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading); } } - if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { - _vel_x_deriv.reset(); - _vel_y_deriv.reset(); - } - - if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { - _vel_z_deriv.reset(); - } - // save latest reset counters _vxy_reset_counter = vehicle_local_position.vxy_reset_counter; _vz_reset_counter = vehicle_local_position.vz_reset_counter; @@ -328,8 +301,6 @@ void SpacecraftPositionControl::publishLocalPositionSetpoint(vehicle_attitude_se local_position_setpoint.x = _setpoint.position[0]; local_position_setpoint.y = _setpoint.position[1]; local_position_setpoint.z = _setpoint.position[2]; - local_position_setpoint.yaw = NAN; - local_position_setpoint.yawspeed = NAN; local_position_setpoint.vx = _setpoint.velocity[0]; local_position_setpoint.vy = _setpoint.velocity[1]; local_position_setpoint.vz = _setpoint.velocity[2]; @@ -348,10 +319,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, { if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_armed) { if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { - - if (!_vehicle_control_mode.flag_control_climb_rate_enabled && - !_vehicle_control_mode.flag_control_offboard_enabled) { - + if (!_vehicle_control_mode.flag_control_offboard_enabled) { if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_control_position_enabled) { // We are in Stabilized mode @@ -391,7 +359,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, const float pitch_body = 0.0; Quatf q_sp(Eulerf(roll_body, pitch_body, _manual_yaw_sp)); - q_sp.copyTo(_setpoint.attitude); + q_sp.copyTo(_setpoint.quaternion); _setpoint.timestamp = hrt_absolute_time(); @@ -409,7 +377,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, } } -trajectory_setpoint_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, +trajectory_setpoint6dof_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn) { // rate limit the warnings @@ -420,7 +388,7 @@ trajectory_setpoint_s SpacecraftPositionControl::generateFailsafeSetpoint(const _last_warn = now; } - trajectory_setpoint_s failsafe_setpoint = ScPositionControl::empty_trajectory_setpoint; + trajectory_setpoint6dof_s failsafe_setpoint = ScPositionControl::empty_trajectory_setpoint; failsafe_setpoint.timestamp = now; failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = failsafe_setpoint.velocity[2] = 0.f; diff --git a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp index 7058361be7..18d8b54631 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp +++ b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include #include @@ -73,13 +73,13 @@ public: SpacecraftPositionControl(ModuleParams *parent); ~SpacecraftPositionControl() = default; - bool updatePositionControl(); + void updatePositionControl(); protected: /** * Update our local parameter cache. */ - void updateParams(bool force); + void updateParams(); private: @@ -92,7 +92,7 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ - uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint6dof)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; @@ -100,7 +100,7 @@ private: hrt_abstime _time_position_control_enabled{0}; hrt_abstime _manual_setpoint_last_called{0}; - trajectory_setpoint_s _setpoint{ScPositionControl::empty_trajectory_setpoint}; + trajectory_setpoint6dof_s _setpoint{ScPositionControl::empty_trajectory_setpoint}; vehicle_control_mode_s _vehicle_control_mode{}; manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ @@ -127,10 +127,6 @@ private: (ParamFloat) _param_mpc_thr_max ); - control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ - control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ - control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ - matrix::Vector3f target_pos_sp; float yaw_rate; bool stabilized_pos_sp_initialized{false}; @@ -176,5 +172,5 @@ private: * 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. */ - trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn); + trajectory_setpoint6dof_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn); };