Send angles in actual range instead of hardcoded angles

PX4 · Nov 28, 2024 · 66ab707 · 66ab707
1 parent bc0c3fe
commit 66ab707
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 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
@@ -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
@@ -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
@@ -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