From d47d5f860bed1976874d55ad1d9c2cfcd77fc6fa 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 ++++++------ .../init.d-posix/airframes/4012_gz_rover_ackermann | 2 ++ .../init.d-posix/airframes/4030_gz_tiltrotor | 8 ++++---- Tools/module_config/generate_params.py | 4 ++++ .../simulation/gz_bridge/GZMixingInterfaceServo.cpp | 4 ++-- src/modules/simulation/gz_bridge/module.yaml | 8 ++++---- 6 files changed, 22 insertions(+), 16 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 d4c80a29092f..157c33ed0a6d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -61,17 +61,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 900 +param set-default SIM_GZ_SV_MAX3 2700 +param set-default SIM_GZ_SV_DIS3 1800 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 900 +param set-default SIM_GZ_SV_MAX4 2700 +param set-default SIM_GZ_SV_DIS4 1800 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/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..d38799e299f1 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 @@ -49,3 +49,5 @@ param set-default SIM_GZ_WH_DIS1 100 # Steering param set-default SIM_GZ_SV_FUNC1 201 param set-default SIM_GZ_SV_REV 1 +param set-default SIM_GZ_SV_MIN1 1500 +param set-default SIM_GZ_SV_MAX1 2100 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor index 7d5ebf3a873f..72752e9daa8d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor @@ -77,10 +77,10 @@ param set-default SIM_GZ_SV_FUNC3 203 param set-default SIM_GZ_SV_FUNC4 204 param set-default SIM_GZ_SV_FUNC5 205 -param set-default SIM_GZ_SV_MAX4 27000 -param set-default SIM_GZ_SV_MIN4 18000 -param set-default SIM_GZ_SV_MAX5 27000 -param set-default SIM_GZ_SV_MIN5 18000 +param set-default SIM_GZ_SV_MAX4 2700 +param set-default SIM_GZ_SV_MIN4 1800 +param set-default SIM_GZ_SV_MAX5 2700 +param set-default SIM_GZ_SV_MIN5 1800 param set-default NPFG_PERIOD 12 param set-default FW_PR_FF 0.2 diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 92b25936568e..8defa0615c71 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -284,9 +284,13 @@ def add_local_param(param_name, param_def): ''' minimum_description = \ '''Minimum output value (when not disarmed). +Servo output represents angles [-180,180] with an offset of 180 deg, and a scaling of 10. +Ex. ailerons of -45 deg would be 1350. (Must be coherent with joint limits in the airframe/model.sdf) ''' maximum_description = \ '''Maxmimum output value (when not disarmed). +Servo output represents angles [-180,180] deg with an offset of 180 deg, and a scaling of 10. +Ex. ailerons of 45 deg would be 2250. (Must be coherent with joint limits in the airframe/model.sdf) ''' failsafe_description = \ '''This is the output value that is set when in failsafe mode. diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 9c7969b2dc1e..1b81ef27d076 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] / 10. - 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..bd810369929a 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -20,10 +20,10 @@ 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 } - failsafe: { min: 0, max: 1000 } + disarmed: { min: 0, max: 3600, default: 1800 } + min: { min: 0, max: 3600, default: 1350 } + max: { min: 0, max: 3600, default: 2250 } + failsafe: { min: 0, max: 3600 } num_channels: 8 - param_prefix: SIM_GZ_WH group_label: 'Wheels'