From 697bcde57516a7ea728726c3dcd5968fd46e2e92 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 13 May 2024 13:49:45 +0200 Subject: [PATCH] fix: format checks --- src/lib/mixer_module/actuator_test.cpp | 1 + .../functions/FunctionThrusters.hpp | 5 +- src/lib/mixer_module/mixer_module.cpp | 2 +- src/modules/commander/Commander.cpp | 1 + .../commander/ModeUtil/control_mode.cpp | 3 +- src/modules/commander/commander_helper.cpp | 3 +- src/modules/commander/commander_helper.h | 2 +- .../ActuatorEffectivenessSpacecraft.cpp | 2 +- .../ActuatorEffectivenessThrusters.cpp | 29 +- .../ActuatorEffectivenessThrusters.hpp | 7 +- .../control_allocator/ControlAllocator.cpp | 12 +- .../sc_att_control/sc_att_control_main.cpp | 15 +- .../PositionControl/PositionControl.cpp | 6 +- .../SpacecraftPositionControl.cpp | 20 +- .../SpacecraftPositionControl.hpp | 2 +- .../sc_rate_control/SpacecraftRateControl.cpp | 475 +++++++++--------- .../sc_thruster_controller.cpp | 298 +++++------ .../sc_thruster_controller.hpp | 63 +-- 18 files changed, 498 insertions(+), 448 deletions(-) diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp index 4d98fef3a1f6..5347be6b1129 100644 --- a/src/lib/mixer_module/actuator_test.cpp +++ b/src/lib/mixer_module/actuator_test.cpp @@ -93,6 +93,7 @@ void ActuatorTest::update(int num_outputs, float thrust_curve) value += trim.trim[idx]; } } + _current_outputs[i] = value; } else { diff --git a/src/lib/mixer_module/functions/FunctionThrusters.hpp b/src/lib/mixer_module/functions/FunctionThrusters.hpp index de7e7871cac8..61449c021fb5 100644 --- a/src/lib/mixer_module/functions/FunctionThrusters.hpp +++ b/src/lib/mixer_module/functions/FunctionThrusters.hpp @@ -44,7 +44,8 @@ class FunctionThrusters : public FunctionProviderBase static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1, "Unexpected num thrusters"); - static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1, "Unexpected thruster idx"); + static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1, + "Unexpected thruster idx"); FunctionThrusters(const Context &context) : _topic(&context.work_item, ORB_ID(actuator_motors)), @@ -76,7 +77,7 @@ class FunctionThrusters : public FunctionProviderBase { // TODO(@Pedro-Roque): for now bypass this /*if (thrust_factor > 0.f && thrust_factor <= 1.f) { - + // thrust factor // rel_thrust = factor * x^2 + (1-factor) * x, const float a = thrust_factor; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index bc9fa590d8a1..0a3b58ad821c 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -239,7 +239,7 @@ bool MixingOutput::updateSubscriptions(bool allow_wq_switch) if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &function) == 0) { if ((function >= (int32_t)OutputFunction::Motor1 && function <= (int32_t)OutputFunction::MotorMax) || - (function >= (int32_t)OutputFunction::Thruster1 && function <= (int32_t)OutputFunction::ThrusterMax)) { + (function >= (int32_t)OutputFunction::Thruster1 && function <= (int32_t)OutputFunction::ThrusterMax)) { switch_requested = true; } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 323b710a5475..7a20ea0a0911 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1718,6 +1718,7 @@ void Commander::updateParameters() } else if (is_ground) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + } else if (is_space) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index f53f807484b2..83c698d2bd4b 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -39,7 +39,8 @@ namespace mode_util static bool stabilization_required(uint8_t vehicle_type) { - return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || vehicle_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; + return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } static bool if_not_spacecraft(uint8_t vehicle_type) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 988505927e4c..559cd4a76c18 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -126,7 +126,8 @@ bool is_ground_vehicle(const vehicle_status_s ¤t_status) bool is_spacecraft(const vehicle_status_s ¤t_status) { - return (current_status.system_type == VEHICLE_TYPE_SPACECRAFT_2D || current_status.system_type == VEHICLE_TYPE_SPACECRAFT_3D); + return (current_status.system_type == VEHICLE_TYPE_SPACECRAFT_2D + || current_status.system_type == VEHICLE_TYPE_SPACECRAFT_3D); } // End time for currently blinking LED message, 0 if no blink message diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index daa8abdc0056..631025817f88 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -56,7 +56,7 @@ bool is_vtol(const vehicle_status_s ¤t_status); bool is_vtol_tailsitter(const vehicle_status_s ¤t_status); bool is_fixed_wing(const vehicle_status_s ¤t_status); bool is_ground_vehicle(const vehicle_status_s ¤t_status); -bool is_spacecraft(const vehicle_status_s& current_status); +bool is_spacecraft(const vehicle_status_s ¤t_status); int buzzer_init(void); void buzzer_deinit(void); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp index 526fd23e53f6..ff440dadea4a 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp @@ -61,5 +61,5 @@ void ActuatorEffectivenessSpacecraft::updateSetpoint(const matrix::VectorallocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); + if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); } + _control_allocation[i]->clipActuatorSetpoint(); } } @@ -690,15 +692,20 @@ ControlAllocator::publish_actuator_controls() // Actuator setpoints are shared between Motors or Thrusters. For now, we assume we have only either of them int actuator_type = 0; + if (_num_actuators[(int)ActuatorType::THRUSTERS] > 0) { actuator_type = (int)ActuatorType::THRUSTERS; } + // motors int motors_idx; - for (motors_idx = 0; motors_idx < _num_actuators[actuator_type] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + + for (motors_idx = 0; motors_idx < _num_actuators[actuator_type] + && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + if (stopped_motors & (1u << motors_idx)) { actuator_motors.control[motors_idx] = NAN; } @@ -717,7 +724,8 @@ ControlAllocator::publish_actuator_controls() if (_num_actuators[(int)ActuatorType::SERVOS] > 0) { int servos_idx; - for (servos_idx = 0; servos_idx < _num_actuators[(int)ActuatorType::SERVOS] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { + for (servos_idx = 0; servos_idx < _num_actuators[(int)ActuatorType::SERVOS] + && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; diff --git a/src/modules/sc_att_control/sc_att_control_main.cpp b/src/modules/sc_att_control/sc_att_control_main.cpp index 0ee05b9a9b80..7256657b8832 100644 --- a/src/modules/sc_att_control/sc_att_control_main.cpp +++ b/src/modules/sc_att_control/sc_att_control_main.cpp @@ -35,7 +35,7 @@ * @file sc_att_control_main.cpp * Spacecraft attitude controller. * Based off the multicopter attitude controller module. - * + * * @author Pedro Roque, * */ @@ -86,7 +86,7 @@ SpacecraftAttitudeControl::parameters_updated() using math::radians; _attitude_control.setRateLimit(Vector3f(radians(_param_sc_rollrate_max.get()), radians(_param_sc_pitchrate_max.get()), radians(_param_sc_yawrate_max.get()))); - _man_tilt_max = math::radians(_param_sc_man_tilt_max.get()); + _man_tilt_max = math::radians(_param_sc_man_tilt_max.get()); } void @@ -126,6 +126,7 @@ SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max), -_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max)); float v_norm = v.norm(); // the norm of v defines the tilt angle + if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle v *= _man_tilt_max / v_norm; } @@ -197,8 +198,8 @@ SpacecraftAttitudeControl::Run() if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { /* Removing print - PX4_INFO("Current setpoint: %f %f %f %f", (double)vehicle_attitude_setpoint.q_d[0], - (double)vehicle_attitude_setpoint.q_d[1], (double)vehicle_attitude_setpoint.q_d[2], + PX4_INFO("Current setpoint: %f %f %f %f", (double)vehicle_attitude_setpoint.q_d[0], + (double)vehicle_attitude_setpoint.q_d[1], (double)vehicle_attitude_setpoint.q_d[2], (double)vehicle_attitude_setpoint.q_d[3]);*/ _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d)); @@ -244,6 +245,7 @@ SpacecraftAttitudeControl::Run() } bool attitude_setpoint_generated = false; + if (_vehicle_control_mode.flag_control_attitude_enabled) { // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode @@ -259,11 +261,12 @@ SpacecraftAttitudeControl::Run() _man_roll_input_filter.reset(0.f); _man_pitch_input_filter.reset(0.f); } + Vector3f rates_sp = _attitude_control.update(q); - + // publish rate setpoint vehicle_rates_setpoint_s rates_setpoint{}; - rates_setpoint.roll = rates_sp(0); + rates_setpoint.roll = rates_sp(0); rates_setpoint.pitch = rates_sp(1); rates_setpoint.yaw = rates_sp(2); _thrust_setpoint_body.copyTo(rates_setpoint.thrust_body); diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp index 4c848e940766..3bee760fd2fc 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -161,7 +161,8 @@ bool ScPositionControl::_inputValid() return valid; } -void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, vehicle_attitude_s &v_att) const +void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, + vehicle_attitude_s &v_att) const { // Set thrust setpoint attitude_setpoint.thrust_body[0] = _thr_sp(0); @@ -169,11 +170,12 @@ void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitud attitude_setpoint.thrust_body[2] = _thr_sp(2); // Bypass attitude control by giving same attitude setpoint to att control - if(PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3)) ) { + if (PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3))) { attitude_setpoint.q_d[0] = _quat_sp(0); attitude_setpoint.q_d[1] = _quat_sp(1); attitude_setpoint.q_d[2] = _quat_sp(2); attitude_setpoint.q_d[3] = _quat_sp(3); + } else { attitude_setpoint.q_d[0] = v_att.q[0]; attitude_setpoint.q_d[1] = v_att.q[1]; diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index 03801ba32ba5..8b7a9ff16ed6 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -93,9 +93,11 @@ void SpacecraftPositionControl::parameters_update(bool force) if (responsiveness > 0.6f) { num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); + } else { num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); } + num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); } @@ -194,8 +196,10 @@ PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicl _vel_z_deriv.reset(); } - if(PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) && PX4_ISFINITE(vehicle_attitude.q[3])) { + if (PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) + && PX4_ISFINITE(vehicle_attitude.q[3])) { states.quaternion = Quatf(vehicle_attitude.q); + } else { states.quaternion = Quatf(); } @@ -298,8 +302,8 @@ void SpacecraftPositionControl::Run() if ((_setpoint.timestamp < _time_position_control_enabled) && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { PX4_INFO("Setpoint time: %f, Vehicle local pos time: %f, Pos Control Enabled time: %f", - (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample, - (double)_time_position_control_enabled); + (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample, + (double)_time_position_control_enabled); _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false); } } @@ -347,15 +351,16 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, if (_vehicle_control_mode.flag_control_attitude_enabled) { // We are in Stabilized mode // Generate position setpoints - if(!stabilized_pos_sp_initialized) { + if (!stabilized_pos_sp_initialized) { // Initialize position setpoint target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, - vehicle_local_position.z); + vehicle_local_position.z); const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); _manual_yaw_sp = vehicle_yaw; stabilized_pos_sp_initialized = true; } + // Update velocity setpoint Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0); // TODO(@Pedro-Roque): probably need to move velocity to inertial frame @@ -372,9 +377,11 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, // Generate attitude setpoints float yaw_sp_move_rate = 0.0; - if (_manual_control_setpoint.throttle > -0.9f){ + + if (_manual_control_setpoint.throttle > -0.9f) { yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; } + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt); const float roll_body = 0.0; const float pitch_body = 0.0; @@ -383,6 +390,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, q_sp.copyTo(_setpoint.attitude); _setpoint.timestamp = hrt_absolute_time(); + } else { // We are in Manual mode stabilized_pos_sp_initialized = false; diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index b77c4573bf4c..685d0240634b 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -174,7 +174,7 @@ class SpacecraftPositionControl : public ModuleBase, * Check for manual setpoints. */ void poll_manual_setpoint(const float dt, const vehicle_local_position_s - &vehicle_local_position, const vehicle_attitude_s &_vehicle_att); + &vehicle_local_position, const vehicle_attitude_s &_vehicle_att); /** * Generate setpoint to bridge no executable setpoint being available. diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.cpp b/src/modules/sc_rate_control/SpacecraftRateControl.cpp index d0691f8940e5..54ffc4daeaaa 100644 --- a/src/modules/sc_rate_control/SpacecraftRateControl.cpp +++ b/src/modules/sc_rate_control/SpacecraftRateControl.cpp @@ -44,271 +44,278 @@ using namespace time_literals; using math::radians; SpacecraftRateControl::SpacecraftRateControl() - : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _vehicle_torque_setpoint_pub(ORB_ID(vehicle_torque_setpoint)), - _vehicle_thrust_setpoint_pub(ORB_ID(vehicle_thrust_setpoint)), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")) { - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; - - parameters_updated(); - _controller_status_pub.advertise(); + : ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _vehicle_torque_setpoint_pub(ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(ORB_ID(vehicle_thrust_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; + + parameters_updated(); + _controller_status_pub.advertise(); } SpacecraftRateControl::~SpacecraftRateControl() { perf_free(_loop_perf); } -bool SpacecraftRateControl::init() { - if (!_vehicle_angular_velocity_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } +bool SpacecraftRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } - return true; + return true; } -void SpacecraftRateControl::parameters_updated() { - // rate control parameters - // The controller gain K is used to convert the parallel (P + I/s + sD) form - // to the ideal (K * [1 + 1/sTi + sTd]) form - const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get()); +void SpacecraftRateControl::parameters_updated() +{ + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get()); - _rate_control.setPidGains( - rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())), - rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())), - rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get()))); + _rate_control.setPidGains( + rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get()))); - _rate_control.setIntegratorLimit( - Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get())); + _rate_control.setIntegratorLimit( + Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get())); - _rate_control.setFeedForwardGain( - Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get())); + _rate_control.setFeedForwardGain( + Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get())); - // manual rate control acro mode rate limits - _acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()), - radians(_param_sc_acro_y_max.get())); + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()), + radians(_param_sc_acro_y_max.get())); } -void SpacecraftRateControl::Run() { - if (should_exit()) { - _vehicle_angular_velocity_sub.unregisterCallback(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - parameters_updated(); - } - - /* run controller on gyro changes */ - vehicle_angular_velocity_s angular_velocity; - - if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { - const hrt_abstime now = angular_velocity.timestamp_sample; - - // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); - _last_run = now; - - const Vector3f rates{angular_velocity.xyz}; - const Vector3f angular_accel{angular_velocity.xyz_derivative}; - - /* check for updates in other topics */ - _vehicle_control_mode_sub.update(&_vehicle_control_mode); - - if (_vehicle_land_detected_sub.updated()) { - vehicle_land_detected_s vehicle_land_detected; - - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { - _landed = vehicle_land_detected.landed; - _maybe_landed = vehicle_land_detected.maybe_landed; - } - } - - _vehicle_status_sub.update(&_vehicle_status); - - // use rates setpoint topic - vehicle_rates_setpoint_s vehicle_rates_setpoint{}; - - if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { - // generate the rate setpoint from sticks - manual_control_setpoint_s manual_control_setpoint; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - // manual rates control - ACRO mode - const Vector3f man_rate_sp{ - math::superexpo(manual_control_setpoint.roll, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), - math::superexpo(-manual_control_setpoint.pitch, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), - math::superexpo(manual_control_setpoint.yaw, _param_sc_acro_expo_y.get(), _param_sc_acro_supexpoy.get())}; - - _rates_setpoint = man_rate_sp.emult(_acro_rate_max); - _thrust_setpoint(2) = -manual_control_setpoint.throttle; - _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; - - // publish rate setpoint - vehicle_rates_setpoint.roll = _rates_setpoint(0); - vehicle_rates_setpoint.pitch = _rates_setpoint(1); - vehicle_rates_setpoint.yaw = _rates_setpoint(2); - _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); - vehicle_rates_setpoint.timestamp = hrt_absolute_time(); - - _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); - } - - } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { - if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { - _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); - _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); - _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); - _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); - } - } - - // run the rate controller - if (_vehicle_control_mode.flag_control_rates_enabled) { - // reset integral if disarmed - if (!_vehicle_control_mode.flag_armed || - _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rate_control.resetIntegral(); - } - - // update saturation status from control allocation feedback - control_allocator_status_s control_allocator_status; - - if (_control_allocator_status_sub.update(&control_allocator_status)) { - Vector saturation_positive; - Vector saturation_negative; - - if (!control_allocator_status.torque_setpoint_achieved) { - for (size_t i = 0; i < 3; i++) { - if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { - saturation_positive(i) = true; - - } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { - saturation_negative(i) = true; - } - } - } - - // TODO: send the unallocated value directly for better anti-windup - _rate_control.setSaturationStatus(saturation_positive, saturation_negative); - } - - // run rate controller - const Vector3f att_control = - _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); - - // publish rate controller status - rate_ctrl_status_s rate_ctrl_status{}; - _rate_control.getRateControlStatus(rate_ctrl_status); - rate_ctrl_status.timestamp = hrt_absolute_time(); - _controller_status_pub.publish(rate_ctrl_status); - - // publish thrust and torque setpoints - vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - vehicle_torque_setpoint_s vehicle_torque_setpoint{}; - - _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); - vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f; - vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; - vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; - - // scale setpoints by battery status if enabled - if (_param_sc_bat_scale_en.get()) { - if (_battery_status_sub.updated()) { - battery_status_s battery_status; - - if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { - _battery_status_scale = battery_status.scale; - } - } - - if (_battery_status_scale > 0.f) { - for (int i = 0; i < 3; i++) { - vehicle_thrust_setpoint.xyz[i] = - math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); - vehicle_torque_setpoint.xyz[i] = - math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); - } - } - } - - vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; - vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); - _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); - - vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; - vehicle_torque_setpoint.timestamp = hrt_absolute_time(); - _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); - - updateActuatorControlsStatus(vehicle_torque_setpoint, dt); - } - } - - perf_end(_loop_perf); +void SpacecraftRateControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + /* check for updates in other topics */ + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + + _vehicle_status_sub.update(&_vehicle_status); + + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint{}; + + if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{ + math::superexpo(manual_control_setpoint.roll, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.pitch, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.yaw, _param_sc_acro_expo_y.get(), _param_sc_acro_supexpoy.get())}; + + _rates_setpoint = man_rate_sp.emult(_acro_rate_max); + _thrust_setpoint(2) = -manual_control_setpoint.throttle; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + + // publish rate setpoint + vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint.pitch = _rates_setpoint(1); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); + _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); + vehicle_rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + } + + } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + } + } + + // run the rate controller + if (_vehicle_control_mode.flag_control_rates_enabled) { + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed || + _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + // run rate controller + const Vector3f att_control = + _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; + + // scale setpoints by battery status if enabled + if (_param_sc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.f) { + for (int i = 0; i < 3; i++) { + vehicle_thrust_setpoint.xyz[i] = + math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + vehicle_torque_setpoint.xyz[i] = + math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + } + } + } + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } + + perf_end(_loop_perf); } -void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s& vehicle_torque_setpoint, - float dt) { - for (int i = 0; i < 3; i++) { - _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; - } +void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, + float dt) +{ + for (int i = 0; i < 3; i++) { + _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; + } - _energy_integration_time += dt; + _energy_integration_time += dt; - if (_energy_integration_time > 500e-3f) { - actuator_controls_status_s status; - status.timestamp = vehicle_torque_setpoint.timestamp; + if (_energy_integration_time > 500e-3f) { + actuator_controls_status_s status; + status.timestamp = vehicle_torque_setpoint.timestamp; - for (int i = 0; i < 3; i++) { - status.control_power[i] = _control_energy[i] / _energy_integration_time; - _control_energy[i] = 0.f; - } + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } - _actuator_controls_status_pub.publish(status); - _energy_integration_time = 0.f; - } + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } } -int SpacecraftRateControl::task_spawn(int argc, char* argv[]) { +int SpacecraftRateControl::task_spawn(int argc, char *argv[]) +{ - SpacecraftRateControl* instance = new SpacecraftRateControl(); + SpacecraftRateControl *instance = new SpacecraftRateControl(); - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; - if (instance->init()) { - return PX4_OK; - } + if (instance->init()) { + return PX4_OK; + } - } else { - PX4_ERR("alloc failed"); - } + } else { + PX4_ERR("alloc failed"); + } - delete instance; - _object.store(nullptr); - _task_id = -1; + delete instance; + _object.store(nullptr); + _task_id = -1; - return PX4_ERROR; + return PX4_ERROR; } -int SpacecraftRateControl::custom_command(int argc, char* argv[]) { return print_usage("unknown command"); } +int SpacecraftRateControl::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int SpacecraftRateControl::print_usage(const char* reason) { - if (reason) { - PX4_WARN("%s\n", reason); - } +int SpacecraftRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( ### Description This implements the spacecraft rate controller. It takes rate setpoints (in acro mode via `manual_control_setpoint` topic) as inputs and outputs torque and thrust setpoints. diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp index 9660b9a47986..e032310ce1b4 100644 --- a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp @@ -40,166 +40,180 @@ #include #include -int ScThrusterController::print_status() { - PX4_INFO("Running"); - // TODO: print additional runtime information about the state of the module +int ScThrusterController::print_status() +{ + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module - return 0; + return 0; } -int ScThrusterController::custom_command(int argc, char* argv[]) { - if (!is_running()) { - print_usage("not running"); - return 1; - } - /* - // additional custom commands can be handled like this: - if (!strcmp(argv[0], "do-something")) { - get_instance()->do_something(); - return 0; - } - */ - - return print_usage("unknown command"); +int ScThrusterController::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + print_usage("not running"); + return 1; + } + + /* + // additional custom commands can be handled like this: + if (!strcmp(argv[0], "do-something")) { + get_instance()->do_something(); + return 0; + } + */ + + return print_usage("unknown command"); } -int ScThrusterController::task_spawn(int argc, char* argv[]) { - _task_id = px4_task_spawn_cmd("module", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, (px4_main_t)&run_trampoline, - (char* const*)argv); +int ScThrusterController::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("module", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; - return -errno; - } + if (_task_id < 0) { + _task_id = -1; + return -errno; + } - return 0; + return 0; } -ScThrusterController* ScThrusterController::instantiate(int argc, char* argv[]) { - int myoptind = 1; - int ch; - bool error_flag = false; - const char *myoptarg = nullptr; - - // parse CLI arguments - _debug_thruster_print = false; - while ((ch = px4_getopt(argc, argv, "d", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'd': - _debug_thruster_print = true; - PX4_INFO("Debugging enabled"); - break; - case '?': - error_flag = true; - break; - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return nullptr; - } - - ScThrusterController* instance = new ScThrusterController(); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - } - - return instance; +ScThrusterController *ScThrusterController::instantiate(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + bool error_flag = false; + const char *myoptarg = nullptr; + + // parse CLI arguments + _debug_thruster_print = false; + + while ((ch = px4_getopt(argc, argv, "d", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + _debug_thruster_print = true; + PX4_INFO("Debugging enabled"); + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return nullptr; + } + + ScThrusterController *instance = new ScThrusterController(); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; } ScThrusterController::ScThrusterController() : ModuleParams(nullptr) {} -void ScThrusterController::run() { - // Subscribe to thruster command - int thrustercmd_sub = orb_subscribe(ORB_ID(thruster_command)); - px4_pollfd_struct_t fds[] = { - {.fd = thrustercmd_sub, .events = POLLIN}, - }; - - // Create publisher for PWM values - struct actuator_motors_s actuator_motors_msg; - memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); - orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); - - // For publishing debug messages - struct my_message_s my_msg; - memset(&my_msg, 0, sizeof(my_msg)); - orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); - - // Thruster command structure - struct thruster_command_s thruster_cmd; - - // initialize parameters - parameters_update(true); - bool debugPrint = ScThrusterController::_debug_thruster_print; - PX4_INFO("Debugging enabled: %d", debugPrint); - - while (!should_exit()) { - // wait for up to 1000ms for data - int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); - - if (pret == 0) { - // Timeout: let the loop run anyway, don't do `continue` here - if (debugPrint) PX4_WARN("Timeout..."); - } else if (pret < 0) { - // this is undesirable but not much we can do - PX4_ERR("poll error %d, %d", pret, errno); - px4_usleep(50000); - continue; - - } else if (fds[0].revents & POLLIN) { - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); - if (debugPrint) - PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", (double)thruster_cmd.x1, (double)thruster_cmd.x2, - (double)thruster_cmd.y1, (double)thruster_cmd.y2); - - actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; - actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; - actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; - actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; - actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; - actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; - actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; - actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; - - orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); - - if (debugPrint) { - strcpy(my_msg.text, "Thruster command received!"); - orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); - } - } - parameters_update(); - } - - orb_unsubscribe(thrustercmd_sub); +void ScThrusterController::run() +{ + // Subscribe to thruster command + int thrustercmd_sub = orb_subscribe(ORB_ID(thruster_command)); + px4_pollfd_struct_t fds[] = { + {.fd = thrustercmd_sub, .events = POLLIN}, + }; + + // Create publisher for PWM values + struct actuator_motors_s actuator_motors_msg; + memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); + orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); + + // For publishing debug messages + struct my_message_s my_msg; + memset(&my_msg, 0, sizeof(my_msg)); + orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); + + // Thruster command structure + struct thruster_command_s thruster_cmd; + + // initialize parameters + parameters_update(true); + bool debugPrint = ScThrusterController::_debug_thruster_print; + PX4_INFO("Debugging enabled: %d", debugPrint); + + while (!should_exit()) { + // wait for up to 1000ms for data + int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); + + if (pret == 0) { + // Timeout: let the loop run anyway, don't do `continue` here + if (debugPrint) { PX4_WARN("Timeout..."); } + + } else if (pret < 0) { + // this is undesirable but not much we can do + PX4_ERR("poll error %d, %d", pret, errno); + px4_usleep(50000); + continue; + + } else if (fds[0].revents & POLLIN) { + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); + + if (debugPrint) + PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", (double)thruster_cmd.x1, (double)thruster_cmd.x2, + (double)thruster_cmd.y1, (double)thruster_cmd.y2); + + actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; + actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; + actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; + actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; + actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; + actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; + actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; + actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; + + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + + if (debugPrint) { + strcpy(my_msg.text, "Thruster command received!"); + orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); + } + } + + parameters_update(); + } + + orb_unsubscribe(thrustercmd_sub); } -void ScThrusterController::parameters_update(bool force) { - // check for parameter updates - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s update; - _parameter_update_sub.copy(&update); - - // update parameters from storage - updateParams(); - } +void ScThrusterController::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); + + // update parameters from storage + updateParams(); + } } -int ScThrusterController::print_usage(const char* reason) { - if (reason) { - PX4_WARN("%s\n", reason); - } +int ScThrusterController::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( ### Description Section that describes the provided module functionality. diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp index 7fc036f82813..db94f6b78800 100644 --- a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp @@ -58,49 +58,50 @@ using namespace time_literals; -extern "C" __EXPORT int sc_thruster_controller_main(int argc, char* argv[]); +extern "C" __EXPORT int sc_thruster_controller_main(int argc, char *argv[]); -class ScThrusterController : public ModuleBase, public ModuleParams { - public: - ScThrusterController(); +class ScThrusterController : public ModuleBase, public ModuleParams +{ +public: + ScThrusterController(); - virtual ~ScThrusterController() = default; + virtual ~ScThrusterController() = default; - /** @see ModuleBase */ - static int task_spawn(int argc, char* argv[]); + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ - static ScThrusterController* instantiate(int argc, char* argv[]); + /** @see ModuleBase */ + static ScThrusterController *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ - static int custom_command(int argc, char* argv[]); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); - /** @see ModuleBase */ - static int print_usage(const char* reason = nullptr); + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::run() */ - void run() override; + /** @see ModuleBase::run() */ + void run() override; - /** @see ModuleBase::print_status() */ - int print_status() override; + /** @see ModuleBase::print_status() */ + int print_status() override; - private: - /** - * Check for parameter changes and update them if needed. - * @param parameter_update_sub uorb subscription to parameter_update - * @param force for a parameter update - */ - void parameters_update(bool force = false); +private: + /** + * Check for parameter changes and update them if needed. + * @param parameter_update_sub uorb subscription to parameter_update + * @param force for a parameter update + */ + void parameters_update(bool force = false); - DEFINE_PARAMETERS((ParamInt)_param_sys_autostart, /**< example parameter */ - (ParamInt)_param_sys_autoconfig /**< another parameter */ - ) + DEFINE_PARAMETERS((ParamInt)_param_sys_autostart, /**< example parameter */ + (ParamInt)_param_sys_autoconfig /**< another parameter */ + ) - // Flags - static bool _debug_thruster_print; + // Flags + static bool _debug_thruster_print; - // Subscriptions - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; }; bool ScThrusterController::_debug_thruster_print = true;