From 66ab707d14403be28e243dbd23aaef390294aefb Mon Sep 17 00:00:00 2001 From: Pernilla Date: Thu, 28 Nov 2024 18:46:30 +0100 Subject: [PATCH] Send angles in actual range instead of hardcoded angles --- .../init.d-posix/airframes/4011_gz_lawnmower | 12 ++++++------ .../simulation/gz_bridge/GZMixingInterfaceServo.cpp | 4 ++-- src/modules/simulation/gz_bridge/module.yaml | 6 +++--- 3 files changed, 11 insertions(+), 11 deletions(-) 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 5f2b635238f7..1ddf0ec41146 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -62,17 +62,17 @@ param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels # 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_MIN3 9000 +param set-default SIM_GZ_SV_MAX3 27000 +param set-default SIM_GZ_SV_DIS3 18000 param set-default SIM_GZ_SV_FAIL3 500 # 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_MIN4 9000 +param set-default SIM_GZ_SV_MAX4 27000 +param set-default SIM_GZ_SV_DIS4 18000 param set-default SIM_GZ_SV_FAIL4 500 # Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]: diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 9c7969b2dc1e..322e82bb8afc 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -64,8 +64,8 @@ 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 = math::radians((double)outputs[i] / 100. - 180.);; + // 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/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 1b38e13d9883..0ffde5f98f0e 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -20,9 +20,9 @@ actuator_output: group_label: 'Servos' channel_label: 'Servo' standard_params: - disarmed: { min: 0, max: 1000, default: 500 } - min: { min: 0, max: 1000, default: 0 } - max: { min: 0, max: 1000, default: 1000 } + disarmed: { min: 0, max: 36000, default: 18000 } + min: { min: 0, max: 36000, default: 15000 } + max: { min: 0, max: 36000, default: 21000 } failsafe: { min: 0, max: 1000 } num_channels: 8 - param_prefix: SIM_GZ_WH