From b16f16598b3c4196f3d827679c266c8a589427fb Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 23 Dec 2022 12:57:18 +0100 Subject: [PATCH] VTOL: remove virtual actuator_controls and instead use virtual torque/thrust topics MC/FW rate controller and auto tuner: remove actuator_controls AirshipAttControl: remove actuator_controls MulticopterLandDetector: remove actuator_controls mavlink streams vfr_hud and high_latency2: remove actuator_controls RoverPositionController: remove actuator_controls UUVAttitudeController: remove actuator_controls battery: use length of thrust_setpoint for throttle compensation VehicleMagnetometer: use length of thrust_setpoint for throttle compensation Signed-off-by: Silvan Fuhrer --- msg/ActuatorControls.msg | 19 --- msg/ActuatorControlsStatus.msg | 7 +- msg/CMakeLists.txt | 1 - msg/VehicleThrustSetpoint.msg | 3 + msg/VehicleTorqueSetpoint.msg | 3 + src/drivers/px4io/px4io.cpp | 1 - src/lib/battery/battery.cpp | 8 +- src/lib/battery/battery.h | 4 +- src/lib/sensor_calibration/Magnetometer.hpp | 1 - src/lib/system_identification/test_data.h | 4 +- .../airship_att_control.hpp | 5 - .../airship_att_control_main.cpp | 46 ++----- src/modules/commander/commander_helper.cpp | 1 - .../ActuatorEffectivenessRotors.cpp | 2 +- .../fw_autotune_attitude_control.cpp | 20 +-- .../fw_autotune_attitude_control.hpp | 4 +- .../fw_rate_control/FixedwingRateControl.cpp | 114 ++++++------------ .../fw_rate_control/FixedwingRateControl.hpp | 12 +- .../land_detector/MulticopterLandDetector.cpp | 10 +- .../land_detector/MulticopterLandDetector.h | 6 +- src/modules/logger/logged_topics.cpp | 13 +- src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 16 +-- src/modules/mavlink/streams/VFR_HUD.hpp | 20 +-- .../mc_autotune_attitude_control.cpp | 20 +-- .../mc_autotune_attitude_control.hpp | 4 +- .../MulticopterRateControl.cpp | 82 +++++-------- .../MulticopterRateControl.hpp | 13 +- src/modules/navigator/mission_block.cpp | 1 - src/modules/navigator/mission_block.h | 1 - .../payload_deliverer/payload_deliverer.h | 1 - src/modules/rc_update/rc_update.h | 1 - .../RoverPositionControl.cpp | 40 +++--- .../RoverPositionControl.hpp | 5 +- .../VehicleMagnetometer.cpp | 8 +- .../VehicleMagnetometer.hpp | 4 +- .../uuv_att_control/uuv_att_control.cpp | 58 +++------ .../uuv_att_control/uuv_att_control.hpp | 5 +- .../uuv_pos_control/uuv_pos_control.cpp | 2 +- .../uuv_pos_control/uuv_pos_control.hpp | 1 - src/modules/vtol_att_control/standard.cpp | 114 ++++++++---------- src/modules/vtol_att_control/tailsitter.cpp | 66 +++------- src/modules/vtol_att_control/tiltrotor.cpp | 61 +++------- .../vtol_att_control_main.cpp | 28 +++-- .../vtol_att_control/vtol_att_control_main.h | 27 ++--- src/modules/vtol_att_control/vtol_type.cpp | 10 +- src/modules/vtol_att_control/vtol_type.h | 8 +- 47 files changed, 332 insertions(+), 549 deletions(-) delete mode 100644 msg/ActuatorControls.msg diff --git a/msg/ActuatorControls.msg b/msg/ActuatorControls.msg deleted file mode 100644 index 56699d0314..0000000000 --- a/msg/ActuatorControls.msg +++ /dev/null @@ -1,19 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint8 NUM_ACTUATOR_CONTROLS = 9 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 -uint8 INDEX_ROLL = 0 -uint8 INDEX_PITCH = 1 -uint8 INDEX_YAW = 2 -uint8 INDEX_THROTTLE = 3 -uint8 INDEX_GIMBAL_SHUTTER = 3 -uint8 INDEX_CAMERA_ZOOM = 4 - -uint8 GROUP_INDEX_ATTITUDE = 0 -uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 -uint8 GROUP_INDEX_GIMBAL = 2 - -uint64 timestamp_sample # the timestamp the data this control response is based on was sampled -float32[9] control - -# TOPICS actuator_controls_0 actuator_controls_1 actuator_controls_2 -# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc diff --git a/msg/ActuatorControlsStatus.msg b/msg/ActuatorControlsStatus.msg index 287fac8a4d..c89f669eb1 100644 --- a/msg/ActuatorControlsStatus.msg +++ b/msg/ActuatorControlsStatus.msg @@ -1,10 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint8 INDEX_ROLL = 0 -uint8 INDEX_PITCH = 1 -uint8 INDEX_YAW = 2 -uint8 INDEX_THROTTLE = 3 - -float32[4] control_power +float32[3] control_power # TOPICS actuator_controls_status_0 actuator_controls_status_1 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index f13b7fb9d7..9e49bc016f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -39,7 +39,6 @@ include(px4_list_make_absolute) set(msg_files ActionRequest.msg ActuatorArmed.msg - ActuatorControls.msg ActuatorControlsStatus.msg ActuatorMotors.msg ActuatorOutputs.msg diff --git a/msg/VehicleThrustSetpoint.msg b/msg/VehicleThrustSetpoint.msg index 27c35726b9..444ee50032 100644 --- a/msg/VehicleThrustSetpoint.msg +++ b/msg/VehicleThrustSetpoint.msg @@ -3,3 +3,6 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) float32[3] xyz # thrust setpoint along X, Y, Z body axis [-1, 1] + +# TOPICS vehicle_thrust_setpoint +# TOPICS vehicle_thrust_setpoint_virtual_fw vehicle_thrust_setpoint_virtual_mc diff --git a/msg/VehicleTorqueSetpoint.msg b/msg/VehicleTorqueSetpoint.msg index 988d583592..c20519b16e 100644 --- a/msg/VehicleTorqueSetpoint.msg +++ b/msg/VehicleTorqueSetpoint.msg @@ -3,3 +3,6 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) float32[3] xyz # torque setpoint about X, Y, Z body axis (normalized) + +# TOPICS vehicle_torque_setpoint +# TOPICS vehicle_torque_setpoint_virtual_fw vehicle_torque_setpoint_virtual_mc diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 70d8ec6004..20bdfcb96e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -68,7 +68,6 @@ #include #include #include -#include #include #include #include diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 28e1f680a4..2ed157dee4 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -213,9 +213,11 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f cell_voltage += _params.r_internal * current_a; } else { - actuator_controls_s actuator_controls{}; - _actuator_controls_0_sub.copy(&actuator_controls); - const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + _vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint); + const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz); + const float throttle = thrust_setpoint.length(); + _throttle_filter.update(throttle); if (!_battery_initialized) { diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index e30313a272..26dc4e3e61 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -55,9 +55,9 @@ #include #include #include -#include #include #include +#include /** * BatteryBase is a base class for any type of battery. @@ -153,7 +153,7 @@ private: void computeScale(); float computeRemainingTime(float current_a); - uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 961155e12f..481cc58e8d 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -38,7 +38,6 @@ #include #include #include -#include #include namespace calibration diff --git a/src/lib/system_identification/test_data.h b/src/lib/system_identification/test_data.h index 6d6039cac7..e04d628935 100644 --- a/src/lib/system_identification/test_data.h +++ b/src/lib/system_identification/test_data.h @@ -40,9 +40,9 @@ * * Sampling frequency: ~285Hz, y_data is aligned with the closest next u_data * - * u_data: actuator_controls_0.control[0] (roll control signal, used as the input) + * u_data: vehicle_torque_setpoint[0] (roll control signal, used as the input) * y_data: vehicle_angular_velocity.xyz[0] (filtered roll angular rate, used as the output) - * t_data: actuator_controls_0.timestamp + * t_data: vehicle_torque_setpoint.timestamp */ #pragma once diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index b6ff54a464..285fc0b620 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include #include @@ -77,8 +76,6 @@ private: */ void parameter_update_poll(); - void publish_actuator_controls(); - void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); void publishThrustSetpoint(const hrt_abstime ×tamp_sample); @@ -88,13 +85,11 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; - uORB::Publication _actuator_controls_0_pub; uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; manual_control_setpoint_s _manual_control_setpoint{}; vehicle_status_s _vehicle_status{}; - actuator_controls_s _actuator_controls{}; perf_counter_t _loop_perf; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 0a4b8bdab3..2a18b349da 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -45,7 +45,6 @@ using namespace matrix; AirshipAttitudeControl::AirshipAttitudeControl() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _actuator_controls_0_pub(ORB_ID(actuator_controls_0)), _loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control")) { } @@ -80,36 +79,18 @@ AirshipAttitudeControl::parameter_update_poll() } } -void -AirshipAttitudeControl::publish_actuator_controls() -{ - // zero actuators if not armed - if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { - for (uint8_t i = 0 ; i < 4 ; i++) { - _actuator_controls.control[i] = 0.0f; - } - - } else { - _actuator_controls.control[0] = 0.0f; - _actuator_controls.control[1] = _manual_control_setpoint.pitch; - _actuator_controls.control[2] = _manual_control_setpoint.yaw; - _actuator_controls.control[3] = (_manual_control_setpoint.throttle + 1.f) * .5f; - } - - // note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run() - _actuator_controls.timestamp = hrt_absolute_time(); - - _actuator_controls_0_pub.publish(_actuator_controls); -} - void AirshipAttitudeControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample) { vehicle_torque_setpoint_s v_torque_sp = {}; v_torque_sp.timestamp = hrt_absolute_time(); v_torque_sp.timestamp_sample = timestamp_sample; - v_torque_sp.xyz[0] = _actuator_controls.control[0]; - v_torque_sp.xyz[1] = _actuator_controls.control[1]; - v_torque_sp.xyz[2] = _actuator_controls.control[2]; + + // zero actuators if not armed + if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + v_torque_sp.xyz[0] = 0.f; + v_torque_sp.xyz[1] = _manual_control_setpoint.pitch; + v_torque_sp.xyz[2] = _manual_control_setpoint.yaw; + } _vehicle_torque_setpoint_pub.publish(v_torque_sp); } @@ -119,9 +100,11 @@ void AirshipAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tamp_ vehicle_thrust_setpoint_s v_thrust_sp = {}; v_thrust_sp.timestamp = hrt_absolute_time(); v_thrust_sp.timestamp_sample = timestamp_sample; - v_thrust_sp.xyz[0] = _actuator_controls.control[3]; - v_thrust_sp.xyz[1] = 0.0f; - v_thrust_sp.xyz[2] = 0.0f; + + // zero actuators if not armed + if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + v_thrust_sp.xyz[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; + } _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); } @@ -142,10 +125,7 @@ AirshipAttitudeControl::Run() if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { - _actuator_controls.timestamp_sample = angular_velocity.timestamp_sample; - /* run the rate controller immediately after a gyro update */ - publish_actuator_controls(); publishTorqueSetpoint(angular_velocity.timestamp_sample); publishThrustSetpoint(angular_velocity.timestamp_sample); @@ -190,8 +170,6 @@ int AirshipAttitudeControl::print_status() perf_print_counter(_loop_perf); - print_message(ORB_ID(actuator_controls_0), _actuator_controls); - return 0; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 4790d3d2f7..f642541fb7 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -53,7 +53,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 2afb41b002..1cf88c67b6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -226,7 +226,7 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry effectiveness(0 + 3, i + actuator_start_index) = 0.f; effectiveness(1 + 3, i + actuator_start_index) = 0.f; - effectiveness(2 + 3, i + actuator_start_index) = ct; + effectiveness(2 + 3, i + actuator_start_index) = -ct; } } diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index ffef587b27..ec274f28da 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -44,7 +44,7 @@ using namespace matrix; FwAutotuneAttitudeControl::FwAutotuneAttitudeControl(bool is_vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _actuator_controls_sub(this, is_vtol ? ORB_ID(actuator_controls_1) : ORB_ID(actuator_controls_0)), + _vehicle_torque_setpoint_sub(this, ORB_ID(vehicle_torque_setpoint), is_vtol ? 1 : 0), _actuator_controls_status_sub(is_vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)) { _autotune_attitude_control_status_pub.advertise(); @@ -65,7 +65,7 @@ bool FwAutotuneAttitudeControl::init() _signal_filter.setParameters(_publishing_dt_s, .2f); // runs in the slow publishing loop - if (!_actuator_controls_sub.registerCallback()) { + if (!_vehicle_torque_setpoint_sub.registerCallback()) { PX4_ERR("callback registration failed"); return false; } @@ -81,7 +81,7 @@ void FwAutotuneAttitudeControl::Run() { if (should_exit()) { _parameter_update_sub.unregisterCallback(); - _actuator_controls_sub.unregisterCallback(); + _vehicle_torque_setpoint_sub.unregisterCallback(); exit_and_cleanup(); return; } @@ -101,7 +101,7 @@ void FwAutotuneAttitudeControl::Run() // new control data needed every iteration if ((_state == state::idle && !_aux_switch_en) - || !_actuator_controls_sub.updated()) { + || !_vehicle_torque_setpoint_sub.updated()) { return; } @@ -123,17 +123,17 @@ void FwAutotuneAttitudeControl::Run() } } - actuator_controls_s controls; + vehicle_torque_setpoint_s vehicle_torque_setpoint; vehicle_angular_velocity_s angular_velocity; - if (!_actuator_controls_sub.copy(&controls) + if (!_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint) || !_vehicle_angular_velocity_sub.copy(&angular_velocity)) { return; } perf_begin(_cycle_perf); - const hrt_abstime timestamp_sample = controls.timestamp; + const hrt_abstime timestamp_sample = vehicle_torque_setpoint.timestamp; // collect sample interval average for filters if (_last_run > 0) { @@ -152,15 +152,15 @@ void FwAutotuneAttitudeControl::Run() checkFilters(); if (_state == state::roll) { - _sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_ROLL], + _sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[0], angular_velocity.xyz[0]); } else if (_state == state::pitch) { - _sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_PITCH], + _sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[1], angular_velocity.xyz[1]); } else if (_state == state::yaw) { - _sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_YAW], + _sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[2], angular_velocity.xyz[2]); } diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp index efe7159973..213518ef72 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp @@ -52,13 +52,13 @@ #include #include #include -#include #include #include #include #include #include #include +#include #include #include @@ -108,7 +108,7 @@ private: const matrix::Vector3f getIdentificationSignal(); - uORB::SubscriptionCallbackWorkItem _actuator_controls_sub; + uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub; uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)}; uORB::Subscription _actuator_controls_status_sub; diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 970d3c77e7..09a3e024d0 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -43,8 +43,9 @@ using math::radians; FixedwingRateControl::FixedwingRateControl(bool vtol) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)), _actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)), + _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_fw) : ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { /* fetch initial parameter values */ @@ -123,24 +124,23 @@ FixedwingRateControl::vehicle_manual_poll() if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { // the controls must always be published in body (hover) frame - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - _manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = - -(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get()); + _vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + + _param_trim_yaw.get(), -1.f, 1.f); + _vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + + _param_trim_roll.get(), -1.f, 1.f); } else { - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - _manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get(); - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = - _manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + _vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + + _param_trim_roll.get(), -1.f, 1.f); + _vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + + _param_trim_yaw.get(), -1.f, 1.f); } - _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = - -_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f; + _vehicle_torque_setpoint.xyz[1] = math::constrain(-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + + _param_trim_pitch.get(), -1.f, 1.f); + _vehicle_thrust_setpoint.xyz[0] = math::constrain((_manual_control_setpoint.throttle + 1.f) * .5f, 0.f, 1.f); } } - } } @@ -350,31 +350,26 @@ void FixedwingRateControl::Run() float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0); float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward; - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - (PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; + _vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1); float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward; - _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = - (PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; + _vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw, - -1.f, 1.f) : trim_yaw; + _vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) { _rate_control.resetIntegral(); } /* throttle passed through if it is finite */ - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = - (PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f; + _vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f; /* scale effort by battery status */ - if (_param_fw_bat_scale_en.get() && - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { + if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) { if (_battery_status_sub.updated()) { battery_status_s battery_status{}; @@ -384,7 +379,7 @@ void FixedwingRateControl::Run() } } - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale; + _vehicle_thrust_setpoint.xyz[0] *= _battery_scale; } } @@ -402,31 +397,28 @@ void FixedwingRateControl::Run() // Add feed-forward from roll control output to yaw control output // This can be used to counteract the adverse yaw effect when rolling the plane - _actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get() - * constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); + _vehicle_torque_setpoint.xyz[2] = math::constrain(_vehicle_torque_setpoint.xyz[2] + _param_fw_rll_to_yaw_ff.get() * + _vehicle_torque_setpoint.xyz[0], -1.f, 1.f); // Tailsitter: rotate back to body frame from airspeed frame if (_vehicle_status.is_vtol_tailsitter) { - const float helper = _actuator_controls.control[actuator_controls_s::INDEX_ROLL]; - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - _actuator_controls.control[actuator_controls_s::INDEX_YAW]; - - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = -helper; + const float helper = _vehicle_torque_setpoint.xyz[0]; + _vehicle_torque_setpoint.xyz[0] = _vehicle_torque_setpoint.xyz[2]; + _vehicle_torque_setpoint.xyz[2] = -helper; } - /* lazily publish the setpoint only once available */ - _actuator_controls.timestamp = hrt_absolute_time(); - _actuator_controls.timestamp_sample = vehicle_angular_velocity.timestamp; - /* Only publish if any of the proper modes are enabled */ if (_vcontrol_mode.flag_control_rates_enabled || _vcontrol_mode.flag_control_attitude_enabled || _vcontrol_mode.flag_control_manual_enabled) { - _actuator_controls_0_pub.publish(_actuator_controls); + { + _vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + _vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint); - if (!_vehicle_status.is_vtol) { - publishTorqueSetpoint(angular_velocity.timestamp_sample); - publishThrustSetpoint(angular_velocity.timestamp_sample); + _vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + _vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint); } } @@ -479,44 +471,14 @@ void FixedwingRateControl::Run() perf_end(_loop_perf); } -void FixedwingRateControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample) -{ - vehicle_torque_setpoint_s v_torque_sp = {}; - v_torque_sp.timestamp = hrt_absolute_time(); - v_torque_sp.timestamp_sample = timestamp_sample; - v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL]; - v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH]; - v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW]; - - _vehicle_torque_setpoint_pub.publish(v_torque_sp); -} - -void FixedwingRateControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) -{ - vehicle_thrust_setpoint_s v_thrust_sp = {}; - v_thrust_sp.timestamp = hrt_absolute_time(); - v_thrust_sp.timestamp_sample = timestamp_sample; - v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; - v_thrust_sp.xyz[1] = 0.0f; - v_thrust_sp.xyz[2] = 0.0f; - - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); -} - void FixedwingRateControl::updateActuatorControlsStatus(float dt) { - for (int i = 0; i < 4; i++) { - float control_signal; + for (int i = 0; i < 3; i++) { - if (i <= actuator_controls_status_s::INDEX_YAW) { - // We assume that the attitude is actuated by control surfaces - // consuming power only when they move - control_signal = _actuator_controls.control[i] - _control_prev[i]; - _control_prev[i] = _actuator_controls.control[i]; - - } else { - control_signal = _actuator_controls.control[i]; - } + // We assume that the attitude is actuated by control surfaces + // consuming power only when they move + const float control_signal = _vehicle_torque_setpoint.xyz[i] - _control_prev[i]; + _control_prev[i] = _vehicle_torque_setpoint.xyz[i]; _control_energy[i] += control_signal * control_signal * dt; } @@ -526,9 +488,9 @@ void FixedwingRateControl::updateActuatorControlsStatus(float dt) if (_energy_integration_time > 500e-3f) { actuator_controls_status_s status; - status.timestamp = _actuator_controls.timestamp; + status.timestamp = _vehicle_torque_setpoint.timestamp; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 3; i++) { status.control_power[i] = _control_energy[i] / _energy_integration_time; _control_energy[i] = 0.f; } diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index d24edd0d84..a6ceaabf00 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -53,7 +53,6 @@ #include #include #include -#include #include #include #include @@ -98,9 +97,6 @@ public: private: void Run() override; - void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); - void publishThrustSetpoint(const hrt_abstime ×tamp_sample); - uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -117,18 +113,18 @@ private: uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; - uORB::Publication _actuator_controls_0_pub; uORB::Publication _actuator_controls_status_pub; uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; - uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; - uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub; + uORB::Publication _vehicle_thrust_setpoint_pub; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; - actuator_controls_s _actuator_controls{}; manual_control_setpoint_s _manual_control_setpoint{}; vehicle_control_mode_s _vcontrol_mode{}; + vehicle_thrust_setpoint_s _vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s _vehicle_torque_setpoint{}; vehicle_rates_setpoint_s _rates_sp{}; vehicle_status_s _vehicle_status{}; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 9786a48f46..95fba63f10 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -86,10 +86,10 @@ MulticopterLandDetector::MulticopterLandDetector() void MulticopterLandDetector::_update_topics() { - actuator_controls_s actuator_controls; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; - if (_actuator_controls_sub.update(&actuator_controls)) { - _actuator_controls_throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) { + _vehicle_thrust_setpoint_throttle = -vehicle_thrust_setpoint.xyz[2]; } vehicle_control_mode_s vehicle_control_mode; @@ -207,7 +207,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() // low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f; const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover; - _has_low_throttle = (_actuator_controls_throttle <= sys_low_throttle); + _has_low_throttle = (_vehicle_thrust_setpoint_throttle <= sys_low_throttle); bool ground_contact = _has_low_throttle; // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, @@ -256,7 +256,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() minimum_thrust_threshold = (_params.minManThrottle + 0.01f); } - const bool minimum_thrust_now = _actuator_controls_throttle <= minimum_thrust_threshold; + const bool minimum_thrust_now = _vehicle_thrust_setpoint_throttle <= minimum_thrust_threshold; _minimum_thrust_8s_hysteresis.set_state_and_update(minimum_thrust_now, now); // Next look if vehicle is not rotating (do not consider yaw) diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 785a6ada3f..01535be1dc 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -42,10 +42,10 @@ #pragma once -#include #include #include #include +#include #include #include "LandDetector.h" @@ -105,7 +105,7 @@ private: float crawlSpeed; } _params{}; - uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; @@ -118,7 +118,7 @@ private: bool _flag_control_climb_rate_enabled{false}; bool _hover_thrust_initialized{false}; - float _actuator_controls_throttle{0.f}; + float _vehicle_thrust_setpoint_throttle{0.f}; uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 0545057d8a..c242b5b3a0 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -47,9 +47,6 @@ void LoggedTopics::add_default_topics() { add_topic("action_request"); add_topic("actuator_armed"); - add_topic("actuator_controls_0", 50); - add_topic("actuator_controls_1", 100); - add_topic("actuator_controls_2", 100); add_optional_topic("actuator_controls_status_0", 300); add_topic("airspeed", 1000); add_optional_topic("airspeed_validated", 200); @@ -226,7 +223,7 @@ void LoggedTopics::add_default_topics() // additional control allocation logging add_topic("actuator_motors", 100); add_topic("actuator_servos", 100); - add_topic_multi("vehicle_thrust_setpoint", 20, 1); + add_topic_multi("vehicle_thrust_setpoint", 20, 2); add_topic_multi("vehicle_torque_setpoint", 20, 2); // SYS_HITL: default ground truth logging for simulation @@ -241,10 +238,12 @@ void LoggedTopics::add_default_topics() } #ifdef CONFIG_ARCH_BOARD_PX4_SITL - add_topic("actuator_controls_virtual_fw"); - add_topic("actuator_controls_virtual_mc"); add_topic("fw_virtual_attitude_setpoint"); add_topic("mc_virtual_attitude_setpoint"); + add_optional_topic("vehicle_torque_setpoint_virtual_mc"); + add_optional_topic("vehicle_torque_setpoint_virtual_fw"); + add_optional_topic("vehicle_thrust_setpoint_virtual_mc"); + add_optional_topic("vehicle_thrust_setpoint_virtual_fw"); add_topic("time_offset"); add_topic("vehicle_angular_velocity", 10); add_topic("vehicle_angular_velocity_groundtruth", 10); @@ -380,8 +379,6 @@ void LoggedTopics::add_raw_imu_accel_fifo() void LoggedTopics::add_system_identification_topics() { // for system id need to log imu and controls at full rate - add_topic("actuator_controls_0"); - add_topic("actuator_controls_1"); add_topic("sensor_combined"); add_topic("vehicle_angular_velocity"); add_topic("vehicle_torque_setpoint"); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f239b2b4ef..f179d76885 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -61,7 +61,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index ca6bf41231..c96d0b6cb9 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -39,7 +39,6 @@ #include #include -#include #include #include #include @@ -51,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -561,16 +561,16 @@ private: if (_status_sub.update(&status)) { if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - actuator_controls_s actuator{}; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; if (status.is_vtol && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_actuator_1_sub.copy(&actuator)) { - _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered); + if (_vehicle_thrust_setpoint_1_sub.copy(&vehicle_thrust_setpoint)) { + _throttle.add_value(vehicle_thrust_setpoint.xyz[0], _update_rate_filtered); } } else { - if (_actuator_0_sub.copy(&actuator)) { - _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered); + if (_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint)) { + _throttle.add_value(-vehicle_thrust_setpoint.xyz[2], _update_rate_filtered); } } @@ -621,8 +621,8 @@ private: msg.wp_num = UINT16_MAX; } - uORB::Subscription _actuator_0_sub{ORB_ID(actuator_controls_0)}; - uORB::Subscription _actuator_1_sub{ORB_ID(actuator_controls_1)}; + uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0}; + uORB::Subscription _vehicle_thrust_setpoint_1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index b5a038ab5d..66df116a1e 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -35,10 +35,10 @@ #define VFR_HUD_HPP #include -#include #include #include #include +#include class MavlinkStreamVFRHUD : public MavlinkStream { @@ -65,8 +65,8 @@ private: uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; - uORB::Subscription _act0_sub{ORB_ID(actuator_controls_0)}; - uORB::Subscription _act1_sub{ORB_ID(actuator_controls_1)}; + uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0}; + uORB::Subscription _vehicle_thrust_setpoint_1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; @@ -89,19 +89,19 @@ private: msg.heading = math::degrees(matrix::wrap_2pi(lpos.heading)); if (armed.armed) { - actuator_controls_s act0{}; - actuator_controls_s act1{}; - _act0_sub.copy(&act0); - _act1_sub.copy(&act1); + vehicle_thrust_setpoint_s vehicle_thrust_setpoint_0{}; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint_1{}; + _vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint_0); + _vehicle_thrust_setpoint_1_sub.copy(&vehicle_thrust_setpoint_1); // VFR_HUD throttle should only be used for operator feedback. - // VTOLs switch between actuator_controls_0 and actuator_controls_1. During transition there isn't a + // VTOLs switch between vehicle_thrust_setpoint_0 and vehicle_thrust_setpoint_1. During transition there isn't a // a single throttle value, but this should still be a useful heuristic for operator awareness. // // Use ACTUATOR_CONTROL_TARGET if accurate states are needed. msg.throttle = 100 * math::max( - act0.control[actuator_controls_s::INDEX_THROTTLE], - act1.control[actuator_controls_s::INDEX_THROTTLE]); + -vehicle_thrust_setpoint_0.xyz[2], + vehicle_thrust_setpoint_1.xyz[0]); } else { msg.throttle = 0.0f; diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp index 2a982c36c5..749186ee8d 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -74,7 +74,7 @@ void McAutotuneAttitudeControl::Run() { if (should_exit()) { _parameter_update_sub.unregisterCallback(); - _actuator_controls_sub.unregisterCallback(); + _vehicle_torque_setpoint_sub.unregisterCallback(); exit_and_cleanup(); return; } @@ -92,7 +92,7 @@ void McAutotuneAttitudeControl::Run() // new control data needed every iteration if (_state == state::idle - || !_actuator_controls_sub.updated()) { + || !_vehicle_torque_setpoint_sub.updated()) { return; } @@ -112,17 +112,17 @@ void McAutotuneAttitudeControl::Run() } } - actuator_controls_s controls; + vehicle_torque_setpoint_s vehicle_torque_setpoint; vehicle_angular_velocity_s angular_velocity; - if (!_actuator_controls_sub.copy(&controls) + if (!_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint) || !_vehicle_angular_velocity_sub.copy(&angular_velocity)) { return; } perf_begin(_cycle_perf); - const hrt_abstime timestamp_sample = controls.timestamp; + const hrt_abstime timestamp_sample = vehicle_torque_setpoint.timestamp; // collect sample interval average for filters if (_last_run > 0) { @@ -142,15 +142,15 @@ void McAutotuneAttitudeControl::Run() // Send data to the filters at maximum frequency if (_state == state::roll) { - _sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_ROLL], + _sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[0], angular_velocity.xyz[0]); } else if (_state == state::pitch) { - _sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_PITCH], + _sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[1], angular_velocity.xyz[1]); } else if (_state == state::yaw) { - _sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_YAW], + _sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[2], angular_velocity.xyz[2]); } @@ -498,7 +498,7 @@ void McAutotuneAttitudeControl::revertParamGains() bool McAutotuneAttitudeControl::registerActuatorControlsCallback() { - if (!_actuator_controls_sub.registerCallback()) { + if (!_vehicle_torque_setpoint_sub.registerCallback()) { PX4_ERR("callback registration failed"); return false; } @@ -581,7 +581,7 @@ void McAutotuneAttitudeControl::stopAutotune() { _param_mc_at_start.set(false); _param_mc_at_start.commit(); - _actuator_controls_sub.unregisterCallback(); + _vehicle_torque_setpoint_sub.unregisterCallback(); } const Vector3f McAutotuneAttitudeControl::getIdentificationSignal() diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp index 3c5218bd19..4307edfcd1 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp @@ -51,13 +51,13 @@ #include #include #include -#include #include #include #include #include #include #include +#include #include using namespace time_literals; @@ -102,7 +102,7 @@ private: const matrix::Vector3f getIdentificationSignal(); - uORB::SubscriptionCallbackWorkItem _actuator_controls_sub{this, ORB_ID(actuator_controls_0)}; + uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)}; uORB::Subscription _actuator_controls_status_sub{ORB_ID(actuator_controls_status_0)}; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 3a3219d3f3..d580dcf8ae 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -46,7 +46,8 @@ using math::radians; MulticopterRateControl::MulticopterRateControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)), + _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc) : ORB_ID(vehicle_thrust_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; @@ -221,21 +222,16 @@ MulticopterRateControl::Run() rate_ctrl_status.timestamp = hrt_absolute_time(); _controller_status_pub.publish(rate_ctrl_status); - // publish actuator controls - actuator_controls_s actuators{}; - actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f; - actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f; - actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f; - actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint( - 2) : 0.0f; - actuators.timestamp_sample = angular_velocity.timestamp_sample; + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; - if (!_vehicle_status.is_vtol) { - publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample); - publishThrustSetpoint(angular_velocity.timestamp_sample); - } + _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 effort by battery status if enabled + // scale setpoints by battery status if enabled if (_param_mc_bat_scale_en.get()) { if (_battery_status_sub.updated()) { battery_status_s battery_status; @@ -245,55 +241,35 @@ MulticopterRateControl::Run() } } - if (_battery_status_scale > 0.0f) { - for (int i = 0; i < 4; i++) { - actuators.control[i] *= _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); } } } - actuators.timestamp = hrt_absolute_time(); - _actuator_controls_0_pub.publish(actuators); + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); - updateActuatorControlsStatus(actuators, dt); + 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); - } else if (_vehicle_control_mode.flag_control_termination_enabled) { - if (!_vehicle_status.is_vtol) { - // publish actuator controls - actuator_controls_s actuators{}; - actuators.timestamp = hrt_absolute_time(); - _actuator_controls_0_pub.publish(actuators); - } } } perf_end(_loop_perf); } -void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime ×tamp_sample) +void MulticopterRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, + float dt) { - vehicle_torque_setpoint_s vehicle_torque_setpoint{}; - vehicle_torque_setpoint.timestamp_sample = timestamp_sample; - vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; - vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; - vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; - vehicle_torque_setpoint.timestamp = hrt_absolute_time(); - _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); -} - -void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) -{ - vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - vehicle_thrust_setpoint.timestamp_sample = timestamp_sample; - _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); - vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); - _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); -} - -void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt) -{ - for (int i = 0; i < 4; i++) { - _control_energy[i] += actuators.control[i] * actuators.control[i] * 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; @@ -301,14 +277,14 @@ void MulticopterRateControl::updateActuatorControlsStatus(const actuator_control if (_energy_integration_time > 500e-3f) { actuator_controls_status_s status; - status.timestamp = actuators.timestamp; + status.timestamp = vehicle_torque_setpoint.timestamp; - for (int i = 0; i < 4; i++) { + 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_0_pub.publish(status); + _actuator_controls_status_pub.publish(status); _energy_integration_time = 0.f; } } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index ab1aa7926f..0f402c12c3 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -88,10 +87,7 @@ private: */ void parameters_updated(); - void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt); - - void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime ×tamp_sample); - void publishThrustSetpoint(const hrt_abstime ×tamp_sample); + void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt); RateControl _rate_control; ///< class for rate control calculations @@ -107,12 +103,11 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; - uORB::Publication _actuator_controls_0_pub; - uORB::Publication _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)}; + uORB::Publication _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)}; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; - uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub; + uORB::Publication _vehicle_thrust_setpoint_pub; vehicle_control_mode_s _vehicle_control_mode{}; vehicle_status_s _vehicle_status{}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 3540cc44f7..f368b357f4 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 1a62643041..705d07c246 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/payload_deliverer/payload_deliverer.h b/src/modules/payload_deliverer/payload_deliverer.h index bfe7eb2971..39b4cf4a15 100644 --- a/src/modules/payload_deliverer/payload_deliverer.h +++ b/src/modules/payload_deliverer/payload_deliverer.h @@ -48,7 +48,6 @@ #include #include -#include #include using namespace time_literals; diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 49c7efb8ef..309c67af06 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -53,7 +53,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 861603b6e7..9a0a7d2c20 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -149,13 +149,10 @@ RoverPositionControl::manual_control_setpoint_poll() _attitude_sp_pub.publish(_att_sp); } else { - _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.roll; - _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.pitch; // Set heading from the manual roll input channel - _act_controls.control[actuator_controls_s::INDEX_YAW] = - _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw; + _yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw; // Set throttle from the manual throttle channel - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f; + _throttle_control = (_manual_control_setpoint.throttle + 1.f) * .5f; _reset_yaw_sp = true; } @@ -276,21 +273,21 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, prev_wp(1)); _gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d); - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle; + _throttle_control = mission_throttle; float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand()); float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get()); float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign( _gnd_control.nav_lateral_acceleration_demand()); control_effort = math::constrain(control_effort, -1.0f, 1.0f); - _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + _yaw_control = control_effort; } } break; case STOPPING: { - _act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f; - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + _yaw_control = 0.0f; + _throttle_control = 0.0f; // Note _prev_wp is different to the local prev_wp which is related to a mission waypoint. float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1), (double)curr_wp(0), (double)curr_wp(1)); @@ -336,7 +333,7 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt); //Constrain maximum throttle to mission throttle - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, mission_throttle); + _throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle); Vector3f desired_body_velocity; @@ -352,12 +349,12 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) float control_effort = desired_theta / _param_max_turn_angle.get(); control_effort = math::constrain(control_effort, -1.0f, 1.0f); - _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + _yaw_control = control_effort; } else { - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; - _act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f; + _throttle_control = 0.0f; + _yaw_control = 0.0f; } } @@ -371,11 +368,11 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi float control_effort = euler_sp(2) / _param_max_turn_angle.get(); control_effort = math::constrain(control_effort, -1.0f, 1.0f); - _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + _yaw_control = control_effort; const float control_throttle = att_sp.thrust_body[0]; - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f); + _throttle_control = math::constrain(control_throttle, 0.0f, 1.0f); } @@ -481,22 +478,19 @@ RoverPositionControl::Run() _control_mode.flag_control_attitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_manual_enabled) { - // timestamp and publish controls - _act_controls.timestamp = hrt_absolute_time(); - _actuator_controls_pub.publish(_act_controls); vehicle_thrust_setpoint_s v_thrust_sp{}; v_thrust_sp.timestamp = hrt_absolute_time(); - v_thrust_sp.xyz[0] = _act_controls.control[actuator_controls_s::INDEX_THROTTLE]; + v_thrust_sp.xyz[0] = _throttle_control; v_thrust_sp.xyz[1] = 0.0f; v_thrust_sp.xyz[2] = 0.0f; _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); vehicle_torque_setpoint_s v_torque_sp{}; v_torque_sp.timestamp = hrt_absolute_time(); - v_torque_sp.xyz[0] = _act_controls.control[actuator_controls_s::INDEX_ROLL]; - v_torque_sp.xyz[1] = _act_controls.control[actuator_controls_s::INDEX_PITCH]; - v_torque_sp.xyz[2] = _act_controls.control[actuator_controls_s::INDEX_YAW]; + v_torque_sp.xyz[0] = 0.f; + v_torque_sp.xyz[1] = 0.f; + v_torque_sp.xyz[2] = _yaw_control; _vehicle_torque_setpoint_pub.publish(v_torque_sp); } } @@ -541,7 +535,7 @@ int RoverPositionControl::print_usage(const char *reason) ### Description Controls the position of a ground rover using an L1 controller. -Publishes `actuator_controls_0` messages at IMU_GYRO_RATEMAX. +Publishes `vehicle_thrust_setpoint (only in x) and vehicle_torque_setpoint (only yaw)` messages at IMU_GYRO_RATEMAX. ### Implementation Currently, this implementation supports only a few modes: diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 31614efe74..ee1c65efa0 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -72,7 +72,6 @@ #include #include #include -#include #include #include @@ -108,7 +107,6 @@ private: uORB::Publication _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */ - uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */ uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< control mode subscription */ uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; @@ -125,7 +123,6 @@ private: vehicle_control_mode_s _control_mode{}; /**< control mode */ vehicle_global_position_s _global_pos{}; /**< global vehicle position */ vehicle_local_position_s _local_pos{}; /**< global vehicle position */ - actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; trajectory_setpoint_s _trajectory_setpoint{}; uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; @@ -170,6 +167,8 @@ private: float _manual_yaw_sp{0.0}; bool _reset_yaw_sp{true}; + float _throttle_control{0.f}; + float _yaw_control{0.f}; DEFINE_PARAMETERS( (ParamFloat) _param_l1_period, diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 374f2025e8..8ee9380f30 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -364,11 +364,13 @@ void VehicleMagnetometer::UpdatePowerCompensation() if (_mag_comp_type != MagCompensationType::Disabled) { // update power signal for mag compensation if (_armed && (_mag_comp_type == MagCompensationType::Throttle)) { - actuator_controls_s controls; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + + if (_vehicle_thrust_setpoint_0_sub.update(&vehicle_thrust_setpoint)) { + const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz); - if (_actuator_controls_0_sub.update(&controls)) { for (auto &cal : _calibration) { - cal.UpdatePower(controls.control[actuator_controls_s::INDEX_THROTTLE]); + cal.UpdatePower(thrust_setpoint.length()); } } diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index 0fefd32dee..042a401476 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include @@ -60,6 +59,7 @@ #include #include #include +#include using namespace time_literals; @@ -110,7 +110,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0}; uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; uORB::Subscription _magnetometer_bias_estimate_sub{ORB_ID(magnetometer_bias_estimate)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index b87f6ab508..dd985c2788 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -95,34 +95,34 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u { if (PX4_ISFINITE(roll_u)) { roll_u = math::constrain(roll_u, -1.0f, 1.0f); - _actuators.control[actuator_controls_s::INDEX_ROLL] = roll_u; + _vehicle_torque_setpoint.xyz[0] = roll_u; } else { - _actuators.control[actuator_controls_s::INDEX_ROLL] = 0.0f; + _vehicle_torque_setpoint.xyz[0] = 0.0f; } if (PX4_ISFINITE(pitch_u)) { pitch_u = math::constrain(pitch_u, -1.0f, 1.0f); - _actuators.control[actuator_controls_s::INDEX_PITCH] = pitch_u; + _vehicle_torque_setpoint.xyz[1] = pitch_u; } else { - _actuators.control[actuator_controls_s::INDEX_PITCH] = 0.0f; + _vehicle_torque_setpoint.xyz[1] = 0.0f; } if (PX4_ISFINITE(yaw_u)) { yaw_u = math::constrain(yaw_u, -1.0f, 1.0f); - _actuators.control[actuator_controls_s::INDEX_YAW] = yaw_u; + _vehicle_torque_setpoint.xyz[2] = yaw_u; } else { - _actuators.control[actuator_controls_s::INDEX_YAW] = 0.0f; + _vehicle_torque_setpoint.xyz[2] = 0.0f; } if (PX4_ISFINITE(thrust_x)) { thrust_x = math::constrain(thrust_x, -1.0f, 1.0f); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = thrust_x; + _vehicle_thrust_setpoint.xyz[0] = thrust_x; } else { - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + _vehicle_thrust_setpoint.xyz[0] = 0.0f; } } @@ -265,48 +265,24 @@ void UUVAttitudeControl::Run() _manual_control_setpoint.yaw, _manual_control_setpoint.throttle, 0.f, 0.f); } - } - _actuators.timestamp = hrt_absolute_time(); - /* Only publish if any of the proper modes are enabled */ if (_vcontrol_mode.flag_control_manual_enabled || _vcontrol_mode.flag_control_attitude_enabled) { - /* publish the actuator controls */ - _actuator_controls_pub.publish(_actuators); - publishTorqueSetpoint(0); - publishThrustSetpoint(0); + + _vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint.timestamp_sample = 0.f; + _vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint); + + _vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint.timestamp_sample = 0.f; + _vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint); } perf_end(_loop_perf); } -void UUVAttitudeControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample) -{ - vehicle_torque_setpoint_s v_torque_sp = {}; - v_torque_sp.timestamp = hrt_absolute_time(); - v_torque_sp.timestamp_sample = timestamp_sample; - v_torque_sp.xyz[0] = _actuators.control[actuator_controls_s::INDEX_ROLL]; - v_torque_sp.xyz[1] = _actuators.control[actuator_controls_s::INDEX_PITCH]; - v_torque_sp.xyz[2] = _actuators.control[actuator_controls_s::INDEX_YAW]; - - _vehicle_torque_setpoint_pub.publish(v_torque_sp); -} - -void UUVAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) -{ - vehicle_thrust_setpoint_s v_thrust_sp = {}; - v_thrust_sp.timestamp = hrt_absolute_time(); - v_thrust_sp.timestamp_sample = timestamp_sample; - v_thrust_sp.xyz[0] = _actuators.control[actuator_controls_s::INDEX_THROTTLE]; - v_thrust_sp.xyz[1] = 0.0f; - v_thrust_sp.xyz[2] = 0.0f; - - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); -} - - int UUVAttitudeControl::task_spawn(int argc, char *argv[]) { UUVAttitudeControl *instance = new UUVAttitudeControl(); @@ -347,7 +323,7 @@ int UUVAttitudeControl::print_usage(const char *reason) ### Description Controls the attitude of an unmanned underwater vehicle (UUV). -Publishes `actuator_controls_0` messages at a constant 250Hz. +Publishes `vehicle_thrust_setpont` and `vehicle_torque_setpoint` messages at a constant 250Hz. ### Implementation Currently, this implementation supports only a few modes: diff --git a/src/modules/uuv_att_control/uuv_att_control.hpp b/src/modules/uuv_att_control/uuv_att_control.hpp index 4a6cd0dba0..d04f21cb11 100644 --- a/src/modules/uuv_att_control/uuv_att_control.hpp +++ b/src/modules/uuv_att_control/uuv_att_control.hpp @@ -68,7 +68,6 @@ #include #include #include -#include #include #include #include @@ -103,7 +102,6 @@ private: void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); void publishThrustSetpoint(const hrt_abstime ×tamp_sample); - uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; @@ -117,7 +115,8 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; - actuator_controls_s _actuators{}; + vehicle_thrust_setpoint_s _vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s _vehicle_torque_setpoint{}; manual_control_setpoint_s _manual_control_setpoint{}; vehicle_attitude_setpoint_s _attitude_setpoint{}; vehicle_rates_setpoint_s _rates_setpoint{}; diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index 1f9a385187..d402941b4a 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.cpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -242,7 +242,7 @@ int UUVPOSControl::print_usage(const char *reason) R"DESCR_STR( ### Description Controls the attitude of an unmanned underwater vehicle (UUV). -Publishes `actuator_controls_0` messages at a constant 250Hz. +Publishes `attitude_setpoint` messages. ### Implementation Currently, this implementation supports only a few modes: * Full manual: Roll, pitch, yaw, and throttle controls are passed directly through to the actuators diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp index c9a2f253c2..4d94ac9cd9 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.hpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -68,7 +68,6 @@ #include #include #include -#include #include using matrix::Eulerf; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 296975d521..95d2821562 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -273,91 +273,75 @@ void Standard::update_fw_state() */ void Standard::fill_actuator_outputs() { - auto &mc_in = _actuators_mc_in->control; - auto &fw_in = _actuators_fw_in->control; + _torque_setpoint_0->timestamp = hrt_absolute_time(); + _torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample; + _torque_setpoint_0->xyz[0] = 0.f; + _torque_setpoint_0->xyz[1] = 0.f; + _torque_setpoint_0->xyz[2] = 0.f; - auto &mc_out = _actuators_out_0->control; - auto &fw_out = _actuators_out_1->control; + _torque_setpoint_1->timestamp = hrt_absolute_time(); + _torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample; + _torque_setpoint_1->xyz[0] = 0.f; + _torque_setpoint_1->xyz[1] = 0.f; + _torque_setpoint_1->xyz[2] = 0.f; + + _thrust_setpoint_0->timestamp = hrt_absolute_time(); + _thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample; + _thrust_setpoint_0->xyz[0] = 0.f; + _thrust_setpoint_0->xyz[1] = 0.f; + _thrust_setpoint_0->xyz[2] = 0.f; + + _thrust_setpoint_1->timestamp = hrt_absolute_time(); + _thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample; + _thrust_setpoint_1->xyz[0] = 0.f; + _thrust_setpoint_1->xyz[1] = 0.f; + _thrust_setpoint_1->xyz[2] = 0.f; switch (_vtol_mode) { case vtol_mode::MC_MODE: - // MC out = MC in - mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL]; - mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH]; - mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW]; - mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; + // MC actuators: + _torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0]; + _torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1]; + _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2]; + _thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2]; - // FW out = 0, other than roll and pitch depending on elevon lock - fw_out[actuator_controls_s::INDEX_ROLL] = _param_vt_elev_mc_lock.get() ? 0 : - fw_in[actuator_controls_s::INDEX_ROLL]; - fw_out[actuator_controls_s::INDEX_PITCH] = _param_vt_elev_mc_lock.get() ? 0 : - fw_in[actuator_controls_s::INDEX_PITCH]; - fw_out[actuator_controls_s::INDEX_YAW] = 0; - fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; + // FW actuators: + if (!_param_vt_elev_mc_lock.get()) { + _torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0]; + _torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1]; + } + + _thrust_setpoint_0->xyz[0] = _pusher_throttle; break; case vtol_mode::TRANSITION_TO_FW: // FALLTHROUGH case vtol_mode::TRANSITION_TO_MC: - // MC out = MC in (weighted) - mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; - mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; + // MC actuators: + _torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0] * _mc_roll_weight; + _torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1] * _mc_pitch_weight; + _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2] * _mc_yaw_weight; + _thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2] * _mc_throttle_weight; - // FW out = FW in, with VTOL transition controlling throttle and airbrakes - fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL] * (1.f - _mc_roll_weight); - fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH] * (1.f - _mc_pitch_weight); - fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW] * (1.f - _mc_yaw_weight); - fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; + // FW actuators + _torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0] * (1.f - _mc_roll_weight); + _torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1] * (1.f - _mc_pitch_weight); + _torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * (1.f - _mc_yaw_weight); + _thrust_setpoint_0->xyz[0] = _pusher_throttle; break; case vtol_mode::FW_MODE: - // MC out = 0 - mc_out[actuator_controls_s::INDEX_ROLL] = 0; - mc_out[actuator_controls_s::INDEX_PITCH] = 0; - mc_out[actuator_controls_s::INDEX_YAW] = 0; - mc_out[actuator_controls_s::INDEX_THROTTLE] = 0; - // FW out = FW in - fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; - fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; - fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW]; - fw_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + // FW actuators + _torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0]; + _torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1]; + _torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2]; + _thrust_setpoint_0->xyz[0] = _vehicle_thrust_setpoint_virtual_fw->xyz[0]; break; } - - _torque_setpoint_0->timestamp = hrt_absolute_time(); - _torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; - _torque_setpoint_0->xyz[0] = mc_out[actuator_controls_s::INDEX_ROLL]; - _torque_setpoint_0->xyz[1] = mc_out[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_0->xyz[2] = mc_out[actuator_controls_s::INDEX_YAW]; - - _torque_setpoint_1->timestamp = hrt_absolute_time(); - _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - _torque_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL]; - _torque_setpoint_1->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_1->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW]; - - _thrust_setpoint_0->timestamp = hrt_absolute_time(); - _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; - _thrust_setpoint_0->xyz[0] = fw_out[actuator_controls_s::INDEX_THROTTLE]; - _thrust_setpoint_0->xyz[1] = 0.f; - _thrust_setpoint_0->xyz[2] = -mc_out[actuator_controls_s::INDEX_THROTTLE]; - - _thrust_setpoint_1->timestamp = hrt_absolute_time(); - _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - _thrust_setpoint_1->xyz[0] = 0.f; - _thrust_setpoint_1->xyz[1] = 0.f; - _thrust_setpoint_1->xyz[2] = 0.f; - - _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; - _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - - _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); } void diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 4ba2c73d2a..7c19d6cccb 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -257,92 +257,62 @@ void Tailsitter::update_fw_state() */ void Tailsitter::fill_actuator_outputs() { - auto &mc_in = _actuators_mc_in->control; - auto &fw_in = _actuators_fw_in->control; - - auto &mc_out = _actuators_out_0->control; - auto &fw_out = _actuators_out_1->control; - _torque_setpoint_0->timestamp = hrt_absolute_time(); - _torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample; _torque_setpoint_0->xyz[0] = 0.f; _torque_setpoint_0->xyz[1] = 0.f; _torque_setpoint_0->xyz[2] = 0.f; _torque_setpoint_1->timestamp = hrt_absolute_time(); - _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample; _torque_setpoint_1->xyz[0] = 0.f; _torque_setpoint_1->xyz[1] = 0.f; _torque_setpoint_1->xyz[2] = 0.f; _thrust_setpoint_0->timestamp = hrt_absolute_time(); - _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample; _thrust_setpoint_0->xyz[0] = 0.f; _thrust_setpoint_0->xyz[1] = 0.f; _thrust_setpoint_0->xyz[2] = 0.f; _thrust_setpoint_1->timestamp = hrt_absolute_time(); - _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample; _thrust_setpoint_1->xyz[0] = 0.f; _thrust_setpoint_1->xyz[1] = 0.f; _thrust_setpoint_1->xyz[2] = 0.f; - - mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL]; - mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH]; - mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW]; - + // Motors if (_vtol_mode == vtol_mode::FW_MODE) { - mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; - // FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation - _thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE]; + _thrust_setpoint_0->xyz[2] = -_vehicle_thrust_setpoint_virtual_fw->xyz[0]; /* allow differential thrust if enabled */ if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { - float yaw_control = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get(); - mc_out[actuator_controls_s::INDEX_ROLL] = yaw_control; - _torque_setpoint_0->xyz[0] = yaw_control; + _torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get(); } if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::PITCH_BIT)) { - float pitch_control = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get(); - mc_out[actuator_controls_s::INDEX_PITCH] = pitch_control; - _torque_setpoint_0->xyz[1] = pitch_control; + _torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1] * _param_vt_fw_difthr_s_p.get(); } if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::ROLL_BIT)) { - float roll_control = -fw_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get(); - mc_out[actuator_controls_s::INDEX_YAW] = roll_control; - _torque_setpoint_0->xyz[2] = roll_control; + _torque_setpoint_0->xyz[2] = -_vehicle_torque_setpoint_virtual_fw->xyz[0] * _param_vt_fw_difthr_s_r.get(); } } else { - _torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL]; - _torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW]; + _torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0]; + _torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1]; + _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2]; - mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; - _thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE]; + _thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2]; } - if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) { - fw_out[actuator_controls_s::INDEX_ROLL] = 0; - fw_out[actuator_controls_s::INDEX_PITCH] = 0; - - } else { - fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; - fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; - - _torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL]; - _torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW]; + // Control surfaces + if (!_param_vt_elev_mc_lock.get() || _vtol_mode != vtol_mode::MC_MODE) { + _torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0]; + _torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1]; + _torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2]; } - - _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; - _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - - _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index a1a3599c9d..0285e61ea6 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -374,92 +374,67 @@ void Tiltrotor::waiting_on_tecs() _v_att_sp->thrust_body[0] = _thrust_transition; } -/** -* Write data to actuator output topic. -*/ void Tiltrotor::fill_actuator_outputs() { - auto &mc_in = _actuators_mc_in->control; - auto &fw_in = _actuators_fw_in->control; - - auto &mc_out = _actuators_out_0->control; - auto &fw_out = _actuators_out_1->control; _torque_setpoint_0->timestamp = hrt_absolute_time(); - _torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample; _torque_setpoint_0->xyz[0] = 0.f; _torque_setpoint_0->xyz[1] = 0.f; _torque_setpoint_0->xyz[2] = 0.f; _torque_setpoint_1->timestamp = hrt_absolute_time(); - _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample; _torque_setpoint_1->xyz[0] = 0.f; _torque_setpoint_1->xyz[1] = 0.f; _torque_setpoint_1->xyz[2] = 0.f; _thrust_setpoint_0->timestamp = hrt_absolute_time(); - _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample; _thrust_setpoint_0->xyz[0] = 0.f; _thrust_setpoint_0->xyz[1] = 0.f; _thrust_setpoint_0->xyz[2] = 0.f; _thrust_setpoint_1->timestamp = hrt_absolute_time(); - _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample; _thrust_setpoint_1->xyz[0] = 0.f; _thrust_setpoint_1->xyz[1] = 0.f; _thrust_setpoint_1->xyz[2] = 0.f; // Multirotor output - mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; - mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - - _torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; - _torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - _torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + _torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0] * _mc_roll_weight; + _torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1] * _mc_pitch_weight; + _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2] * _mc_yaw_weight; // Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC), - // pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they + // pass the vector magnitude and collective tilt separately. MC also needs collective thrust on z. + // Passing 3D thrust plus tilt is not feasible as they // can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated // by the allocator directly. float collective_thrust_normalized_setpoint = 0.f; if (_vtol_mode == vtol_mode::FW_MODE) { - _thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE]; - collective_thrust_normalized_setpoint = fw_in[actuator_controls_s::INDEX_THROTTLE]; + collective_thrust_normalized_setpoint = _vehicle_thrust_setpoint_virtual_fw->xyz[0]; + _thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint; /* allow differential thrust if enabled */ if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { - mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; - _torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; + _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get() ; } } else { - _thrust_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; - collective_thrust_normalized_setpoint = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; + collective_thrust_normalized_setpoint = -_vehicle_thrust_setpoint_virtual_mc->xyz[2] * _mc_throttle_weight; + _thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint; } // Fixed wing output - if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) { - fw_out[actuator_controls_s::INDEX_ROLL] = 0; - fw_out[actuator_controls_s::INDEX_PITCH] = 0; - fw_out[actuator_controls_s::INDEX_YAW] = 0; - - } else { - fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; - fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; - fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW]; - _torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL]; - _torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW]; + if (!_param_vt_elev_mc_lock.get() || _vtol_mode != vtol_mode::MC_MODE) { + _torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0]; + _torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1]; + _torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2]; } - _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; - _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - - _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); - // publish tiltrotor extra controls tiltrotor_extra_controls_s tiltrotor_extra_controls = {}; tiltrotor_extra_controls.collective_tilt_normalized_setpoint = _tilt_control; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index fab819e29f..cef500334e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -94,12 +94,22 @@ VtolAttitudeControl::~VtolAttitudeControl() bool VtolAttitudeControl::init() { - if (!_actuator_inputs_mc.registerCallback()) { + if (!_vehicle_torque_setpoint_virtual_fw_sub.registerCallback()) { PX4_ERR("callback registration failed"); return false; } - if (!_actuator_inputs_fw.registerCallback()) { + if (!_vehicle_torque_setpoint_virtual_mc_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + if (!_vehicle_thrust_setpoint_virtual_fw_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + if (!_vehicle_thrust_setpoint_virtual_mc_sub.registerCallback()) { PX4_ERR("callback registration failed"); return false; } @@ -270,8 +280,10 @@ void VtolAttitudeControl::Run() { if (should_exit()) { - _actuator_inputs_fw.unregisterCallback(); - _actuator_inputs_mc.unregisterCallback(); + _vehicle_torque_setpoint_virtual_fw_sub.unregisterCallback(); + _vehicle_torque_setpoint_virtual_mc_sub.unregisterCallback(); + _vehicle_thrust_setpoint_virtual_fw_sub.unregisterCallback(); + _vehicle_thrust_setpoint_virtual_mc_sub.unregisterCallback(); exit_and_cleanup(); return; } @@ -305,8 +317,10 @@ VtolAttitudeControl::Run() perf_begin(_loop_perf); - const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in); - const bool updated_mc_in = _actuator_inputs_mc.update(&_actuators_mc_in); + const bool updated_fw_in = _vehicle_torque_setpoint_virtual_fw_sub.update(&_vehicle_torque_setpoint_virtual_fw) + || _vehicle_thrust_setpoint_virtual_fw_sub.update(&_vehicle_thrust_setpoint_virtual_fw); + const bool updated_mc_in = _vehicle_torque_setpoint_virtual_mc_sub.update(&_vehicle_torque_setpoint_virtual_mc) + || _vehicle_thrust_setpoint_virtual_mc_sub.update(&_vehicle_thrust_setpoint_virtual_mc); // run on actuator publications corresponding to VTOL mode bool should_run = false; @@ -414,8 +428,6 @@ VtolAttitudeControl::Run() } _vtol_type->fill_actuator_outputs(); - _actuator_controls_0_pub.publish(_actuators_out_0); - _actuator_controls_1_pub.publish(_actuators_out_1); _vehicle_torque_setpoint0_pub.publish(_torque_setpoint_0); _vehicle_torque_setpoint1_pub.publish(_torque_setpoint_1); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index d44b454aee..94044b3384 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -65,7 +65,6 @@ #include #include #include -#include #include #include #include @@ -125,10 +124,11 @@ public: float getAirDensity() const { return _air_density; } - struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} - struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} - struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} - struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct vehicle_torque_setpoint_s *get_vehicle_torque_setpoint_virtual_mc() {return &_vehicle_torque_setpoint_virtual_mc;} + struct vehicle_torque_setpoint_s *get_vehicle_torque_setpoint_virtual_fw() {return &_vehicle_torque_setpoint_virtual_fw;} + struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_mc() {return &_vehicle_thrust_setpoint_virtual_mc;} + struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_fw() {return &_vehicle_thrust_setpoint_virtual_fw;} + struct airspeed_validated_s *get_airspeed() {return &_airspeed_validated;} struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;} struct tecs_status_s *get_tecs_status() {return &_tecs_status;} @@ -149,9 +149,10 @@ public: private: void Run() override; - - uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)}; - uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)}; + uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_virtual_fw_sub{this, ORB_ID(vehicle_torque_setpoint_virtual_fw)}; + uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_virtual_mc_sub{this, ORB_ID(vehicle_torque_setpoint_virtual_mc)}; + uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_virtual_fw_sub{this, ORB_ID(vehicle_thrust_setpoint_virtual_fw)}; + uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_virtual_mc_sub{this, ORB_ID(vehicle_thrust_setpoint_virtual_mc)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -171,8 +172,6 @@ private: uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Publication _actuator_controls_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust) - uORB::Publication _actuator_controls_1_pub{ORB_ID(actuator_controls_1)}; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; uORB::Publication _vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; @@ -188,10 +187,10 @@ private: vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint vehicle_attitude_setpoint_s _mc_virtual_att_sp{}; // virtual mc attitude setpoint - actuator_controls_s _actuators_fw_in{}; //actuator controls from fw_att_control - actuator_controls_s _actuators_mc_in{}; //actuator controls from mc_att_control - actuator_controls_s _actuators_out_0{}; //actuator controls going to the mc mixer - actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons) + vehicle_torque_setpoint_s _vehicle_torque_setpoint_virtual_mc{}; + vehicle_torque_setpoint_s _vehicle_torque_setpoint_virtual_fw{}; + vehicle_thrust_setpoint_s _vehicle_thrust_setpoint_virtual_mc{}; + vehicle_thrust_setpoint_s _vehicle_thrust_setpoint_virtual_fw{}; vehicle_torque_setpoint_s _torque_setpoint_0{}; vehicle_torque_setpoint_s _torque_setpoint_1{}; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 9617dce518..8f467eecb5 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -62,10 +62,10 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _fw_virtual_att_sp = _attc->get_fw_virtual_att_sp(); _v_control_mode = _attc->get_control_mode(); _vtol_vehicle_status = _attc->get_vtol_vehicle_status(); - _actuators_out_0 = _attc->get_actuators_out0(); - _actuators_out_1 = _attc->get_actuators_out1(); - _actuators_mc_in = _attc->get_actuators_mc_in(); - _actuators_fw_in = _attc->get_actuators_fw_in(); + _vehicle_torque_setpoint_virtual_mc = _attc->get_vehicle_torque_setpoint_virtual_mc(); + _vehicle_torque_setpoint_virtual_fw = _attc->get_vehicle_torque_setpoint_virtual_fw(); + _vehicle_thrust_setpoint_virtual_mc = _attc->get_vehicle_thrust_setpoint_virtual_mc(); + _vehicle_thrust_setpoint_virtual_fw = _attc->get_vehicle_thrust_setpoint_virtual_fw(); _torque_setpoint_0 = _attc->get_torque_setpoint_0(); _torque_setpoint_1 = _attc->get_torque_setpoint_1(); _thrust_setpoint_0 = _attc->get_thrust_setpoint_0(); @@ -108,7 +108,7 @@ void VtolType::update_mc_state() void VtolType::update_fw_state() { resetAccelToPitchPitchIntegrator(); - _last_thr_in_fw_mode = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + _last_thr_in_fw_mode = _vehicle_thrust_setpoint_virtual_fw->xyz[0]; // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index ef5e85add5..e522c1a5a3 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -259,10 +259,10 @@ protected: struct vehicle_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode struct vtol_vehicle_status_s *_vtol_vehicle_status; - struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_rate_control - struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control + struct vehicle_torque_setpoint_s *_vehicle_torque_setpoint_virtual_mc; + struct vehicle_torque_setpoint_s *_vehicle_torque_setpoint_virtual_fw; + struct vehicle_thrust_setpoint_s *_vehicle_thrust_setpoint_virtual_mc; + struct vehicle_thrust_setpoint_s *_vehicle_thrust_setpoint_virtual_fw; struct vehicle_local_position_s *_local_pos; struct vehicle_local_position_setpoint_s *_local_pos_sp; struct airspeed_validated_s *_airspeed_validated; // airspeed