Fix motors to be in range.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
This commit is contained in:
Benjamin Perseghetti 2023-01-26 18:05:30 -05:00
parent c341f47d2a
commit a1090bbf9d
No known key found for this signature in database
GPG Key ID: B2B0FDB44D2F831F
2 changed files with 11 additions and 23 deletions

View File

@ -36,20 +36,21 @@ param set-default CA_ROTOR4_AZ 0.0
param set-default CA_ROTOR4_PX 0.2
param set-default SIM_GZ_EC_FUNC1 101
param set-default SIM_GZ_EC_MIN1 0
param set-default SIM_GZ_EC_MAX1 1000
param set-default SIM_GZ_EC_MIN1 10
param set-default SIM_GZ_EC_MAX1 1500
param set-default SIM_GZ_EC_FUNC2 102
param set-default SIM_GZ_EC_MIN2 0
param set-default SIM_GZ_EC_MAX2 1000
param set-default SIM_GZ_EC_MIN2 10
param set-default SIM_GZ_EC_MAX2 1500
param set-default SIM_GZ_EC_FUNC3 103
param set-default SIM_GZ_EC_MIN3 0
param set-default SIM_GZ_EC_MAX3 1000
param set-default SIM_GZ_EC_MIN3 10
param set-default SIM_GZ_EC_MAX3 1500
param set-default SIM_GZ_EC_FUNC4 104
param set-default SIM_GZ_EC_MIN4 0
param set-default SIM_GZ_EC_MAX4 1000
param set-default SIM_GZ_EC_MIN4 10
param set-default SIM_GZ_EC_MAX4 1500
param set-default SIM_GZ_EC_FUNC5 105
param set-default SIM_GZ_EC_MIN5 0
param set-default SIM_GZ_EC_MAX5 1000
param set-default SIM_GZ_EC_MAX5 3500
param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_FUNC2 202

View File

@ -633,7 +633,6 @@
<axis>
<xyz>0 1 0</xyz>
<limit>
<!-- -30/+30 deg. -->
<lower>-0.53</lower>
<upper>0.53</upper>
</limit>
@ -654,7 +653,6 @@
<axis>
<xyz>1 0 0</xyz>
<limit>
<!-- -30/+30 deg. -->
<lower>-0.01</lower>
<upper>0.01</upper>
</limit>
@ -668,15 +666,6 @@
</ode>
</physics>
</joint>
<!-- <include>
<uri>model://gps</uri>
<pose>0 0 0 0 0 0</pose>
<name>gps</name>
</include>
<joint name='gps_joint' type='fixed'>
<child>gps::link</child>
<parent>base_link</parent>
</joint> -->
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
<!-- <plugin name="left_wing_lift" filename="libLiftDragPlugin.so"> -->
<a0>0.05984281113</a0>
@ -725,7 +714,6 @@
<joint_name>servo_1</joint_name>
<sub_topic>servo_1</sub_topic>
</plugin>
<!-- <plugin name="elevator_lift" filename="libLiftDragPlugin.so"> -->
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
<a0>-0.2</a0>
<cla>4.752798721</cla>
@ -749,7 +737,6 @@
<joint_name>servo_2</joint_name>
<sub_topic>servo_2</sub_topic>
</plugin>
<!-- <plugin name="rudder_lift" filename="libLiftDragPlugin.so"> -->
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
<a0>0.0</a0>
<cla>4.752798721</cla>
@ -855,7 +842,7 @@
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>3500</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.01</momentConstant>
<momentConstant>0.6</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>4</motorNumber>
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>