diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index d4c80a29092f..14c9350c634b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -60,19 +60,25 @@ param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels # controls in practical scenarios. # Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): -param set-default SIM_GZ_SV_FUNC3 203 -param set-default SIM_GZ_SV_MIN3 0 -param set-default SIM_GZ_SV_MAX3 1000 -param set-default SIM_GZ_SV_DIS3 500 -param set-default SIM_GZ_SV_FAIL3 500 +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_MIN1 0 +param set-default SIM_GZ_SV_MAX1 1000 +param set-default SIM_GZ_SV_DIS1 500 +param set-default SIM_GZ_SV_FAIL1 500 +param set-default CA_SV_CS0_MAXA 90 +param set-default CA_SV_CS0_MINA -90 # Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204): # - on minimum when disarmed or failed: -param set-default SIM_GZ_SV_FUNC4 204 -param set-default SIM_GZ_SV_MIN4 0 -param set-default SIM_GZ_SV_MAX4 1000 -param set-default SIM_GZ_SV_DIS4 500 -param set-default SIM_GZ_SV_FAIL4 500 +param set-default SIM_GZ_SV_FUNC2 202 +param set-default SIM_GZ_SV_MIN2 0 +param set-default SIM_GZ_SV_MAX2 1000 +param set-default SIM_GZ_SV_DIS2 500 +param set-default SIM_GZ_SV_FAIL2 500 +param set-default CA_SV_CS1_MAXA 90 +param set-default CA_SV_CS1_MINA -90 + +param set-default CA_SV_CS_COUNT 2 # Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 9e54c4015909..dddd852a2c74 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -47,5 +47,8 @@ param set-default SIM_GZ_WH_MAX1 200 param set-default SIM_GZ_WH_DIS1 100 # Steering +param set-default CA_SV_CS0_MAXA 30 +param set-default CA_SV_CS0_MINA -30 +param set-default CA_SV_CS_COUNT 1 param set-default SIM_GZ_SV_FUNC1 201 param set-default SIM_GZ_SV_REV 1 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 3d80b49bd901..3691e12a8955 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 3d80b49bd9016d4768d3f9898ed78f957907b3e9 +Subproject commit 3691e12a8955d7bf5c452ede672af96f51439152 diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d085704df403..0aaef15f1973 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -342,6 +342,34 @@ parameters: instance_start: 0 default: 0 + CA_SV_CS${i}_MAXA: + description: + short: Control Surface ${i} Angle at Maximum + long: | + Defines the angle when the servo is at the maximum. + Currently only supported in gz simulation. + type: float + decimal: 0 + num_instances: *max_num_servos + instance_start: 0 + min: -180.0 + max: 180.0 + default: 45.0 + + CA_SV_CS${i}_MINA: + description: + short: Control Surface ${i} Angle at Minimum + long: | + Defines the angle when the servo is at the minimum. + Currently only supported in gz simulation. + type: float + decimal: 0 + num_instances: *max_num_servos + instance_start: 0 + min: -180.0 + max: 180.0 + default: -45.0 + # Tilts CA_SV_TL_COUNT: description: diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 9c7969b2dc1e..edf8c3f4e22b 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -33,6 +33,124 @@ #include "GZMixingInterfaceServo.hpp" +float +GZMixingInterfaceServo::get_tl_angle_max(const size_t index) +{ + switch (index) { + case 0: return _ca_sv_tl_max_a_0.get(); + + case 1: return _ca_sv_tl_max_a_1.get(); + + case 2: return _ca_sv_tl_max_a_2.get(); + + case 3: return _ca_sv_tl_max_a_3.get(); + + default: return 0.f; + } +} + +float +GZMixingInterfaceServo::get_tl_angle_min(const size_t index) +{ + switch (index) { + case 0: return _ca_sv_tl_min_a_0.get(); + + case 1: return _ca_sv_tl_min_a_1.get(); + + case 2: return _ca_sv_tl_min_a_2.get(); + + case 3: return _ca_sv_tl_min_a_3.get(); + + default: return 0.f; + } +} + +float +GZMixingInterfaceServo::get_cs_angle_max(const size_t index) +{ + switch (index) { + case 0: return _ca_sv_cs_max_a_0.get(); + + case 1: return _ca_sv_cs_max_a_1.get(); + + case 2: return _ca_sv_cs_max_a_2.get(); + + case 3: return _ca_sv_cs_max_a_3.get(); + + case 4: return _ca_sv_cs_max_a_4.get(); + + case 5: return _ca_sv_cs_max_a_5.get(); + + case 6: return _ca_sv_cs_max_a_6.get(); + + case 7: return _ca_sv_cs_max_a_7.get(); + + default: return 0.f; + } +} + +float +GZMixingInterfaceServo::get_cs_angle_min(const size_t index) +{ + switch (index) { + case 0: return _ca_sv_cs_min_a_0.get(); + + case 1: return _ca_sv_cs_min_a_1.get(); + + case 2: return _ca_sv_cs_min_a_2.get(); + + case 3: return _ca_sv_cs_min_a_3.get(); + + case 4: return _ca_sv_cs_min_a_4.get(); + + case 5: return _ca_sv_cs_min_a_5.get(); + + case 6: return _ca_sv_cs_min_a_6.get(); + + case 7: return _ca_sv_cs_min_a_7.get(); + + default: return 0.f; + } +} + +int GZMixingInterfaceServo::get_active_output_count() +{ + return _control_surface_count.get() + _tilt_count.get(); +} + +bool GZMixingInterfaceServo::is_tiltrotor(const int index) +{ + if (_tilt_count.get() < 1) { + return false; + } + + return (index > _control_surface_count.get() - 1) && (index < get_active_output_count()); +} + +double GZMixingInterfaceServo::get_angle_max_wrapper(const int index) +{ + if (index >= get_active_output_count()) { + return 0.; + } + + double angle = is_tiltrotor(index) ? (double)get_tl_angle_max(index - _control_surface_count.get()) : ( + double)get_cs_angle_max(index); + + return math::radians(angle); +} + +double GZMixingInterfaceServo::get_angle_min_wrapper(const int index) +{ + if (index >= get_active_output_count()) { + return 0.; + } + + double angle = is_tiltrotor(index) ? (double)get_tl_angle_min(index - _control_surface_count.get()) : ( + double)get_cs_angle_min(index); + + return math::radians(angle); +} + bool GZMixingInterfaceServo::init(const std::string &model_name) { // /model/rascal_110_0/servo_2 @@ -46,6 +164,11 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) PX4_ERR("failed to advertise %s", servo_topic.c_str()); return false; } + + double min_val = get_angle_min_wrapper(i); + double max_val = get_angle_max_wrapper(i); + _angle_min_rad.push_back(min_val); + _angular_range_rad.push_back(max_val - min_val); } ScheduleNow(); @@ -64,8 +187,9 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA for (auto &servo_pub : _servos_pub) { if (_mixing_output.isFunctionSet(i)) { gz::msgs::Double servo_output; - ///TODO: Normalize output data - double output = (outputs[i] - 500) / 500.0; + + double output_range = _mixing_output.maxValue(i) - _mixing_output.minValue(i); + double output = _angle_min_rad[i] + _angular_range_rad[i] * (outputs[i] - _mixing_output.minValue(i)) / output_range; // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; // std::cout << " output: " << output << std::endl; servo_output.set_data(output); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 63345b3aed89..12027860aebe 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -36,6 +36,7 @@ #include #include +#include // GZBridge mixing class for Servos. // It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling @@ -66,6 +67,14 @@ class GZMixingInterfaceServo : public OutputModuleInterface friend class GZBridge; void Run() override; + int get_active_output_count(); + float get_tl_angle_max(const size_t index); + float get_tl_angle_min(const size_t index); + float get_cs_angle_max(const size_t index); + float get_cs_angle_min(const size_t index); + bool is_tiltrotor(const int index); + double get_angle_min_wrapper(const int index); + double get_angle_max_wrapper(const int index); gz::transport::Node &_node; pthread_mutex_t &_node_mutex; @@ -73,4 +82,35 @@ class GZMixingInterfaceServo : public OutputModuleInterface MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; std::vector _servos_pub; + std::vector _angle_min_rad; + std::vector _angular_range_rad; + + DEFINE_PARAMETERS( + (ParamFloat) _ca_sv_tl_max_a_0, + (ParamFloat) _ca_sv_tl_min_a_0, + (ParamFloat) _ca_sv_tl_max_a_1, + (ParamFloat) _ca_sv_tl_min_a_1, + (ParamFloat) _ca_sv_tl_max_a_2, + (ParamFloat) _ca_sv_tl_min_a_2, + (ParamFloat) _ca_sv_tl_max_a_3, + (ParamFloat) _ca_sv_tl_min_a_3, + (ParamFloat) _ca_sv_cs_max_a_0, + (ParamFloat) _ca_sv_cs_min_a_0, + (ParamFloat) _ca_sv_cs_max_a_1, + (ParamFloat) _ca_sv_cs_min_a_1, + (ParamFloat) _ca_sv_cs_max_a_2, + (ParamFloat) _ca_sv_cs_min_a_2, + (ParamFloat) _ca_sv_cs_max_a_3, + (ParamFloat) _ca_sv_cs_min_a_3, + (ParamFloat) _ca_sv_cs_max_a_4, + (ParamFloat) _ca_sv_cs_min_a_4, + (ParamFloat) _ca_sv_cs_max_a_5, + (ParamFloat) _ca_sv_cs_min_a_5, + (ParamFloat) _ca_sv_cs_max_a_6, + (ParamFloat) _ca_sv_cs_min_a_6, + (ParamFloat) _ca_sv_cs_max_a_7, + (ParamFloat) _ca_sv_cs_min_a_7, + (ParamInt) _control_surface_count, + (ParamInt) _tilt_count + ) };