Send angles in actual range instead of hardcoded angles

PX4 · Nov 29, 2024 · 4ec9990 · 4ec9990
1 parent ebcb99d
commit 4ec9990
Show file tree

Hide file tree

Showing 6 changed files with 22 additions and 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
@@ -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 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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'