From 9b629a9e958ce3006e12dc7c9050ba52265de5ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 9 Feb 2022 13:36:27 +0100 Subject: [PATCH] hitl,sitl,sih: use separate actuator_outputs_sim for SYS_CTRL_ALLOC==1 - removes the need to do type-specific rescaling of pwm to normalized values - allows to run physical output drivers alongside HIL/SIH --- msg/actuator_outputs.msg | 3 + src/drivers/pwm_out_sim/PWMSim.cpp | 37 +++- src/drivers/pwm_out_sim/PWMSim.hpp | 5 + .../mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp | 131 ++++++++------- src/modules/sih/sih.cpp | 39 ++++- src/modules/sih/sih.hpp | 3 +- src/modules/simulator/simulator.h | 5 +- src/modules/simulator/simulator_mavlink.cpp | 159 ++++++++++-------- 8 files changed, 241 insertions(+), 141 deletions(-) diff --git a/msg/actuator_outputs.msg b/msg/actuator_outputs.msg index ff546948d8..c16b73b5d7 100644 --- a/msg/actuator_outputs.msg +++ b/msg/actuator_outputs.msg @@ -3,3 +3,6 @@ uint8 NUM_ACTUATOR_OUTPUTS = 16 uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking uint32 noutputs # valid outputs float32[16] output # output data, in natural output units + +# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1]) +# TOPICS actuator_outputs actuator_outputs_sim diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 41fd48bffa..0cac5ad29f 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -81,9 +81,40 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - // Nothing to do, as we are only interested in the actuator_outputs topic publication. - // That should only be published once we receive actuator_controls (important for lock-step to work correctly) - return num_control_groups_updated > 0; + // Only publish once we receive actuator_controls (important for lock-step to work correctly) + if (num_control_groups_updated > 0) { + actuator_outputs_s actuator_outputs{}; + actuator_outputs.noutputs = num_outputs; + + const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); + + for (int i = 0; i < (int)num_outputs; i++) { + if (outputs[i] != PWM_SIM_DISARMED_MAGIC) { + + OutputFunction function = _mixing_output.outputFunction(i); + bool is_reversible = reversible_outputs & (1u << i); + float output = outputs[i]; + + if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax) + && !is_reversible) { + // Scale non-reversible motors to [0, 1] + actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC); + + } else { + // Scale everything else to [-1, 1] + const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f; + const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f; + actuator_outputs.output[i] = (output - pwm_center) / pwm_delta; + } + } + } + + actuator_outputs.timestamp = hrt_absolute_time(); + _actuator_outputs_sim_pub.publish(actuator_outputs); + return true; + } + + return false; } int diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index e845def2b9..4ad12dd57b 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -43,6 +43,9 @@ #include #include #include +#include +#include +#include #if defined(CONFIG_ARCH_BOARD_PX4_SITL) #define PARAM_PREFIX "PWM_MAIN" @@ -86,5 +89,7 @@ private: MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Publication _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)}; }; diff --git a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp index 6fff47ded6..d5cf88afd5 100644 --- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp +++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -55,11 +55,21 @@ public: } private: - explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) + { + int32_t sys_ctrl_alloc = 0; + param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc); + _use_dynamic_mixing = sys_ctrl_alloc >= 1; + + if (_use_dynamic_mixing) { + _act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; + } + } uORB::Subscription _act_sub{ORB_ID(actuator_outputs)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + bool _use_dynamic_mixing{false}; bool send() override { @@ -69,81 +79,88 @@ private: mavlink_hil_actuator_controls_t msg{}; msg.time_usec = act.timestamp; - static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; + if (_use_dynamic_mixing) { + for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + msg.controls[i] = act.output[i]; + } - unsigned system_type = _mavlink->get_system_type(); + } else { + static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; - /* scale outputs depending on system type */ - if (system_type == MAV_TYPE_QUADROTOR || - system_type == MAV_TYPE_HEXAROTOR || - system_type == MAV_TYPE_OCTOROTOR || - system_type == MAV_TYPE_VTOL_DUOROTOR || - system_type == MAV_TYPE_VTOL_QUADROTOR || - system_type == MAV_TYPE_VTOL_RESERVED2) { + unsigned system_type = _mavlink->get_system_type(); - /* multirotors: set number of rotor outputs depending on type */ + /* scale outputs depending on system type */ + if (system_type == MAV_TYPE_QUADROTOR || + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR || + system_type == MAV_TYPE_VTOL_DUOROTOR || + system_type == MAV_TYPE_VTOL_QUADROTOR || + system_type == MAV_TYPE_VTOL_RESERVED2) { - unsigned n; + /* multirotors: set number of rotor outputs depending on type */ - switch (system_type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + unsigned n; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + switch (system_type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - case MAV_TYPE_VTOL_DUOROTOR: - n = 2; - break; + case MAV_TYPE_HEXAROTOR: + n = 6; + break; - case MAV_TYPE_VTOL_QUADROTOR: - n = 4; - break; + case MAV_TYPE_VTOL_DUOROTOR: + n = 2; + break; - case MAV_TYPE_VTOL_RESERVED2: - n = 8; - break; + case MAV_TYPE_VTOL_QUADROTOR: + n = 4; + break; - default: - n = 8; - break; - } + case MAV_TYPE_VTOL_RESERVED2: + n = 8; + break; - for (unsigned i = 0; i < 16; i++) { - if (act.output[i] > PWM_DEFAULT_MIN / 2) { - if (i < n) { - /* scale PWM out 900..2100 us to 0..1 for rotors */ - msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + default: + n = 8; + break; + } + + for (unsigned i = 0; i < 16; i++) { + if (act.output[i] > PWM_DEFAULT_MIN / 2) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ + msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + + } else { + /* scale PWM out 900..2100 us to -1..1 for other channels */ + msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + } } else { - /* scale PWM out 900..2100 us to -1..1 for other channels */ - msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + /* send 0 when disarmed and for disabled channels */ + msg.controls[i] = 0.0f; } - - } else { - /* send 0 when disarmed and for disabled channels */ - msg.controls[i] = 0.0f; } - } - } else { - /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ - for (unsigned i = 0; i < 16; i++) { - if (act.output[i] > PWM_DEFAULT_MIN / 2) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + } else { + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ + for (unsigned i = 0; i < 16; i++) { + if (act.output[i] > PWM_DEFAULT_MIN / 2) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + } } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + /* set 0 for disabled channels */ + msg.controls[i] = 0.0f; } - - } else { - /* set 0 for disabled channels */ - msg.controls[i] = 0.0f; } } } diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 46e8396607..9a1a2fb96c 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -73,6 +73,10 @@ Sih::Sih() : _dist_snsr_time = task_start; _vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast(0), static_cast(2)); + + if (_sys_ctrl_alloc.get()) { + _actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; + } } Sih::~Sih() @@ -99,6 +103,11 @@ bool Sih::init() void Sih::Run() { + if (should_exit()) { + exit_and_cleanup(); + return; + } + perf_count(_loop_interval_perf); // check for parameter updates @@ -267,14 +276,28 @@ void Sih::read_motors() float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX); if (_actuator_out_sub.update(&actuators_out)) { - for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals - if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS - && i > 3)) { // control surfaces in range [-1,1] - _u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f); - - } else { // throttle signals in range [0,1] - float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f); - _u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau + + if (_sys_ctrl_alloc.get()) { + for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals + if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) { + _u[i] = actuators_out.output[i]; + + } else { + float u_sp = actuators_out.output[i]; + _u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau + } + } + + } else { + for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals + if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS + && i > 3)) { // control surfaces in range [-1,1] + _u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f); + + } else { // throttle signals in range [0,1] + float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f); + _u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau + } } } } diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 705be853c3..6653ed6065 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -303,6 +303,7 @@ private: (ParamFloat) _sih_distance_snsr_max, (ParamFloat) _sih_distance_snsr_override, (ParamFloat) _sih_thrust_tau, - (ParamInt) _sih_vtype + (ParamInt) _sih_vtype, + (ParamBool) _sys_ctrl_alloc ) }; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 1af550a7b7..dbd38fa709 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -131,9 +131,7 @@ public: #endif private: - Simulator() : ModuleParams(nullptr) - { - } + Simulator(); ~Simulator() { @@ -291,6 +289,7 @@ private: float _last_magx{0.0f}; float _last_magy{0.0f}; float _last_magz{0.0f}; + bool _use_dynamic_mixing{false}; #if defined(ENABLE_LOCKSTEP_SCHEDULER) px4::atomic _has_initialized {false}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 8f24197f99..7020502cc8 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -81,6 +81,14 @@ const unsigned mode_flag_custom = 1; using namespace time_literals; +Simulator::Simulator() + : ModuleParams(nullptr) +{ + int32_t sys_ctrl_alloc = 0; + param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc); + _use_dynamic_mixing = sys_ctrl_alloc >= 1; +} + void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg) { memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t)); @@ -91,88 +99,96 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t * int _system_type = _param_mav_type.get(); - /* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust) - all other motors are configured for -1..1 range */ - unsigned pos_thrust_motors_count; - bool is_fixed_wing; - - switch (_system_type) { - case MAV_TYPE_AIRSHIP: - case MAV_TYPE_VTOL_DUOROTOR: - case MAV_TYPE_COAXIAL: - pos_thrust_motors_count = 2; - is_fixed_wing = false; - break; + if (_use_dynamic_mixing) { + if (armed) { + for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + msg->controls[i] = _actuator_outputs.output[i]; + } + } - case MAV_TYPE_TRICOPTER: - pos_thrust_motors_count = 3; - is_fixed_wing = false; - break; + } else { + /* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust) + all other motors are configured for -1..1 range */ + unsigned pos_thrust_motors_count; + bool is_fixed_wing; + + switch (_system_type) { + case MAV_TYPE_AIRSHIP: + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_COAXIAL: + pos_thrust_motors_count = 2; + is_fixed_wing = false; + break; - case MAV_TYPE_QUADROTOR: - case MAV_TYPE_VTOL_QUADROTOR: - case MAV_TYPE_VTOL_TILTROTOR: - pos_thrust_motors_count = 4; - is_fixed_wing = false; - break; + case MAV_TYPE_TRICOPTER: + pos_thrust_motors_count = 3; + is_fixed_wing = false; + break; - case MAV_TYPE_VTOL_RESERVED2: - pos_thrust_motors_count = 5; - is_fixed_wing = false; - break; + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + pos_thrust_motors_count = 4; + is_fixed_wing = false; + break; - case MAV_TYPE_HEXAROTOR: - pos_thrust_motors_count = 6; - is_fixed_wing = false; - break; + case MAV_TYPE_VTOL_RESERVED2: + pos_thrust_motors_count = 5; + is_fixed_wing = false; + break; - case MAV_TYPE_VTOL_RESERVED3: - // this is the tricopter VTOL / quad plane with 3 motors and 2 servos - pos_thrust_motors_count = 3; - is_fixed_wing = false; - break; + case MAV_TYPE_HEXAROTOR: + pos_thrust_motors_count = 6; + is_fixed_wing = false; + break; - case MAV_TYPE_OCTOROTOR: - pos_thrust_motors_count = 8; - is_fixed_wing = false; - break; + case MAV_TYPE_VTOL_RESERVED3: + // this is the tricopter VTOL / quad plane with 3 motors and 2 servos + pos_thrust_motors_count = 3; + is_fixed_wing = false; + break; - case MAV_TYPE_SUBMARINE: - pos_thrust_motors_count = 0; - is_fixed_wing = false; - break; + case MAV_TYPE_OCTOROTOR: + pos_thrust_motors_count = 8; + is_fixed_wing = false; + break; - case MAV_TYPE_FIXED_WING: - pos_thrust_motors_count = 0; - is_fixed_wing = true; - break; + case MAV_TYPE_SUBMARINE: + pos_thrust_motors_count = 0; + is_fixed_wing = false; + break; - default: - pos_thrust_motors_count = 0; - is_fixed_wing = false; - break; - } + case MAV_TYPE_FIXED_WING: + pos_thrust_motors_count = 0; + is_fixed_wing = true; + break; + + default: + pos_thrust_motors_count = 0; + is_fixed_wing = false; + break; + } - for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { - if (!armed) { - /* send 0 when disarmed and for disabled channels */ - msg->controls[i] = 0.0f; + for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + if (!armed) { + /* send 0 when disarmed and for disabled channels */ + msg->controls[i] = 0.0f; - } else if ((is_fixed_wing && i == 4) || - (!is_fixed_wing && i < pos_thrust_motors_count)) { //multirotor, rotor channel - /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ - msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); - msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f); + } else if ((is_fixed_wing && i == 4) || + (!is_fixed_wing && i < pos_thrust_motors_count)) { //multirotor, rotor channel + /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ + msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f); - } else { - const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; - const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2; + } else { + const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; + const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2; - /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */ - msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta; - msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f); + /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */ + msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta; + msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f); + } } - } msg->mode = mode_flag_custom; @@ -694,7 +710,12 @@ void Simulator::send() // Subscribe to topics. // Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS. - _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + if (_use_dynamic_mixing) { + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0); + + } else { + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + } // Before starting, we ought to send a heartbeat to initiate the SITL // simulator to start sending sensor data which will set the time and