Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

INDI restructuring (#3256)

Browse Source
This commit is contained in:
Ewoud Smeur
2024-04-04 10:55:41 +02:00
committed by GitHub
parent 6667d5091d
commit 69f2248a32
13 changed files with 236 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_v3b.xml
+16 -16
View File
@@ -203,21 +203,21 @@
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="8191"/>
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/>
<servo no="5" name="SERVO_ELEVATOR" min="5000" neutral="5000" max="-5500"/>
<servo no="6" name="SERVO_RUDDER" min="-8191" neutral="0" max="8191"/>
</servos>
<!-- Can bus 2 actuators -->
<servos DRIVER="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="7372"/>
</servos>
<!-- CAN BUS 2 command outputs-->
@@ -297,11 +297,11 @@
<define name="IYY_WING" value="0.38472811"/>
<define name="M" value="6.94"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001734904"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="0.001684595"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001600837,-0.000732843248849}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000947144886298017, -0.000053954}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-1.16702}"/>
@@ -311,7 +311,7 @@
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
@@ -367,7 +367,7 @@
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
<define name="SP_PSI_DELTA_LIMIT" value="5" unit="deg"/>
<!-- Reference model -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
@@ -437,7 +437,7 @@
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100, 100}"/>
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
conf/airframes/tudelft/rot_wing_v3d.xml
+8 -8
View File
@@ -288,21 +288,21 @@
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001734904"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="0.001684595"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001600837,-0.000732843248849}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000947144886298017, -0.000053954}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-1.16702}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
@@ -358,7 +358,7 @@
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
<define name="SP_PSI_DELTA_LIMIT" value="5" unit="deg"/>
<!-- Reference model -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
@@ -429,7 +429,7 @@
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100, 100}"/>
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
conf/airframes/tudelft/rot_wing_v3e.xml
+17 -17
View File
@@ -187,11 +187,11 @@
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="8191"/>
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-3629" neutral="66" max="3760"/>
</servos>
@@ -203,10 +203,10 @@
<!-- Can bus 2 actuators -->
<servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="5" name="BROTATION_MECH" min="-3629" neutral="66" max="3760"/>
</servos>
@@ -288,21 +288,21 @@
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001734904"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="0.001684595"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001600837,-0.000732843248849}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000947144886298017, -0.000053954}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-1.16702}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
@@ -358,7 +358,7 @@
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
<define name="SP_PSI_DELTA_LIMIT" value="5" unit="deg"/>
<!-- Reference model -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
@@ -429,7 +429,7 @@
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100, 100}"/>
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
conf/airframes/tudelft/rot_wing_v3f.xml
+17 -17
View File
@@ -187,11 +187,11 @@
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="8191"/>
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-1769" neutral="66" max="1900"/>
</servos>
@@ -203,10 +203,10 @@
<!-- Can bus 2 actuators -->
<servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="5" name="BROTATION_MECH" min="-1769" neutral="66" max="1900"/>
</servos>
@@ -288,21 +288,21 @@
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001734904"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="0.001684595"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001600837,-0.000732843248849}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000947144886298017, -0.000053954}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-1.16702}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
@@ -358,7 +358,7 @@
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
<define name="SP_PSI_DELTA_LIMIT" value="5" unit="deg"/>
<!-- Reference model -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
@@ -429,7 +429,7 @@
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100, 100}"/>
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
conf/airframes/tudelft/rot_wing_v3g.xml
+17 -17
View File
@@ -187,11 +187,11 @@
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="8191"/>
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-1671" neutral="188" max="2048"/>
</servos>
@@ -203,10 +203,10 @@
<!-- Can bus 2 actuators -->
<servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="5" name="BROTATION_MECH" min="-1671" neutral="188" max="2048"/>
</servos>
@@ -288,21 +288,21 @@
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001734904"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="0.001684595"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001600837,-0.000732843248849}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000947144886298017, -0.000053954}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-1.16702}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
@@ -358,7 +358,7 @@
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
<define name="SP_PSI_DELTA_LIMIT" value="5" unit="deg"/>
<!-- Reference model -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
@@ -429,7 +429,7 @@
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100, 100}"/>
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
conf/airframes/tudelft/rot_wing_v3h.xml
+19 -19
View File
@@ -187,12 +187,12 @@
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="8191"/>
<servo no="5" name="ROTATION_MECH" min="-1515" neutral="328" max="2171"/>
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-1392" neutral="451" max="2294"/>
</servos>
<!-- CAN BUS 1 command outputs-->
@@ -203,11 +203,11 @@
<!-- Can bus 2 actuators -->
<servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="5" name="BROTATION_MECH" min="-1515" neutral="328" max="2171"/>
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="5" name="BROTATION_MECH" min="-1392" neutral="451" max="2294"/>
</servos>
<!-- CAN BUS 2 command outputs-->
@@ -288,21 +288,21 @@
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001734904"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="0.001684595"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001600837,-0.000732843248849}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000947144886298017, -0.000053954}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-1.16702}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
@@ -358,7 +358,7 @@
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
<define name="SP_PSI_DELTA_LIMIT" value="5" unit="deg"/>
<!-- Reference model -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
@@ -429,7 +429,7 @@
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100, 100}"/>
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
conf/flight_plans/tudelft/rotating_wing_EHVB.xml
+20 -16
View File
@@ -9,14 +9,14 @@
<waypoint name="begin_trans" x="300." y="90."/>
<waypoint name="end_trans" x="300." y="120."/>
<waypoint name="STDBY" x="60" y="20"/>
<waypoint name="p1" x="65" y="75"/>
<!-- <waypoint name="p1" x="65" y="75"/>
<waypoint name="p2" x="200" y="120"/>
<waypoint name="p3" x="250" y="-30"/>
<waypoint name="p4" x="120" y="-75"/>
<waypoint name="p4" x="120" y="-75"/> -->
<waypoint name="circ" x="160" y="25"/>
<waypoint name="TD" x="10" y="-1"/>
<waypoint name="APP" x="70" y="-25"/>
<waypoint name="FOLLOW" x="300" y="80"/>
<!-- <waypoint name="APP" x="70" y="-25"/> -->
<!-- <waypoint name="FOLLOW" x="300" y="80"/> -->
<!-- EHVB -->
<waypoint lat="52.169189" lon="4.410820" name="C1"/>
<waypoint lat="52.168049" lon="4.406923" name="C2"/>
@@ -90,10 +90,14 @@
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Approach APP">
<block name="Standby_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<!-- <block name="Approach APP">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
</block> -->
<!-- <block name="follow_module">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
@@ -102,12 +106,12 @@
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="begin_trans" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="line_trans_fwd">
<!-- <block name="line_trans_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<call_once fun="NavSetWaypointHere(WP_begin_trans)"/>
<go wp="end_trans"/>
<deroute block="end_transition"/>
</block>
</block> -->
<block name="end_transition">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="end_trans"/>
@@ -122,22 +126,22 @@
<stay wp="end_trans"/>
<exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CCW_fwd"/>
</block>
<block name="route">
<!-- <block name="route">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/>
</block>
<block name="small_route" strip_button="Route" strip_icon="path.png">
</block> -->
<!-- <block name="small_route" strip_button="Route" strip_icon="path.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<go wp="p2"/>
<go wp="p3"/>
<go wp="p4"/>
<go wp="p1"/>
<deroute block="small_route"/>
</block>
</block> -->
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
@@ -146,24 +150,24 @@
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="-100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CW_fwd">
<!-- <block name="big_Circle_CW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CCW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="-150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
</block> -->
<block name="Transition_quad">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<block name="land here">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<block name="land" strip_button="Land" strip_icon="land-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<go wp="TD"/>
</block>
conf/modules/stabilization_indi.xml
+1
View File
@@ -61,6 +61,7 @@
<dl_setting var="indi_gains.att.r" min="0" step="1" max="2500" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R"/>
<dl_setting var="indi_gains.rate.r" min="0.1" step="0.1" max="100" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="indi_use_adaptive" min="0" step="1" max="1" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8"/>
<dl_setting var="stablization_indi_yaw_dist_limit" min="0" step=".01" max="10.0" shortname="lim_yaw_spec_moment" param="STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT"/>
</dl_settings>
</dl_settings>
</settings>
conf/simulator/jsbsim/aircraft/rotwing3.xml
+1 -1
View File
@@ -917,7 +917,7 @@
<value>2.45</value> <!--Get rid of 0.5 rho-->
<property>fcs/rudder_lag</property>
<value>0.7376</value> <!--Convert to lbsft-->
<value> 0.002 </value>
<value> 0.01 </value>
</product>
</function>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+81 -64
View File
@@ -105,9 +105,9 @@
#endif
#endif
float du_min_stab_indi[INDI_NUM_ACT];
float du_max_stab_indi[INDI_NUM_ACT];
float du_pref_stab_indi[INDI_NUM_ACT];
float u_min_stab_indi[INDI_NUM_ACT];
float u_max_stab_indi[INDI_NUM_ACT];
float u_pref_stab_indi[INDI_NUM_ACT];
float indi_v[INDI_OUTPUTS];
float *Bwls[INDI_OUTPUTS];
int num_iter = 0;
@@ -197,6 +197,15 @@ float indi_Wu[INDI_NUM_ACT] = STABILIZATION_INDI_WLS_WU;
float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT - 1] = 1.0};
#endif
/**
* Limit the maximum specific moment that can be compensated (units rad/s^2)
*/
#ifdef STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
float stablization_indi_yaw_dist_limit = STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
#else // Put a rediculously high limit
float stablization_indi_yaw_dist_limit = 99999.f;
#endif
// variables needed for control
float actuator_state_filt_vect[INDI_NUM_ACT];
struct FloatRates angular_accel_ref = {0., 0., 0.};
@@ -204,8 +213,7 @@ struct FloatRates angular_rate_ref = {0., 0., 0.};
float angular_acceleration[3] = {0., 0., 0.};
float actuator_state[INDI_NUM_ACT];
float indi_u[INDI_NUM_ACT];
float indi_du[INDI_NUM_ACT];
float g2_times_du;
float rate_vect_prev[3] = {0., 0., 0.};
float q_filt = 0.0;
float r_filt = 0.0;
@@ -426,6 +434,8 @@ void stabilization_indi_enter(void)
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
float_vect_zero(rate_vect_prev, 3);
float_vect_zero(du_estimation, INDI_NUM_ACT);
float_vect_zero(ddu_estimation, INDI_NUM_ACT);
}
@@ -554,6 +564,31 @@ void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
*/
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
{
// Propagate actuator filters
get_actuator_state();
float actuator_state_filt_vect_prev[INDI_NUM_ACT];
for (int i = 0; i < INDI_NUM_ACT; i++) {
update_butterworth_2_low_pass(&actuator_lowpass_filters[i], actuator_state[i]);
update_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], actuator_state[i]);
actuator_state_filt_vect[i] = actuator_lowpass_filters[i].o[0];
actuator_state_filt_vect_prev[i] = actuator_lowpass_filters[i].o[1];
// calculate derivatives for estimation
float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
actuator_state_filt_vectd[i] = (estimation_input_lowpass_filters[i].o[0] - estimation_input_lowpass_filters[i].o[1]) *
PERIODIC_FREQUENCY;
actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
}
// Use the last actuator state for this computation
float g2_times_u_act_filt = float_vect_dot_product(g2, actuator_state_filt_vect_prev, INDI_NUM_ACT)/INDI_G_SCALING;
// Predict angular acceleration u*B
float angular_acc_prediction_filt[INDI_OUTPUTS];
float_mat_vect_mul(angular_acc_prediction_filt, Bwls, actuator_state_filt_vect, INDI_OUTPUTS, INDI_NUM_ACT);
angular_acc_prediction_filt[2] -= g2_times_u_act_filt;
/* Propagate the filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
@@ -573,6 +608,10 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
}
// subtract u*B from angular acceleration
float angular_acc_disturbance_estimate[INDI_OUTPUTS];
float_vect_diff(angular_acc_disturbance_estimate, angular_acceleration, angular_acc_prediction_filt, 3);
//The rates used for feedback are by default the measured rates.
//If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
//Note that due to the delay, the PD controller may need relaxed gains.
@@ -610,89 +649,83 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q;
angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r;
g2_times_du = 0.0;
for (i = 0; i < INDI_NUM_ACT; i++) {
g2_times_du += g2[i] * indi_du[i];
}
//G2 is scaled by INDI_G_SCALING to make it readable
g2_times_du = g2_times_du / INDI_G_SCALING;
float use_increment = 0.0;
if (in_flight) {
use_increment = 1.0;
}
struct FloatVect3 v_thrust;
v_thrust.x = 0.0;
v_thrust.y = 0.0;
v_thrust.z = 0.0;
if (indi_thrust_increment_set) {
v_thrust = indi_thrust_increment;
//update thrust command such that the current is correctly estimated
stabilization_cmd[COMMAND_THRUST] = 0;
float current_thrust_filt_z = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i];
current_thrust_filt_z += Bwls[3][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_z[i];
}
stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
// Add the increment to the current estimated thrust
v_thrust.z = current_thrust_filt_z + indi_thrust_increment.z;
#if INDI_OUTPUTS == 5
stabilization_cmd[COMMAND_THRUST_X] = 0;
float current_thrust_filt_x = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
stabilization_cmd[COMMAND_THRUST_X] += actuator_state[i] * (int32_t) act_is_thruster_x[i];
current_thrust_filt_x += Bwls[4][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_x[i];
}
stabilization_cmd[COMMAND_THRUST_X] /= num_thrusters_x;
v_thrust.x = current_thrust_filt_x + indi_thrust_increment.x;
#endif
} else {
// incremental thrust
for (i = 0; i < INDI_NUM_ACT; i++) {
v_thrust.z +=
(stabilization_cmd[COMMAND_THRUST] - use_increment * actuator_state_filt_vect[i]) * Bwls[3][i];
v_thrust.z += stabilization_cmd[COMMAND_THRUST] * Bwls[3][i];
#if INDI_OUTPUTS == 5
stabilization_cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X];
v_thrust.x +=
(stabilization_cmd[COMMAND_THRUST_X] - use_increment * actuator_state_filt_vect[i]) * Bwls[4][i];
v_thrust.x += stabilization_cmd[COMMAND_THRUST_X] * Bwls[4][i];
#endif
}
}
// This term compensates for the spinup torque in the yaw axis
float g2_times_u = float_vect_dot_product(g2, indi_u, INDI_NUM_ACT)/INDI_G_SCALING;
if (in_flight) {
// Limit the estimated disturbance in yaw for drones that are stable in sideslip
BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
} else {
// Not in flight, so don't estimate disturbance
float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
}
// The control objective in array format
indi_v[0] = (angular_accel_ref.p - use_increment * angular_acceleration[0]);
indi_v[1] = (angular_accel_ref.q - use_increment * angular_acceleration[1]);
indi_v[2] = (angular_accel_ref.r - use_increment * angular_acceleration[2] + g2_times_du);
indi_v[0] = (angular_accel_ref.p - angular_acc_disturbance_estimate[0]);
indi_v[1] = (angular_accel_ref.q - angular_acc_disturbance_estimate[1]);
indi_v[2] = (angular_accel_ref.r - angular_acc_disturbance_estimate[2]) + g2_times_u;
indi_v[3] = v_thrust.z;
#if INDI_OUTPUTS == 5
indi_v[4] = v_thrust.x;
#endif
#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
// Calculate the increment for each actuator
for (i = 0; i < INDI_NUM_ACT; i++) {
indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
+ (g1g2_pseudo_inv[i][1] * indi_v[1])
+ (g1g2_pseudo_inv[i][2] * indi_v[2])
+ (g1g2_pseudo_inv[i][3] * indi_v[3]);
indi_u[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
+ (g1g2_pseudo_inv[i][1] * indi_v[1])
+ (g1g2_pseudo_inv[i][2] * indi_v[2])
+ (g1g2_pseudo_inv[i][3] * indi_v[3]);
}
#else
stabilization_indi_set_wls_settings(use_increment);
stabilization_indi_set_wls_settings();
// WLS Control Allocator
num_iter =
wls_alloc(indi_du, indi_v, du_min_stab_indi, du_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, du_pref_stab_indi, 10000, 10,
wls_alloc(indi_u, indi_v, u_min_stab_indi, u_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, u_pref_stab_indi, 10000, 10,
INDI_NUM_ACT, INDI_OUTPUTS);
#endif
if (in_flight) {
// Add the increments to the actuators
float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT);
} else {
// Not in flight, so don't increment
float_vect_copy(indi_u, indi_du, INDI_NUM_ACT);
}
// Bound the inputs to the actuators
for (i = 0; i < INDI_NUM_ACT; i++) {
if (act_is_servo[i]) {
@@ -706,20 +739,6 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
}
}
// Propagate actuator filters
get_actuator_state();
for (i = 0; i < INDI_NUM_ACT; i++) {
update_butterworth_2_low_pass(&actuator_lowpass_filters[i], actuator_state[i]);
update_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], actuator_state[i]);
actuator_state_filt_vect[i] = actuator_lowpass_filters[i].o[0];
// calculate derivatives for estimation
float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
actuator_state_filt_vectd[i] = (estimation_input_lowpass_filters[i].o[0] - estimation_input_lowpass_filters[i].o[1]) *
PERIODIC_FREQUENCY;
actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
}
// Use online effectiveness estimation only when flying
if (in_flight && indi_use_adaptive) {
lms_estimation();
@@ -732,17 +751,15 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
}
/**
* @param use_increment
*
* Function that sets the du_min, du_max and du_pref if function not elsewhere defined
*/
void WEAK stabilization_indi_set_wls_settings(float use_increment)
void WEAK stabilization_indi_set_wls_settings(void)
{
// Calculate the min and max increments
for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
du_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i] - use_increment * actuator_state_filt_vect[i];
du_max_stab_indi[i] = MAX_PPRZ - use_increment * actuator_state_filt_vect[i];
du_pref_stab_indi[i] = act_pref[i] - use_increment * actuator_state_filt_vect[i];
u_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i];
u_max_stab_indi[i] = MAX_PPRZ;
u_pref_stab_indi[i] = act_pref[i];
#ifdef GUIDANCE_INDI_MIN_THROTTLE
float airspeed = stateGetAirspeed_f();
@@ -750,9 +767,9 @@ void WEAK stabilization_indi_set_wls_settings(float use_increment)
if (!act_is_servo[i]) {
if ((guidance_h.mode == GUIDANCE_H_MODE_HOVER) || (guidance_h.mode == GUIDANCE_H_MODE_NAV)) {
if (airspeed < STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD) {
du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE - use_increment * actuator_state_filt_vect[i];
u_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE;
} else {
du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - use_increment * actuator_state_filt_vect[i];
u_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD;
}
}
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+6 -4
View File
@@ -38,9 +38,9 @@ extern bool act_is_servo[INDI_NUM_ACT];
extern bool indi_use_adaptive;
extern float du_min_stab_indi[INDI_NUM_ACT];
extern float du_max_stab_indi[INDI_NUM_ACT];
extern float du_pref_stab_indi[INDI_NUM_ACT];
extern float u_min_stab_indi[INDI_NUM_ACT];
extern float u_max_stab_indi[INDI_NUM_ACT];
extern float u_pref_stab_indi[INDI_NUM_ACT];
extern float *Bwls[INDI_OUTPUTS];
extern float thrust_bx_eff;
@@ -57,6 +57,8 @@ struct Indi_gains {
struct FloatRates rate;
};
extern float stablization_indi_yaw_dist_limit;
extern struct Indi_gains indi_gains;
extern void stabilization_indi_init(void);
@@ -67,7 +69,7 @@ extern void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
extern void stabilization_indi_set_wls_settings(float use_increment);
extern void stabilization_indi_set_wls_settings(void);
extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c
+31 -43
View File
@@ -128,6 +128,7 @@ struct rot_wing_eff_sched_param_t eff_sched_p = {
.Ixx_wing = ROT_WING_EFF_SCHED_IXX_WING,
.Iyy_wing = ROT_WING_EFF_SCHED_IYY_WING,
.m = ROT_WING_EFF_SCHED_M,
.DMdpprz_hover_roll = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
.hover_roll_pitch_coef = ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
.hover_roll_roll_coef = ROT_WING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
.k_elevator = ROT_WING_EFF_SCHED_K_ELEVATOR,
@@ -158,7 +159,7 @@ inline void eff_scheduling_rot_wing_update_pusher_effectiveness(void);
inline void eff_scheduling_rot_wing_schedule_liftd(void);
inline float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED);
inline void stabilization_indi_set_wls_settings(float use_increment);
void stabilization_indi_set_wls_settings(void);
/** ABI binding wing position data.
@@ -198,9 +199,10 @@ void eff_scheduling_rot_wing_init(void)
// Set moment derivative variables
eff_sched_var.pitch_motor_dMdpprz = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH;
eff_sched_var.roll_motor_dMdpprz = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL;
eff_sched_var.roll_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_roll[0] + (MAX_PPRZ/2.) * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust
eff_sched_var.cmd_elevator = 0;
eff_sched_var.cmd_pusher = 0;
eff_sched_var.cmd_pusher_scaled = 0;
eff_sched_var.cmd_T_mean_scaled = 0;
@@ -258,9 +260,10 @@ void eff_scheduling_rot_wing_update_MMOI(void)
void eff_scheduling_rot_wing_update_cmd(void)
{
// These indexes depend on the INDI sequence, not the actuator IDX
eff_sched_var.cmd_elevator = actuators_pprz[5];
eff_sched_var.cmd_pusher_scaled = actuators_pprz[8] * 0.000853229; // Scaled with 8181 / 9600 / 1000
eff_sched_var.cmd_T_mean_scaled = (actuators_pprz[0] + actuators_pprz[1] + actuators_pprz[2] + actuators_pprz[3]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000
eff_sched_var.cmd_elevator = actuator_state_filt_vect[5];
eff_sched_var.cmd_pusher = actuator_state_filt_vect[8];
eff_sched_var.cmd_pusher_scaled = actuator_state_filt_vect[8] * 0.000853229; // Scaled with 8181 / 9600 / 1000
eff_sched_var.cmd_T_mean_scaled = (actuator_state_filt_vect[0] + actuator_state_filt_vect[1] + actuator_state_filt_vect[2] + actuator_state_filt_vect[3]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000
}
void eff_scheduling_rot_wing_update_airspeed(void)
@@ -278,12 +281,20 @@ void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void)
float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy;
// Roll motor effectiveness
float dM_dpprz_right = (eff_sched_p.DMdpprz_hover_roll[0] + actuator_state_filt_vect[1] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
float dM_dpprz_left = (eff_sched_p.DMdpprz_hover_roll[0] + actuator_state_filt_vect[3] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
float roll_motor_p_eff = (eff_sched_var.roll_motor_dMdpprz * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
Bound(roll_motor_p_eff, 0.00001, 1);
// Bound dM_dpprz to half and 2 times the hover effectiveness
Bound(dM_dpprz_right, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
Bound(dM_dpprz_left, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
float roll_motor_p_eff_right = -(dM_dpprz_right * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
Bound(roll_motor_p_eff_right, -1, -0.00001);
float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx;
float roll_motor_p_eff_compensated = roll_motor_p_eff + roll_motor_airspeed_compensation;
Bound(roll_motor_p_eff_compensated, 0.00001, 1);
roll_motor_p_eff_left += roll_motor_airspeed_compensation;
Bound(roll_motor_p_eff_left, 0.00001, 1);
float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy;
Bound(roll_motor_q_eff, 0, 1);
@@ -295,11 +306,11 @@ void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void)
g1g2[1][2] = -pitch_motor_q_eff; // pitch effectiveness back motor
// Update right motor p and q effectiveness
g1g2[0][1] = -roll_motor_p_eff; // roll effectiveness right motor (no airspeed compensation)
g1g2[0][1] = roll_motor_p_eff_right; // roll effectiveness right motor (no airspeed compensation)
g1g2[1][1] = roll_motor_q_eff; // pitch effectiveness right motor
// Update left motor p and q effectiveness
g1g2[0][3] = roll_motor_p_eff_compensated; // roll effectiveness left motor
g1g2[0][3] = roll_motor_p_eff_left; // roll effectiveness left motor
g1g2[1][3] = -roll_motor_q_eff; // pitch effectiveness left motor
}
@@ -334,7 +345,7 @@ void eff_scheduling_rot_wing_update_rudder_effectiveness(void)
// Convert moment to effectiveness
float eff_z_rudder = dMzdpprz / eff_sched_p.Izz;
Bound(eff_z_rudder, 0.00001, 0.1);
Bound(eff_z_rudder, 0.000001, 0.1);
g1g2[2][4] = eff_z_rudder;
}
@@ -357,10 +368,10 @@ void eff_scheduling_rot_wing_update_flaperon_effectiveness(void)
void eff_scheduling_rot_wing_update_pusher_effectiveness(void)
{
float rpmP = eff_sched_p.k_rpm_pprz_pusher[0] + eff_sched_p.k_rpm_pprz_pusher[1] * eff_sched_var.cmd_pusher_scaled + eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled;
float rpmP = eff_sched_p.k_rpm_pprz_pusher[0] + eff_sched_p.k_rpm_pprz_pusher[1] * eff_sched_var.cmd_pusher + eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher * eff_sched_var.cmd_pusher;
float dFxdrpmP = eff_sched_p.k_pusher[0]*rpmP + eff_sched_p.k_pusher[1] * eff_sched_var.airspeed;
float drpmPdpprz = eff_sched_p.k_rpm_pprz_pusher[1] + 2. * eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher_scaled;
float drpmPdpprz = eff_sched_p.k_rpm_pprz_pusher[1] + 2. * eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher;
float eff_pusher = (dFxdrpmP * drpmPdpprz / eff_sched_p.m) / 10000.;
@@ -389,39 +400,16 @@ float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED) {
return eff_scheduling_rot_wing_lift_d;
}
void stabilization_indi_set_wls_settings(float use_increment)
void stabilization_indi_set_wls_settings(void)
{
// Calculate the min and max increments
for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
du_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i] - use_increment*actuator_state_filt_vect[i];
du_max_stab_indi[i] = MAX_PPRZ - use_increment*actuator_state_filt_vect[i];
du_pref_stab_indi[i] = act_pref[i] - use_increment*actuator_state_filt_vect[i];
u_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i];
u_max_stab_indi[i] = MAX_PPRZ;
u_pref_stab_indi[i] = act_pref[i];
if (i == 5) {
du_pref_stab_indi[i] = 0; // Set change in prefered state to 0 for elevator
du_min_stab_indi[i] = - use_increment*actuator_state_filt_vect[i]; // cmd 0 is lowest position for elevator
u_pref_stab_indi[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator
u_min_stab_indi[i] = 0; // cmd 0 is lowest position for elevator
}
}
#if ROTWING_V3B
float min_pprz_cmd_ail = -MAX_PPRZ;
if (eff_sched_var.wing_rotation_deg < 15) {
min_pprz_cmd_ail = 0;
}
Bound(min_pprz_cmd_ail, -9600, 0);
du_min_stab_indi[6] = min_pprz_cmd_ail - use_increment*actuators_pprz[6];
float min_pprz_cmd_flap_ail = -MAX_PPRZ;
if (eff_sched_var.wing_rotation_deg < 38) {
min_pprz_cmd_flap_ail = -1000;
} if (eff_sched_var.wing_rotation_deg > 50) {
min_pprz_cmd_flap_ail = -9600;
} else {
min_pprz_cmd_flap_ail = -5.596578906693223 * eff_sched_var.wing_rotation_deg * eff_sched_var.wing_rotation_deg * eff_sched_var.wing_rotation_deg + 654.186408367317 * eff_sched_var.wing_rotation_deg * eff_sched_var.wing_rotation_deg - 25577.0135504177 * eff_sched_var.wing_rotation_deg + 333307.855118805;
}
Bound(min_pprz_cmd_flap_ail, -9600, 0);
du_min_stab_indi[7] = min_pprz_cmd_flap_ail - use_increment*actuators_pprz[7];
#endif //ROTWING_V3B
}
sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h
+2
View File
@@ -35,6 +35,7 @@ struct rot_wing_eff_sched_param_t {
float Ixx_wing; // wing MMOI around the chordwise direction of the wing [kgm²]
float Iyy_wing; // wing MMOI around the spanwise direction of the wing [kgm²]
float m; // mass [kg]
float DMdpprz_hover_roll[2]; // Moment coeficients for roll motors (Scaled by 10000)
float hover_roll_pitch_coef[2]; // Model coefficients to correct pitch effective for roll motors
float hover_roll_roll_coef[2]; // Model coefficients to correct roll effectiveness for roll motors
float k_elevator[3];
@@ -67,6 +68,7 @@ struct rot_wing_eff_sched_var_t {
// commands
float cmd_elevator;
float cmd_pusher;
float cmd_pusher_scaled;
float cmd_T_mean_scaled;