10 changes: 5 additions & 5 deletions conf/airframes/OPENUAS/openuas_parrot_bebop.xml
View file
Expand Up @@ -67,7 +67,7 @@
<module name="stabilization" type="indi">
<define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
</module>
</target>
Expand Down Expand Up @@ -484,11 +484,11 @@ second order filter parameters -->
<define name="FILT_CUTOFF_RDOT" value="4.0"/> -->

<!-- first order actuator dynamics -->
<!-- <define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>-->
<!-- <define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>-->

<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
View file
Expand Up @@ -492,9 +492,9 @@ The most crucial part for the magnetometer calibration:
<define name="FILT_CUTOFF" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/> <!-- make true if all works -->
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
View file
Expand Up @@ -586,9 +586,9 @@ The most crucial part for the magnetometer calibration:
<define name="FILT_CUTOFF_RDOT" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
<define name="ACT_FREQ_P" value="15.6"/>
<define name="ACT_FREQ_Q" value="15.6"/>
<define name="ACT_FREQ_R" value="15.6"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/examples/bebop2_indi.xml
View file
Expand Up @@ -130,9 +130,9 @@
<define name="FILT_CUTOFF" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/examples/bebop2_opticflow.xml
View file
Expand Up @@ -277,9 +277,9 @@
<define name="FILT_CUTOFF_R" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
View file
Expand Up @@ -162,9 +162,9 @@
<define name="FILT_CUTOFF" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
7 changes: 4 additions & 3 deletions conf/airframes/examples/cube_orange.xml
View file
Expand Up @@ -126,6 +126,7 @@
<define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/>
<define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="2500"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="18"/>
</module>

<module name="motor_mixing"/>
Expand Down Expand Up @@ -300,9 +301,9 @@
<define name="FILT_CUTOFF_R" value="4.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.0354"/>
<define name="ACT_DYN_Q" value="0.0354"/>
<define name="ACT_DYN_R" value="0.0354"/>
<define name="ACT_FREQ_P" value="18"/>
<define name="ACT_FREQ_Q" value="18"/>
<define name="ACT_FREQ_R" value="18"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/examples/matek_h743_slim.xml
View file
Expand Up @@ -215,9 +215,9 @@
<define name="FILT_CUTOFF_R" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/examples/matek_h7_rotorcraft_hitl.xml
View file
Expand Up @@ -214,9 +214,9 @@
<define name="FILT_CUTOFF_R" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml
View file
Expand Up @@ -235,9 +235,9 @@
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_RDOT" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
<define name="ACT_FREQ_P" value="15.6"/>
<define name="ACT_FREQ_Q" value="15.6"/>
<define name="ACT_FREQ_R" value="15.6"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/examples/quadshot_asp21_spektrum.xml
View file
Expand Up @@ -233,9 +233,9 @@
<define name="FILT_CUTOFF" value="8.0"/>
<define name="FILT_CUTOFF_RDOT" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
<define name="ACT_FREQ_P" value="15.6"/>
<define name="ACT_FREQ_Q" value="15.6"/>
<define name="ACT_FREQ_R" value="15.6"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/examples/trashcan.xml
View file
Expand Up @@ -388,9 +388,9 @@
<define name="FILT_CUTOFF" value="5.0"/>

<!-- first order actuator dynamics (indi_simple) -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/ardrone2_OF_hover.xml
View file
Expand Up @@ -197,9 +197,9 @@
<define name="FILT_CUTOFF" value="3.2" />

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06" />
<define name="ACT_DYN_Q" value="0.06" />
<define name="ACT_DYN_R" value="0.06" />
<define name="ACT_FREQ_P" value="31.7" />
<define name="ACT_FREQ_Q" value="31.7" />
<define name="ACT_FREQ_R" value="31.7" />

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE" />
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/ardrone2_indi.xml
View file
Expand Up @@ -152,9 +152,9 @@
<define name="FILT_CUTOFF" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/ardrone2_optitrack.xml
View file
Expand Up @@ -126,9 +126,9 @@
<define name="FILT_CUTOFF" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
2 changes: 1 addition & 1 deletion conf/airframes/tudelft/bebop2_indi.xml
View file
Expand Up @@ -115,7 +115,7 @@
<define name="ESTIMATION_FILT_CUTOFF" value="3.2f"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.08f, 0.08f, 0.08f, 0.08f}"/>
<define name="ACT_FREQ" value="{42.7, 42.7, 42.7, 42.7}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop2_indi_convergence.xml
View file
Expand Up @@ -154,9 +154,9 @@
<define name="FILT_CUTOFF_P" value="10.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
8 changes: 4 additions & 4 deletions conf/airframes/tudelft/bebop2_no_damping.xml
View file
Expand Up @@ -25,7 +25,7 @@
<module name="guidance" type="indi">
<define name="GUIDANCE_INDI_POS_GAIN" value="0.5"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.8"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.06"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="31.7"/>
</module>

<module name="ahrs" type="int_cmpl_quat">
Expand Down Expand Up @@ -150,9 +150,9 @@
<define name="FILT_CUTOFF_RDOT" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
4 changes: 2 additions & 2 deletions conf/airframes/tudelft/bebop2_no_damping_WLS.xml
View file
Expand Up @@ -24,7 +24,7 @@
<module name="guidance" type="indi">
<define name="GUIDANCE_INDI_POS_GAIN" value="0.5"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.8"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.06"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="31.7"/>
</module>

<module name="ahrs" type="int_cmpl_quat">
Expand Down Expand Up @@ -141,7 +141,7 @@
<define name="FILT_CUTOFF" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop2_optitrack.xml
View file
Expand Up @@ -150,9 +150,9 @@
<define name="FILT_CUTOFF_RDOT" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
View file
Expand Up @@ -191,9 +191,9 @@
<define name="FILT_CUTOFF_RDOT" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop2_undistort_front.xml
View file
Expand Up @@ -174,9 +174,9 @@
<define name="FILT_CUTOFF" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
8 changes: 4 additions & 4 deletions conf/airframes/tudelft/bebop_OF_hover.xml
View file
Expand Up @@ -24,7 +24,7 @@
<module name="ins" type="gps_passthrough"/>
<!-- module name="guidance" type="indi"/-->
<!-- <module name="guidance" type="indi"> <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN"
value="-500.0"/> <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
value="-500.0"/> <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="TRUE"/> </module> -->


Expand Down Expand Up @@ -175,9 +175,9 @@
<define name="FILT_CUTOFF" value="8.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<define name="ACT_FREQ_P" value="53.9"/>
<define name="ACT_FREQ_Q" value="53.9"/>
<define name="ACT_FREQ_R" value="53.9"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop_autonomous_race_2018.xml
View file
Expand Up @@ -215,9 +215,9 @@
<define name="FILT_ZETA_R" value="0.55"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<define name="ACT_FREQ_P" value="53.9"/>
<define name="ACT_FREQ_Q" value="53.9"/>
<define name="ACT_FREQ_R" value="53.9"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop_course_orangeavoid.xml
View file
Expand Up @@ -210,9 +210,9 @@
<define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<define name="ACT_FREQ_P" value="53.9"/>
<define name="ACT_FREQ_Q" value="53.9"/>
<define name="ACT_FREQ_R" value="53.9"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
View file
Expand Up @@ -224,9 +224,9 @@
<define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<define name="ACT_FREQ_P" value="53.9"/>
<define name="ACT_FREQ_Q" value="53.9"/>
<define name="ACT_FREQ_R" value="53.9"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop_flip.xml
View file
Expand Up @@ -172,9 +172,9 @@
<define name="FILT_CUTOFF" value="8.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<define name="ACT_FREQ_P" value="53.9"/>
<define name="ACT_FREQ_Q" value="53.9"/>
<define name="ACT_FREQ_R" value="53.9"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
20 changes: 13 additions & 7 deletions conf/airframes/tudelft/bebop_indi_actuators.xml
View file
Expand Up @@ -32,7 +32,7 @@
<module name="ins" type="extended"/>
<module name="guidance" type="indi">
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="WLS_N_U" value="4"/>
<define name="WLS_N_V" value="4"/>
Expand All @@ -45,7 +45,12 @@
</module-->
</firmware>

<commands>
<commands><!-- Order must match the INDI ctrl_eff: INDI_NUM_ACT -->
<axis name="TOP_LEFT" failsafe_value="0"/>
<axis name="TOP_RIGHT" failsafe_value="0"/>
<axis name="BOTTOM_RIGHT" failsafe_value="0"/>
<axis name="BOTTOM_LEFT" failsafe_value="0"/>

<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
Expand All @@ -60,10 +65,10 @@
</servos>

<command_laws>
<set servo="TOP_LEFT" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
<set servo="TOP_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
<set servo="BOTTOM_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
<set servo="BOTTOM_LEFT" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
<set servo="TOP_LEFT" value="autopilot_get_motors_on() ? @TOP_LEFT : -MAX_PPRZ"/>
<set servo="TOP_RIGHT" value="autopilot_get_motors_on() ? @TOP_RIGHT : -MAX_PPRZ"/>
<set servo="BOTTOM_RIGHT" value="autopilot_get_motors_on() ? @BOTTOM_RIGHT : -MAX_PPRZ"/>
<set servo="BOTTOM_LEFT" value="autopilot_get_motors_on() ? @BOTTOM_LEFT : -MAX_PPRZ"/>
</command_laws>

<section name="AIR_DATA" prefix="AIR_DATA_">
Expand Down Expand Up @@ -128,7 +133,7 @@
<define name="FILT_CUTOFF" value="5.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
Expand Down Expand Up @@ -165,6 +170,7 @@

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="COMMANDS_NB" value="4"/>
<define name="JSBSIM_MODEL" value="bebop" type="string"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
</section>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop_mavlink.xml
View file
Expand Up @@ -182,9 +182,9 @@
<define name="FILT_CUTOFF" value="8.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<define name="ACT_FREQ_P" value="53.9"/>
<define name="ACT_FREQ_Q" value="53.9"/>
<define name="ACT_FREQ_R" value="53.9"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
4 changes: 2 additions & 2 deletions conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml
View file
Expand Up @@ -40,7 +40,7 @@

<module name="guidance" type="indi">
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
</module>

Expand Down Expand Up @@ -127,7 +127,7 @@
<define name="REF_RATE_R" value="28.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
<define name="FILT_CUTOFF" value="5.0"/>
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>
<define name="USE_ADAPTIVE" value="TRUE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop_opticflow.xml
View file
Expand Up @@ -155,9 +155,9 @@
<define name="FILT_CUTOFF" value="8.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<define name="ACT_FREQ_P" value="53.9"/>
<define name="ACT_FREQ_Q" value="53.9"/>
<define name="ACT_FREQ_R" value="53.9"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
17 changes: 11 additions & 6 deletions conf/airframes/tudelft/bebop_opticflow_indoor.xml
View file
Expand Up @@ -66,7 +66,12 @@
</module>
</firmware>

<commands>
<commands><!-- Order must match the INDI ctrl_eff: INDI_NUM_ACT -->
<axis name="TOP_LEFT" failsafe_value="0"/>
<axis name="TOP_RIGHT" failsafe_value="0"/>
<axis name="BOTTOM_RIGHT" failsafe_value="0"/>
<axis name="BOTTOM_LEFT" failsafe_value="0"/>

<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
Expand All @@ -81,10 +86,10 @@
</servos>

<command_laws>
<set servo="TOP_LEFT" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
<set servo="TOP_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
<set servo="BOTTOM_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
<set servo="BOTTOM_LEFT" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
<set servo="TOP_LEFT" value="autopilot_get_motors_on() ? @TOP_LEFT : -MAX_PPRZ"/>
<set servo="TOP_RIGHT" value="autopilot_get_motors_on() ? @TOP_RIGHT : -MAX_PPRZ"/>
<set servo="BOTTOM_RIGHT" value="autopilot_get_motors_on() ? @BOTTOM_RIGHT : -MAX_PPRZ"/>
<set servo="BOTTOM_LEFT" value="autopilot_get_motors_on() ? @BOTTOM_LEFT : -MAX_PPRZ"/>
</command_laws>

<section name="IMU" prefix="IMU_">
Expand Down Expand Up @@ -146,7 +151,7 @@
<define name="FILT_CUTOFF" value="5.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
2 changes: 1 addition & 1 deletion conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml
View file
Expand Up @@ -179,7 +179,7 @@ pyramid level 2: 21 fps average, min=11fps
<define name="FILT_CUTOFF" value="5.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
17 changes: 11 additions & 6 deletions conf/airframes/tudelft/bebop_optitrack.xml
View file
Expand Up @@ -33,7 +33,12 @@
</module>
</firmware>

<commands>
<commands><!-- Order must match the INDI ctrl_eff: INDI_NUM_ACT -->
<axis name="TOP_LEFT" failsafe_value="0"/>
<axis name="TOP_RIGHT" failsafe_value="0"/>
<axis name="BOTTOM_RIGHT" failsafe_value="0"/>
<axis name="BOTTOM_LEFT" failsafe_value="0"/>

<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
Expand All @@ -48,10 +53,10 @@
</servos>

<command_laws>
<set servo="TOP_LEFT" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
<set servo="TOP_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
<set servo="BOTTOM_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
<set servo="BOTTOM_LEFT" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
<set servo="TOP_LEFT" value="autopilot_get_motors_on() ? @TOP_LEFT : -MAX_PPRZ"/>
<set servo="TOP_RIGHT" value="autopilot_get_motors_on() ? @TOP_RIGHT : -MAX_PPRZ"/>
<set servo="BOTTOM_RIGHT" value="autopilot_get_motors_on() ? @BOTTOM_RIGHT : -MAX_PPRZ"/>
<set servo="BOTTOM_LEFT" value="autopilot_get_motors_on() ? @BOTTOM_LEFT : -MAX_PPRZ"/>
</command_laws>

<section name="IMU" prefix="IMU_">
Expand Down Expand Up @@ -120,7 +125,7 @@
<define name="FILT_CUTOFF" value="5.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
View file
Expand Up @@ -220,9 +220,9 @@
<define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<define name="ACT_FREQ_P" value="53.9"/>
<define name="ACT_FREQ_Q" value="53.9"/>
<define name="ACT_FREQ_R" value="53.9"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
2 changes: 1 addition & 1 deletion conf/airframes/tudelft/cyfoam.xml
View file
Expand Up @@ -306,7 +306,7 @@
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>
<define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="15.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.045"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="23.6"/>
<!--<define name="KNIFE_EDGE_TEST" value="TRUE"/>-->
<!--Flap effectiveness on lift-->
<define name="FE_LIFT_A_PITCH" value="0.00018"/>
Expand Down
2 changes: 1 addition & 1 deletion conf/airframes/tudelft/disco_rotorcraft_indi.xml
View file
Expand Up @@ -179,7 +179,7 @@
<define name="FILT_CUTOFF" value="5.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.1, 0.1, 0.045}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 23.6}"/>
<define name="ACT_RATE_LIMIT" value="{170, 170, 9600}"/>
<define name="ACT_IS_SERVO" value="{1, 1, 0}"/>

Expand Down
4 changes: 2 additions & 2 deletions conf/airframes/tudelft/fan_demo.xml
View file
Expand Up @@ -101,7 +101,7 @@

<section name="GUIDANCE_INDI">
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.7"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.5"/>
Expand Down Expand Up @@ -146,7 +146,7 @@
<define name="FILT_CUTOFF" value="5.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/guido_ardrone2_optitrack.xml
View file
Expand Up @@ -133,9 +133,9 @@ ARDrone2 with optical_flow landing.
<define name="FILT_CUTOFF" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<define name="ACT_FREQ_P" value="31.7"/>
<define name="ACT_FREQ_Q" value="31.7"/>
<define name="ACT_FREQ_R" value="31.7"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
9 changes: 4 additions & 5 deletions conf/airframes/tudelft/hybrid_nav_test.xml
View file
Expand Up @@ -167,11 +167,10 @@
<define name="FILT_CUTOFF_R" value="4.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.047"/>
<define name="ACT_DYN_Q" value="0.047"/>
<define name="ACT_DYN_R" value="0.047"/>
<define name="ACT_DYN_B" value="0.047"/>
<define name="ACT_DYN_P" value="0.047"/>
<define name="ACT_FREQ_P" value="24.1"/>
<define name="ACT_FREQ_Q" value="24.1"/>
<define name="ACT_FREQ_R" value="24.1"/>
<define name="ACT_FREQ_B" value="24.1"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/iris_indi.xml
View file
Expand Up @@ -212,9 +212,9 @@
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_RDOT" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.04" />
<define name="ACT_DYN_Q" value="0.04" />
<define name="ACT_DYN_R" value="0.04" />
<define name="ACT_FREQ_P" value="20.9"/>
<define name="ACT_FREQ_Q" value="20.9"/>
<define name="ACT_FREQ_R" value="20.9"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE" />
<define name="ADAPTIVE_MU" value="0.0001" />
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/mavtec1.xml
View file
Expand Up @@ -187,9 +187,9 @@
<define name="FILT_CUTOFF_RDOT" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
<define name="ACT_FREQ_P" value="15.6"/>
<define name="ACT_FREQ_Q" value="15.6"/>
<define name="ACT_FREQ_R" value="15.6"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/mavtec5.xml
View file
Expand Up @@ -177,9 +177,9 @@
<define name="FILT_CUTOFF_RDOT" value="3.2"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.15"/>
<define name="ACT_DYN_Q" value="0.15"/>
<define name="ACT_DYN_R" value="0.15"/>
<define name="ACT_FREQ_P" value="83.2"/>
<define name="ACT_FREQ_Q" value="83.2"/>
<define name="ACT_FREQ_R" value="83.2"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
50 changes: 25 additions & 25 deletions conf/airframes/tudelft/nederdrone4.xml
View file
Expand Up @@ -67,7 +67,7 @@
<module name="fdm" type="jsbsim"/>

<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
<define name="FILE_LOGGER_PATH" value="~/"/>
</module>

<!--Not dealing with these in the simulation-->
Expand Down Expand Up @@ -225,38 +225,38 @@
<define name="ROLL_COEF" value="{256, 253, 159, -159, -253, -256, 256, 157, 56, -56, -157, -256}"/>
<define name="PITCH_COEF" value="{256, 256, 256, 256, 256, 256, -256, -256, -256, -256, -256, -256}"/>
<define name="YAW_COEF" value="{251, -256, 252, -252, 256, -251, -256, 252, -254, 254, -252, 256}"/>
<!--<define name="YAW_COEF" value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
<define name="THRUST_COEF" value="{256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256}"/>
</section>

<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>

<!-- Tip props always at 80% -->
<call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*85*sched_ratio_tip_props);"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>

<set servo="MOTOR_1" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : actuators_pprz[3])"/>
<let var="tip_thruster" value="(Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*85*sched_ratio_tip_props)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,values)"/>
<call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,values)"/>

<set servo="MOTOR_1" value="($th_hold? -9600 : $tip_thruster)"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : @FRONT_LEFT)"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : @FRONT_LEFT)"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : @FRONT_RIGHT)"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : @FRONT_RIGHT)"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : $tip_thruster)"/>

<set servo="MOTOR_7" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : @REAR_RIGHT)"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : @REAR_RIGHT)"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : @REAR_RIGHT)"/>

<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="($th_hold? 9600 : actuators_pprz[4])"/>
<set servo="AIL_2" value="($th_hold? 9600 : actuators_pprz[5])"/>
<set servo="AIL_3" value="actuators_pprz[6]"/>
<set servo="AIL_4" value="actuators_pprz[7]"/>
<set servo="FLAP_3" value="actuators_pprz[6]"/>
<set servo="FLAP_4" value="actuators_pprz[7]"/>
<set servo="AIL_1" value="($th_hold? 9600 : @FRONT_LEFT_FLAP)"/>
<set servo="AIL_2" value="($th_hold? 9600 : @FRONT_RIGHT_FLAP)"/>
<set servo="AIL_3" value="@REAR_LEFT_FLAP"/>
<set servo="AIL_4" value="@REAR_RIGHT_FLAP"/>
<set servo="FLAP_3" value="@REAR_LEFT_FLAP"/>
<set servo="FLAP_4" value="@REAR_RIGHT_FLAP"/>
</command_laws>

<section name="MISC">
Expand Down Expand Up @@ -387,7 +387,7 @@
<define name="FILT_CUTOFF_R" value="4.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.0354, 0.0354, 0.0354, 0.0354, 0.05, 0.05, 0.05, 0.05}"/>
<define name="ACT_FREQ" value="{18.0, 18.0, 18.0, 18.0, 25.6, 25.6, 25.6, 25.6}"/>
<define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 9600, 170, 170, 170, 170}"/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 0, 1, 1, 1, 1}"/>
<define name="WLS_WU" value="{1.,1.,1.,1.,1.,1.,1.,1.}"/>
Expand Down
55 changes: 34 additions & 21 deletions conf/airframes/tudelft/nederdrone6.xml
View file
Expand Up @@ -68,7 +68,7 @@
<module name="fdm" type="jsbsim"/>

<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
<define name="FILE_LOGGER_PATH" value="~/"/>
</module>

<!--Not dealing with these in the simulation-->
Expand Down Expand Up @@ -189,7 +189,20 @@
<servo name="AIL_4" no="7" min="6000" neutral="0" max="-6000"/>
</servos>

<commands>
<commands><!-- Order must match the INDI ctrl_eff: INDI_NUM_ACT -->
<axis name="FRONT_LEFT" failsafe_value="0"/>
<axis name="FRONT_RIGHT" failsafe_value="0"/>
<axis name="REAR_LEFT" failsafe_value="0"/>
<axis name="REAR_RIGHT" failsafe_value="0"/>

<axis name="FRONT_LEFT_FLAP" failsafe_value="0"/>
<axis name="FRONT_RIGHT_FLAP" failsafe_value="0"/>
<axis name="REAR_LEFT_FLAP" failsafe_value="0"/>
<axis name="REAR_RIGHT_FLAP" failsafe_value="0"/>

<axis name="TIP_THRUSTERS" failsafe_value="0"/>

<!-- Default commands -->
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="-300"/>
<axis name="YAW" failsafe_value="0"/>
Expand All @@ -204,34 +217,34 @@
<define name="ROLL_COEF" value="{256, 157, 56, -56, -157, -256, 256, 157, 56, -56, -157, -256}"/>
<define name="PITCH_COEF" value="{256, 256, 256, 256, 256, 256, -256, -256, -256, -256, -256, -256}"/>
<define name="YAW_COEF" value="{251, -256, 252, -252, 256, -251, -256, 252, -254, 254, -252, 256}"/>
<!--<define name="YAW_COEF" value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
<define name="THRUST_COEF" value="{256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256}"/>
</section>

<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>

<!-- Tip props always at 80% -->
<call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props);"/>

<set servo="MOTOR_1" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : actuators_pprz[3])"/>
<let var="tip_thruster" value="(Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props)"/>

<set servo="MOTOR_1" value="($th_hold? -9600 : $tip_thruster)"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : @FRONT_LEFT)"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : @FRONT_LEFT)"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : @FRONT_RIGHT)"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : @FRONT_RIGHT)"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : $tip_thruster)"/>

<set servo="MOTOR_7" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : @REAR_RIGHT)"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : @REAR_RIGHT)"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : @REAR_RIGHT)"/>

<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="($th_hold? 9600 : actuators_pprz[4])"/>
<set servo="AIL_2" value="($th_hold? 9600 : actuators_pprz[5])"/>
<set servo="AIL_3" value="actuators_pprz[6]"/>
<set servo="AIL_4" value="actuators_pprz[7]"/>
<set servo="AIL_1" value="($th_hold? 9600 : @FRONT_LEFT_FLAP)"/>
<set servo="AIL_2" value="($th_hold? 9600 : @FRONT_RIGHT_FLAP)"/>
<set servo="AIL_3" value="@REAR_LEFT_FLAP"/>
<set servo="AIL_4" value="@REAR_RIGHT_FLAP"/>
</command_laws>

<section name="MISC">
Expand Down
59 changes: 36 additions & 23 deletions conf/airframes/tudelft/nederdrone7.xml
View file
Expand Up @@ -247,7 +247,20 @@
<servo name="AIL_4" no="7" min="6000" neutral="0" max="-6000"/>
</servos>

<commands>
<commands><!-- Order must match the INDI ctrl_eff: INDI_NUM_ACT -->
<axis name="FRONT_LEFT" failsafe_value="0"/>
<axis name="FRONT_RIGHT" failsafe_value="0"/>
<axis name="REAR_LEFT" failsafe_value="0"/>
<axis name="REAR_RIGHT" failsafe_value="0"/>

<axis name="FRONT_LEFT_FLAP" failsafe_value="0"/>
<axis name="FRONT_RIGHT_FLAP" failsafe_value="0"/>
<axis name="REAR_LEFT_FLAP" failsafe_value="0"/>
<axis name="REAR_RIGHT_FLAP" failsafe_value="0"/>

<axis name="TIP_THRUSTERS" failsafe_value="0"/>

<!-- Default commands -->
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="-300"/>
<axis name="YAW" failsafe_value="0"/>
Expand All @@ -262,34 +275,34 @@
<define name="ROLL_COEF" value="{256, 157, 56, -56, -157, -256, 256, 157, 56, -56, -157, -256}"/>
<define name="PITCH_COEF" value="{256, 256, 256, 256, 256, 256, -256, -256, -256, -256, -256, -256}"/>
<define name="YAW_COEF" value="{251, -256, 252, -252, 256, -251, -256, 252, -254, 254, -252, 256}"/>
<!--<define name="YAW_COEF" value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
<define name="THRUST_COEF" value="{256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256}"/>
</section>

<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>

<!-- Tip props always at 80% -->
<call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props);"/>

<set servo="MOTOR_1" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : actuators_pprz[3])"/>
<let var="tip_thruster" value="(Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props)"/>

<set servo="MOTOR_1" value="($th_hold? -9600 : $tip_thruster)"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : @FRONT_LEFT)"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : @FRONT_LEFT)"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : @FRONT_RIGHT)"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : @FRONT_RIGHT)"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : $tip_thruster)"/>

<set servo="MOTOR_7" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : @REAR_RIGHT)"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : @REAR_RIGHT)"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : @REAR_RIGHT)"/>

<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="($th_hold? 9600 : actuators_pprz[4])"/>
<set servo="AIL_2" value="($th_hold? 9600 : actuators_pprz[5])"/>
<set servo="AIL_3" value="actuators_pprz[6]"/>
<set servo="AIL_4" value="actuators_pprz[7]"/>
<set servo="AIL_1" value="($th_hold? 9600 : @FRONT_LEFT_FLAP)"/>
<set servo="AIL_2" value="($th_hold? 9600 : @FRONT_RIGHT_FLAP)"/>
<set servo="AIL_3" value="@REAR_LEFT_FLAP"/>
<set servo="AIL_4" value="@REAR_RIGHT_FLAP"/>
</command_laws>

<section name="MISC">
Expand Down Expand Up @@ -398,10 +411,10 @@
<define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
<define name="FILT_CUTOFF_P" value="20."/>
<define name="FILT_CUTOFF_Q" value="20."/>
<define name="FILT_CUTOFF_R" value="4."/>
<define name="FILT_CUTOFF_R" value="4.0"/>

<!-- first order actuator dynamics -->
<!--define name="ACT_DYN" value="{0.009, 0.009, 0.009, 0.009, 0.05, 0.05, 0.05, 0.05}"/-->
<define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028, 0.05, 0.05, 0.05, 0.05}"/>
<define name="ACT_FREQ" value="{14.2, 14.2, 14.2, 14.2, 25.6, 25.6, 25.6, 25.6}"/>
<define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 9600, 170, 170, 170, 170}"/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 0, 1, 1, 1, 1}"/>
<define name="WLS_WU" value="{1.,1.,1.,1.,1.,1.,1.,1.}"/>
Expand Down
56 changes: 34 additions & 22 deletions conf/airframes/tudelft/nederdrone8.xml
View file
Expand Up @@ -68,7 +68,7 @@
<module name="fdm" type="jsbsim"/>

<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
<define name="FILE_LOGGER_PATH" value="~/"/>
</module>

<!--Not dealing with these in the simulation-->
Expand Down Expand Up @@ -189,7 +189,19 @@
<servo name="AIL_4" no="7" min="6000" neutral="0" max="-6000"/>
</servos>

<commands>
<commands><!-- Order must match the INDI ctrl_eff: INDI_NUM_ACT -->
<axis name="FRONT_LEFT" failsafe_value="0"/>
<axis name="FRONT_RIGHT" failsafe_value="0"/>
<axis name="REAR_LEFT" failsafe_value="0"/>
<axis name="REAR_RIGHT" failsafe_value="0"/>

<axis name="FRONT_LEFT_FLAP" failsafe_value="0"/>
<axis name="FRONT_RIGHT_FLAP" failsafe_value="0"/>
<axis name="REAR_LEFT_FLAP" failsafe_value="0"/>
<axis name="REAR_RIGHT_FLAP" failsafe_value="0"/>

<axis name="TIP_THRUSTERS" failsafe_value="0"/>

<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="-300"/>
<axis name="YAW" failsafe_value="0"/>
Expand All @@ -204,34 +216,34 @@
<define name="ROLL_COEF" value="{256, 157, 56, -56, -157, -256, 256, 157, 56, -56, -157, -256}"/>
<define name="PITCH_COEF" value="{256, 256, 256, 256, 256, 256, -256, -256, -256, -256, -256, -256}"/>
<define name="YAW_COEF" value="{251, -256, 252, -252, 256, -251, -256, 252, -254, 254, -252, 256}"/>
<!--<define name="YAW_COEF" value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
<define name="THRUST_COEF" value="{256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256}"/>
</section>

<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>

<!-- Tip props always at 80% -->
<call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props);"/>

<set servo="MOTOR_1" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : actuators_pprz[3])"/>
<let var="tip_thruster" value="(Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props)"/>

<set servo="MOTOR_1" value="($th_hold? -9600 : $tip_thruster)"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : @FRONT_LEFT)"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : @FRONT_LEFT)"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : @FRONT_RIGHT)"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : @FRONT_RIGHT)"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : $tip_thruster)"/>

<set servo="MOTOR_7" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : @REAR_LEFT)"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : @REAR_RIGHT)"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : @REAR_RIGHT)"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : @REAR_RIGHT)"/>

<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="($th_hold? 9600 : actuators_pprz[4])"/>
<set servo="AIL_2" value="($th_hold? 9600 : actuators_pprz[5])"/>
<set servo="AIL_3" value="actuators_pprz[6]"/>
<set servo="AIL_4" value="actuators_pprz[7]"/>
<set servo="AIL_1" value="($th_hold? 9600 : @FRONT_LEFT_FLAP)"/>
<set servo="AIL_2" value="($th_hold? 9600 : @FRONT_RIGHT_FLAP)"/>
<set servo="AIL_3" value="@REAR_LEFT_FLAP"/>
<set servo="AIL_4" value="@REAR_RIGHT_FLAP"/>
</command_laws>

<section name="MISC">
Expand Down Expand Up @@ -343,7 +355,7 @@
<define name="FILT_CUTOFF_R" value="4.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028, 0.05, 0.05, 0.05, 0.05}"/>
<define name="ACT_FREQ" value="{14.2, 14.2, 14.2, 14.2, 25.6, 25.6, 25.6, 25.6}"/>
<define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 9600, 170, 170, 170, 170}"/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 0, 1, 1, 1, 1}"/>
<define name="WLS_WU" value="{1.,1.,1.,1.,1.,1.,1.,1.}"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/nederquad.xml
View file
Expand Up @@ -218,9 +218,9 @@
<define name="FILT_CUTOFF" value="1.5"/>
<define name="FILT_CUTOFF_R" value="1.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.0354" />
<define name="ACT_DYN_Q" value="0.0354" />
<define name="ACT_DYN_R" value="0.0354" />
<define name="ACT_FREQ_P" value="18.0" />
<define name="ACT_FREQ_Q" value="18.0" />
<define name="ACT_FREQ_R" value="18.0" />
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE" />
<define name="ADAPTIVE_MU" value="0.0001" />
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
View file
Expand Up @@ -248,9 +248,9 @@
<define name="FILT_ZETA_R" value="0.55"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
<define name="ACT_FREQ_P" value="15.6"/>
<define name="ACT_FREQ_Q" value="15.6"/>
<define name="ACT_FREQ_R" value="15.6"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/robird.xml
View file
Expand Up @@ -197,9 +197,9 @@ Flapping wing frame equiped with
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_RDOT" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.04" />
<define name="ACT_DYN_Q" value="0.04" />
<define name="ACT_DYN_R" value="0.04" />
<define name="ACT_FREQ_P" value="20.9"/>
<define name="ACT_FREQ_Q" value="20.9"/>
<define name="ACT_FREQ_R" value="20.9"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE" />
<define name="ADAPTIVE_MU" value="0.0001" />
Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/rot_wing_25kg.xml
View file
Expand Up @@ -362,9 +362,9 @@
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>

<!-- Actuator dynamics -->
<define name="ACT_DYN" value="{0.024, 0.024, 0.024, 0.024, 0.1, 0.1, 0.1, 0.1, 0.047}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}" />
<define name="ACT_FREQ" value="{12.1, 12.1, 12.1, 12.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}" />

<define VALUE="{1000, 1000, 1, 100, 100}" NAME="WLS_PRIORITIES"/>
<define VALUE="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}" NAME="WLS_WU"/>
Expand Down
4 changes: 3 additions & 1 deletion conf/airframes/tudelft/rot_wing_v3b.xml
View file
Expand Up @@ -74,11 +74,13 @@
<define name="MS45XX_PRESSURE_SCALE" value="1.9077609"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="0"/>
</module>
<module name="airspeed" type="uavcan">
<define name="AIRSPEED_UAVCAN_LOWPASS_FILTER" value="TRUE" />
<define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" />
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
<define name="AIRSPEED_UAVCAN_LOWPASS_TAU" value="0.15" />
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="1" /> <!-- Read Airspeed for logging but do not use it -->
</module>
<module name="air_data"/>
<module name="gps" type="ublox">
Expand Down
66 changes: 43 additions & 23 deletions conf/airframes/tudelft/rot_wing_v3d.xml
View file
Expand Up @@ -74,10 +74,12 @@
<define name="MS45XX_PRESSURE_SCALE" value="1.9077609"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
</module>
<module name="airspeed" type="uavcan">
<define name="AIRSPEED_UAVCAN_LOWPASS_FILTER" value="TRUE" />
<define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" />
<define name="AIRSPEED_UAVCAN_LOWPASS_TAU" value="0.15" />
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module>
<module name="air_data"/>
Expand Down Expand Up @@ -123,6 +125,7 @@
<module name="ground_detect_sensor"/>
<module name="rotwing_state"/>
<module name="preflight_checks"/>
<module name="pfc_actuators"/>
<module name="agl_dist"/>
<module name="approach_moving_target"/>

Expand Down Expand Up @@ -218,12 +221,24 @@
<servo no="11" name="AIL_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
</servos>

<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
<axis name="THRUST_X" failsafe_value="0"/>
<commands><!-- order must match the INDI ctrl_eff -->
<!-- commands from INDI -->
<axis name="FRONT_MOTOR" failsafe_value="0"/>
<axis name="RIGHT_MOTOR" failsafe_value="0"/>
<axis name="BACK_MOTOR" failsafe_value="0"/>
<axis name="LEFT_MOTOR" failsafe_value="0"/>
<axis name="RUDDER" failsafe_value="0"/>
<axis name="ELEVATOR" failsafe_value="0"/>
<axis name="AILERON" failsafe_value="0"/>
<axis name="FLAP" failsafe_value="0"/>
<axis name="THRUST_X" failsafe_value="0"/>
<!-- commands modules -->
<axis name="SKEW" failsafe_value="0"/>
<!-- default commands -->
<axis name="THRUST" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
</commands>

<command_laws>
Expand All @@ -234,25 +249,26 @@
<let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/>

<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[0])" SERVO="MOTOR_FRONT"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[1])" SERVO="MOTOR_RIGHT"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[2])" SERVO="MOTOR_BACK"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[3])" SERVO="MOTOR_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : actuators_pprz[4])" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="ROTATION_MECH"/>
<set VALUE="$ail_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
<set VALUE="$flap_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_RIGHT"/>
<call fun="pfc_actuators_run()"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(7, -9600)) : @FRONT_MOTOR)" SERVO="MOTOR_FRONT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : @RIGHT_MOTOR)" SERVO="MOTOR_RIGHT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : @BACK_MOTOR)" SERVO="MOTOR_BACK"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : @LEFT_MOTOR)" SERVO="MOTOR_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, @RUDDER))" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : @ELEVATOR))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="pfc_actuators_value(6, @SKEW)" SERVO="ROTATION_MECH"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : @AILERON)" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : @AILERON)" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : @FLAP)" SERVO="FLAP_LEFT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(4, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : @FLAP)" SERVO="FLAP_RIGHT"/>

<!-- Backup commands -->
<set VALUE="($hover_off? -9600 : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
<set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="BROTATION_MECH"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(7, -9600)) : @FRONT_MOTOR)" SERVO="BMOTOR_FRONT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : @RIGHT_MOTOR)" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : @BACK_MOTOR)" SERVO="BMOTOR_BACK"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : @LEFT_MOTOR)" SERVO="BMOTOR_LEFT"/>
<set VALUE="pfc_actuators_value(6, @SKEW)" SERVO="BROTATION_MECH"/>
</command_laws>

<section PREFIX="SYS_ID_" NAME="SYS_ID">
Expand Down Expand Up @@ -299,6 +315,10 @@
<section name="MISC">
<!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>

<!-- Preflight check actuators (ELE, RUD, AIL_L, FLAP_L, FLAP_R, AIL_R, ROT_M, M_FRONT, M_RIGHT_, M_BACK, M_LEFT, M_PUSH) -->
<define name="PFC_ACTUATORS" value="{{.feedback_id=SERVO_SERVO_ELEVATOR_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=0.85, .high_feedback=0.3, .timeout=1},{.feedback_id=SERVO_SERVO_RUDDER_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_AIL_LEFT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_FLAP_LEFT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_FLAP_RIGHT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_AIL_RIGHT_IDX, .feedback_id2=255, .low=-4500, .high=4500, .low_feedback=-0.15, .high_feedback=0.15, .timeout=1},{.feedback_id=SERVO_ROTATION_MECH_IDX, .feedback_id2=SERVO_BROTATION_MECH_IDX, .low=-9600, .high=9600, .low_feedback=1.57, .high_feedback=0, .timeout=5},{.feedback_id=SERVO_MOTOR_FRONT_IDX, .feedback_id2=SERVO_BMOTOR_FRONT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_RIGHT_IDX, .feedback_id2=SERVO_BMOTOR_RIGHT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_BACK_IDX, .feedback_id2=SERVO_BMOTOR_BACK_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_LEFT_IDX, .feedback_id2=SERVO_BMOTOR_LEFT_IDX, .low=-9600, .high=0, .low_feedback=0, .high_feedback=975, .timeout=3},{.feedback_id=SERVO_MOTOR_PUSH_IDX, .feedback_id2=255, .low=-9600, .high=2000, .low_feedback=0, .high_feedback=2200, .timeout=3}}"/>

<!-- Others -->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
Expand Down
4 changes: 2 additions & 2 deletions conf/airframes/tudelft/splash4.xml
View file
Expand Up @@ -97,7 +97,7 @@
<module name="guidance" type="indi">
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.0354"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="18.0"/>
</module>

<module name="air_data"/>
Expand Down Expand Up @@ -275,7 +275,7 @@
<define name="FILTER_YAW_RATE" value="TRUE"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.0354, 0.0354, 0.0354, 0.0354}"/>
<define name="ACT_FREQ" value="{18.0, 18.0, 18.0, 18.0}"/>

<define name="WLS_PRIORITIES" value="{1000.f, 1000.f, 1.f, 100.f}"/>

Expand Down
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/splash_px4.xml
View file
Expand Up @@ -175,9 +175,9 @@
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.04" />
<define name="ACT_DYN_Q" value="0.04" />
<define name="ACT_DYN_R" value="0.04" />
<define name="ACT_FREQ_P" value="20.9"/>
<define name="ACT_FREQ_Q" value="20.9"/>
<define name="ACT_FREQ_R" value="20.9"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE" />
<define name="ADAPTIVE_MU" value="0.0001" />
Expand Down
2 changes: 1 addition & 1 deletion conf/conf_tests_coverity.xml
View file
Expand Up @@ -84,7 +84,7 @@
telemetry="telemetry/default_fixedwing_imu.xml"
flight_plan="flight_plans/nav_modules.xml"
settings="settings/fixedwing_basic.xml settings/control/ctl_dash_loiter_trim.xml settings/nps.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/stabilization_attitude_fw.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/digital_cam_common.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/stabilization_attitude_fw.xml"
gui_color="blue"
/>
<aircraft
Expand Down
16 changes: 16 additions & 0 deletions conf/flash_modes.xml
View file
Expand Up @@ -36,6 +36,22 @@
<board name="holybro_.*"/>
</boards>
</mode>
<mode name="USB CubeProgrammer">
<variable name="FLASH_MODE" value="DFU_CUBE"/>
<boards>
<board name="apogee_.*"/>
<board name="li[s]?a_mx_.*"/>
<board name="elle*"/>
<board name="openpilot_revo.*"/>
<board name="chimera_.*"/>
<board name="tawaki_.*"/>
<board name="crazybee_f4_.*"/>
<board name="crazyflie_.*"/>
<board name="nucleo.*"/>
<board name="matek_.*"/>
<board name="holybro_.*"/>
</boards>
</mode>
<mode name="STLink (SWD)">
<variable name="FLASH_MODE" value="STLINK"/>
<boards>
Expand Down
8 changes: 4 additions & 4 deletions conf/modules/actuators_asctec_v2.xml
View file
Expand Up @@ -25,10 +25,10 @@
<define name="USE_$(ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER)"/>
<file name="actuators_asctec_v2.c"/>
<test>
<define name="SERVO_FRONT" value="0"/>
<define name="SERVO_BACK" value="1"/>
<define name="SERVO_LEFT" value="2"/>
<define name="SERVO_RIGHT" value="3"/>
<define name="SERVO_FRONT_IDX" value="0"/>
<define name="SERVO_BACK_IDX" value="1"/>
<define name="SERVO_LEFT_IDX" value="2"/>
<define name="SERVO_RIGHT_IDX" value="3"/>
<define name="USE_I2C1"/>
<define name="ACTUATORS_ASCTEC_V2_I2C_DEV" value="i2c1"/>
</test>
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/actuators_bebop.xml
View file
Expand Up @@ -23,7 +23,7 @@
<configure name="SRC_BOARD" value="boards/bebop"/>
<define name="BEBOP_ACTUATORS_I2C_DEV" value="i2c1"/>
<define name="USE_I2C1"/>
<define name="SERVOS_DEFAULT_OFFSET" value="0"/>
<define name="get_servo_idx(X)" value="X"/>
</test>
</makefile>
</module>
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/actuators_disco.xml
View file
Expand Up @@ -21,7 +21,7 @@
<define name="USE_I2C1"/>
<include name="arch/linux"/>
<configure name="SRC_BOARD" value="boards/disco"/>
<define name="SERVOS_DEFAULT_OFFSET" value="0"/>
<define name="SERVO_MOTOR_IDX" value="0"/>
</test>
</makefile>
</module>
Expand Down
10 changes: 10 additions & 0 deletions conf/modules/airspeed_uavcan.xml
View file
Expand Up @@ -9,7 +9,17 @@
<define name="AIRSPEED_UAVCAN_LOWPASS_TAU" value="0.15" description="Time constant for second order Butterworth low pass filter"/>
<define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" description="Period at which the sensor is sending airspeed"/>
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="true" description="Send the uavcan airspeed sensor over ABI"/>
<define name="AIRSPEED_UAVCAN_DIFF_P_SCALE" value="1.0" description="Pressure scaling correcting factor"/>
</doc>
<settings>
<dl_settings>
<dl_settings name="Airspeed UAVCAN">
<dl_setting shortname="autoset offset" var="autoset_offset" handler="autoset_offset" min="0" max="1" step="1" values="FALSE|TRUE" type="fun" module="modules/sensors/airspeed_uavcan"/>
<dl_setting shortname="diff_p offset" var="airspeed_uavcan.diff_p_offset" min="-2000" max="1000" step="0.1" type="float"/>
<dl_setting shortname="diff_p scale" var="airspeed_uavcan.diff_p_scale" min="0" max="1" step="0.01" type="float" param="AIRSPEED_UAVCAN_DIFF_P_SCALE"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>uavcan</depends>
<provides>airspeed</provides>
Expand Down
1 change: 1 addition & 0 deletions conf/modules/imu_mpu9250_i2c.xml
View file
Expand Up @@ -42,6 +42,7 @@
<test>
<define name="IMU_MPU9250_I2C_DEV" value="i2c1"/>
<define name="USE_I2C1"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_MPU9250_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
Expand Down
1 change: 1 addition & 0 deletions conf/modules/imu_mpu9250_spi.xml
View file
Expand Up @@ -51,6 +51,7 @@
<define name="USE_SPI1"/>
<define name="IMU_MPU9250_SPI_SLAVE_IDX" value="0"/>
<define name="USE_SPI_SLAVE0"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_MPU9250_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
Expand Down
36 changes: 36 additions & 0 deletions conf/modules/pfc_actuators.xml
View file
@@ -0,0 +1,36 @@
<!DOCTYPE module SYSTEM "module.dtd">

<module name="pfc_actuators" dir="checks">
<doc>
<description>
Preform a pre flight check of the actuators and validate by looking at the feedback.
</description>
<define name="PFC_ACTUATORS" value="{}" description="Struct containing the setup of the preflight check"/>
<define name="PFC_ACTUATORS_MAX_ANGLE_ERROR" value="0.1" description="Maximum allowed angle error in radians +/-"/>
<define name="PFC_ACTUATORS_MAX_RPM_ERROR" value="250" description="Maximum allowed RPM error +/-"/>
<define name="PFC_ACTUATORS_DEBUG" value="false" description="Enable debug output in the GCS"/>
</doc>
<settings>
<dl_settings>
<dl_settings name="Checks">
<dl_setting var="act_start" min="0" step="1" max="1" values="OFF|START" handler="start" module="checks/pfc_actuators" type="fun"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>@datalink,preflight_checks</depends>
</dep>
<header>
<file name="pfc_actuators.h"/>
</header>
<init fun="pfc_actuators_init()"/>
<makefile>
<file name="pfc_actuators.c"/>
<test>
<define name="PFC_ACTUATORS" value="{{}}"/>
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
<define name="DOWNLINK_DEVICE" value="uart0"/>
<define name="USE_UART0"/>
</test>
</makefile>
</module>
3 changes: 3 additions & 0 deletions conf/modules/preflight_checks.xml
View file
Expand Up @@ -18,6 +18,9 @@
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>@datalink</depends>
</dep>
<header>
<file name="preflight_checks.h"/>
</header>
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/sensors_hitl.xml
View file
Expand Up @@ -33,7 +33,7 @@
<file name="sensors_hitl.c"/>
<test firmware="rotorcraft">
<include name="../../conf/simulator/nps"/>
<define name="NPS_PROPAGATE" value="500"/>
<define name="PERIODIC_FREQUENCY" value="500"/>
<define name="HITL_DEVICE" value="usb_serial"/>
<define name="USE_USB_SERIAL"/>
</test>
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/stabilization_indi.xml
View file
Expand Up @@ -38,7 +38,7 @@
<define name="STABILIZATION_INDI_FILT_CUTOFF_Q" value="20.0" description="First order cutoff freq for angular rate in Hz"/>
<define name="STABILIZATION_INDI_FILT_CUTOFF_R" value="20.0" description="First order cutoff freq for angular rate in Hz"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}" description="actuator dynamics"/>
<define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}" description="actuator dynamics [rad/s]"/>
<define name="ACT_IS_SERVO" value="{0,0,0,0}" description="1 for every actuator that is a servo"/>
<define name="ACT_IS_THRUSTER_X" value="{0,0,0,0}" description="1 for every actuator that is a thruster in the body-x direction"/>
<define name="ACT_RATE_LIMIT" value="{9600,9600,9600,9600}" description="rate limit in PPRZ units per timestep (depends on control frequency)"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/modules/stabilization_indi_simple.xml
View file
Expand Up @@ -40,9 +40,9 @@
<define name="FILT_CUTOFF" value="8.0" description="second order filter cutoff frequency Hz"/>
<define name="FILT_CUTOFF_RDOT" value="8.0" description="second order filter cutoff frequency rdot Hz"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="ACT_DYN_P" value="0.1" description="first order actuator dynamics on roll rate"/>
<define name="ACT_DYN_Q" value="0.1" description="first order actuator dynamics on pitch rate"/>
<define name="ACT_DYN_R" value="0.1" description="first order actuator dynamics on yaw rate"/>
<define name="ACT_FREQ_P" value="53.9" description="first order actuator dynamics on roll rate [rad/s]"/>
<define name="ACT_FREQ_Q" value="53.9" description="first order actuator dynamics on pitch rate [rad/s]"/>
<define name="ACT_FREQ_R" value="53.9" description="first order actuator dynamics on yaw rate [rad/s]"/>
<define name="USE_ADAPTIVE" value="FALSE|TRUE" description="enable adaptive gains"/>
<define name="ADAPTIVE_MU" value="0.0001" description="adaptation parameter"/>
<define name="FULL_AUTHORITY" value="FALSE" description="Enable full control authority"/>
Expand Down
2 changes: 1 addition & 1 deletion conf/telemetry/highspeed_rotorcraft.xml
View file
Expand Up @@ -24,7 +24,7 @@
<message name="ENERGY" period="0.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="0.6"/>
<message name="AIR_DATA" period="0.1"/>
<message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.05"/>
<message name="VECTORNAV_INFO" period="0.5"/>
Expand Down
10 changes: 5 additions & 5 deletions conf/userconf/tudelft/conf.xml
View file
Expand Up @@ -7,7 +7,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/cv_opticflow.xml] modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/cv_opticflow.xml] modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
gui_color="red"
/>
<aircraft
Expand All @@ -18,7 +18,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
gui_color="#ffffd633d633"
/>
<aircraft
Expand Down Expand Up @@ -260,7 +260,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="#ffff00000000"
/>
<aircraft
Expand All @@ -282,7 +282,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_cyberzoo.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="blue"
/>
<aircraft
Expand All @@ -304,7 +304,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
gui_color="blue"
/>
<aircraft
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/arch/chibios/mcu_periph/i2c_arch.c
View file
Expand Up @@ -159,12 +159,12 @@ static void handle_i2c_thd(struct i2c_periph *p)
} else {
#if defined(STM32F7XX) || defined(STM32H7XX)
// we do stupid mem copy because F7 needs a special RAM for DMA operation
memcpy(i->dma_buf, (void *)t->buf, (size_t)(t->len_w));
status = i2cMasterReceiveTimeout(
(I2CDriver *)p->reg_addr,
(i2caddr_t)((t->slave_addr) >> 1),
(uint8_t *)i->dma_buf, (size_t)(t->len_r),
tmo);
cacheBufferInvalidate(i->dma_buf, t->len_r);
memcpy((void *)t->buf, i->dma_buf, (size_t)(t->len_r));
#else
status = i2cMasterReceiveTimeout(
Expand Down
6 changes: 1 addition & 5 deletions sw/airborne/boards/bebop/actuators.c
View file
Expand Up @@ -141,11 +141,7 @@ void actuators_bebop_commit(void)
// Send ABI message
struct act_feedback_t feedback[4];
for (int i=0;i<4;i++) {
#ifdef SERVOS_BEBOP_OFFSET
feedback[i].idx = SERVOS_BEBOP_OFFSET + i;
#else
feedback[i].idx = SERVOS_DEFAULT_OFFSET + i;
#endif
feedback[i].idx = get_servo_idx(i);
feedback[i].rpm = actuators_bebop.rpm_obs[i];
feedback[i].set.rpm = true;
}
Expand Down
6 changes: 6 additions & 0 deletions sw/airborne/boards/cube/orange/board.cfg
View file
Expand Up @@ -31,13 +31,19 @@ HEADER
#define ADC_CHANNEL_VSUPPLY ADC_1
#endif

/* allow to define ADC_CHANNEL_VBOARD in the airframe file*/
#ifndef ADC_CHANNEL_VBOARD
#define ADC_CHANNEL_VBOARD ADC_3
#endif

/* allow to define ADC_CHANNEL_CURRENT in the airframe file*/
#if !defined(ADC_CHANNEL_CURRENT) && !ADC_CURRENT_DISABLE
#define ADC_CHANNEL_CURRENT ADC_2
#endif

/* Default powerbrick values */
#define DefaultVoltageOfAdc(adc) ((3.3f/65536.0f) * 13.38f * adc)
#define DefaultVBoardOfAdc(adc) ((3.3f/65536.0f) * 1.89036f * adc)
#define DefaultMilliAmpereOfAdc(adc) ((3.3f/65536.0f) * 39.877f * adc)

/* Battery monitoring for file closing */
Expand Down
20 changes: 13 additions & 7 deletions sw/airborne/boards/cube/orange/board.h
View file
Expand Up @@ -45,13 +45,19 @@
#define ADC_CHANNEL_VSUPPLY ADC_1
#endif

/* allow to define ADC_CHANNEL_VBOARD in the airframe file*/
#ifndef ADC_CHANNEL_VBOARD
#define ADC_CHANNEL_VBOARD ADC_3
#endif

/* allow to define ADC_CHANNEL_CURRENT in the airframe file*/
#if !defined(ADC_CHANNEL_CURRENT) && !ADC_CURRENT_DISABLE
#define ADC_CHANNEL_CURRENT ADC_2
#endif

/* Default powerbrick values */
#define DefaultVoltageOfAdc(adc) ((3.3f/65536.0f) * 13.38f * adc)
#define DefaultVBoardOfAdc(adc) ((3.3f/65536.0f) * 1.89036 * adc)
#define DefaultMilliAmpereOfAdc(adc) ((3.3f/65536.0f) * 39.877f * adc)

/* Battery monitoring for file closing */
Expand Down Expand Up @@ -1661,6 +1667,13 @@
#define BOARD_GROUP_FOR(array, index, group) \
for (ioline_t index=0, *array = (ioline_t *) group ## _ARRAY; index < group ## _SIZE; index++)

#define ENERGY_SAVE_LOWS \
LINE_VDD_5V_PERIPH_EN, \
LINE_ALARM, \
LINE_PWM_VOLT_SEL, \
LINE_VDD_3V3_SENSORS_EN
#define ENERGY_SAVE_LOWS_SIZE 4

#define ENERGY_SAVE_INPUTS \
LINE_SPI_SLAVE0, \
LINE_SPI_SLAVE1, \
Expand All @@ -1680,13 +1693,6 @@
LINE_SERVO1
#define ENERGY_SAVE_INPUTS_SIZE 16

#define ENERGY_SAVE_LOWS \
LINE_VDD_5V_PERIPH_EN, \
LINE_ALARM, \
LINE_PWM_VOLT_SEL, \
LINE_VDD_3V3_SENSORS_EN
#define ENERGY_SAVE_LOWS_SIZE 4

#if !defined(_FROM_ASM_)
#ifdef __cplusplus
extern "C" {
Expand Down
11 changes: 6 additions & 5 deletions sw/airborne/boards/disco/actuators.c
View file
Expand Up @@ -40,6 +40,11 @@

#include <stdio.h>


#ifndef SERVO_MOTOR_IDX
#warning "Disco actuators require a <servo name=MOTOR>"
#endif

/**
* private observation structure
*/
Expand Down Expand Up @@ -170,11 +175,7 @@ void actuators_disco_commit(void)

// Send ABI message
struct act_feedback_t feedback = {0};
#ifdef SERVOS_DISCO_OFFSET
feedback.idx = SERVOS_DISCO_OFFSET;
#else
feedback.idx = SERVOS_DEFAULT_OFFSET;
#endif
feedback.idx = SERVO_MOTOR_IDX;
feedback.rpm = actuators_disco.rpm_obs;
feedback.set.rpm = true;
AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_BOARD_ID, &feedback, 1);
Expand Down
4 changes: 4 additions & 0 deletions sw/airborne/boards/lisa_mx/chibios/v2.1/board.h
View file
Expand Up @@ -1173,7 +1173,11 @@
#endif

#ifndef USE_PWM_TIM4
#ifdef USE_SERVOS_7AND8
#define USE_PWM_TIM4 1
#else
#define USE_PWM_TIM4 0
#endif
#endif

#ifndef USE_PWM_TIM5
Expand Down
4 changes: 4 additions & 0 deletions sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h
View file
Expand Up @@ -1172,7 +1172,11 @@
#endif

#ifndef USE_PWM_TIM4
#ifdef USE_SERVOS_7AND8
#define USE_PWM_TIM4 1
#else
#define USE_PWM_TIM4 0
#endif
#endif

#ifndef USE_PWM_TIM5
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
View file
Expand Up @@ -132,7 +132,7 @@ static void send_status(struct transport_tx *trans, struct link_device *dev)
&radio_control.status, &radio_control.frame_rate,
&fix, &autopilot.mode, &in_flight, &motors_on,
&autopilot.arming_status, &guidance_h.mode, &guidance_v.mode,
&time_sec, &electrical.vsupply);
&time_sec, &electrical.vsupply, &electrical.vboard);
}

static void send_energy(struct transport_tx *trans, struct link_device *dev)
Expand Down
7 changes: 3 additions & 4 deletions sw/airborne/firmwares/rotorcraft/autopilot_utils.c
View file
Expand Up @@ -130,9 +130,8 @@ void WEAK set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flig
cmd_in[COMMAND_THRUST] = 0;
}
#endif
cmd_out[COMMAND_ROLL] = cmd_in[COMMAND_ROLL];
cmd_out[COMMAND_PITCH] = cmd_in[COMMAND_PITCH];
cmd_out[COMMAND_YAW] = cmd_in[COMMAND_YAW];
cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST];
for (int i=0; i < COMMANDS_NB; i++) {
cmd_out[i] = cmd_in[i];
}
}

8 changes: 4 additions & 4 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
View file
Expand Up @@ -88,13 +88,11 @@ static void guidance_indi_filter_thrust(void);
#endif

#ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
#ifndef GUIDANCE_INDI_THRUST_DYNAMICS
#ifndef STABILIZATION_INDI_ACT_FREQ_P
#error "You need to define GUIDANCE_INDI_THRUST_DYN_FREQ to be able to use indi vertical control"
#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
#else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
#endif
#endif // GUIDANCE_INDI_THRUST_DYN_FREQ
#endif //GUIDANCE_INDI_THRUST_DYNAMICS_FREQ


Expand Down Expand Up @@ -176,11 +174,13 @@ void guidance_indi_enter(void)
thrust_in = stabilization_cmd[COMMAND_THRUST];
thrust_act = thrust_in;

#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
#ifdef GUIDANCE_INDI_THRUST_DYNAMICS
thrust_dyn = GUIDANCE_INDI_THRUST_DYNAMICS;
#else
thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
#endif
#endif //GUIDANCE_INDI_THRUST_DYNAMICS
#endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN

float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
float sample_time = 1.0 / PERIODIC_FREQUENCY;
Expand Down
8 changes: 4 additions & 4 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
View file
Expand Up @@ -127,14 +127,14 @@ static void guidance_indi_filter_thrust(void);
#warning GUIDANCE_INDI_THRUST_DYNAMICS is deprecated, use GUIDANCE_INDI_THRUST_DYNAMICS_FREQ instead.
#warning "The thrust dynamics are now specified in continuous time with the corner frequency of the first order model!"
#warning "define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ in rad/s"
#warning "Use -log(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old value."
#warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old value."
#endif

#ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
#ifndef STABILIZATION_INDI_ACT_DYN_P
#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS to be able to use indi vertical control"
#ifndef STABILIZATION_INDI_ACT_FREQ_P
#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
#else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_DYN_P
#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
#endif
#endif //GUIDANCE_INDI_THRUST_DYNAMICS_FREQ

Expand Down
4 changes: 3 additions & 1 deletion sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
View file
Expand Up @@ -723,7 +723,9 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)

/*Commit the actuator command*/
for (i = 0; i < INDI_NUM_ACT; i++) {
actuators_pprz[i] = (int16_t) indi_u[i];
if (i < COMMANDS_NB) {
stabilization_cmd[i] = (int32_t) indi_u[i];
}
}

// Set the stab_cmd to 42 to indicate that it is not used
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
View file
Expand Up @@ -34,11 +34,11 @@
#include "generated/airframe.h"

// Check critical global definitiones
#ifndef SERVO_MOTOR_THROTTLE
#ifndef SERVO_MOTOR_THROTTLE_IDX
#error "Steering rover firmware requires the servo MOTOR_THROTTLE"
#endif

#ifndef SERVO_MOTOR_STEERING
#ifndef SERVO_MOTOR_STEERING_IDX
#error "Steering rover firmware requires the servo MOTOR_STEERING"
#endif

Expand Down
3 changes: 0 additions & 3 deletions sw/airborne/modules/actuators/actuators.c
View file
Expand Up @@ -48,9 +48,6 @@ static void send_actuators(struct transport_tx *trans, struct link_device *dev)

int16_t actuators[ACTUATORS_NB];

// Can be used to directly control each actuator from the control algorithm
int16_t actuators_pprz[ACTUATORS_NB];

uint32_t actuators_delay_time;
bool actuators_delay_done;

Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/actuators/actuators.h
View file
Expand Up @@ -66,7 +66,7 @@ extern int16_t actuators[ACTUATORS_NB];
* Can be used to directly control actuators from the control algorithm
* if the command_laws are set up appropriately in the airframe file
*/
extern int16_t actuators_pprz[ACTUATORS_NB];
//extern int16_t actuators_pprz[ACTUATORS_NB];

/** Set actuators.
* @param _n actuators name as given in airframe file, servos section
Expand Down
8 changes: 4 additions & 4 deletions sw/airborne/modules/actuators/actuators_asctec_v2.c
View file
Expand Up @@ -109,10 +109,10 @@ void actuators_asctec_v2_set(void)
actuators_asctec_v2.i2c_trans.len_w = 4;
break;
case NONE:
actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.cmds[SERVO_FRONT];
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cmds[SERVO_BACK];
actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.cmds[SERVO_LEFT];
actuators_asctec_v2.i2c_trans.buf[3] = actuators_asctec_v2.cmds[SERVO_RIGHT];
actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.cmds[SERVO_FRONT_IDX];
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cmds[SERVO_BACK_IDX];
actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.cmds[SERVO_LEFT_IDX];
actuators_asctec_v2.i2c_trans.buf[3] = actuators_asctec_v2.cmds[SERVO_RIGHT_IDX];
actuators_asctec_v2.i2c_trans.buf[4] = 0xAA + actuators_asctec_v2.i2c_trans.buf[0] +
actuators_asctec_v2.i2c_trans.buf[1] +
actuators_asctec_v2.i2c_trans.buf[2] +
Expand Down
120 changes: 84 additions & 36 deletions sw/airborne/modules/actuators/actuators_uavcan.c
View file
Expand Up @@ -49,13 +49,12 @@ struct actuators_uavcan_telem_t {
float position;
};

/* The transmitted actuator values */
#ifdef SERVOS_UAVCAN1_NB
int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB];
static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB] = {0};
#endif
#ifdef SERVOS_UAVCAN2_NB
int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB];
static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB] = {0};
#endif
#ifdef SERVOS_UAVCAN1CMD_NB
int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB];
Expand All @@ -64,6 +63,23 @@ int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB];
int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB];
#endif

/* Set the actual telemetry length (ID's from actuators can't collide with the command version) */
#if SERVOS_UAVCAN1CMD_NB > SERVOS_UAVCAN1_NB
#define UAVCAN1_TELEM_NB SERVOS_UAVCAN1CMD_NB
static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1CMD_NB] = {0};
#elif defined(SERVOS_UAVCAN1_NB)
#define UAVCAN1_TELEM_NB SERVOS_UAVCAN1_NB
static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB] = {0};
#endif

#if SERVOS_UAVCAN2CMD_NB > SERVOS_UAVCAN2_NB
#define UAVCAN2_TELEM_NB SERVOS_UAVCAN2CMD_NB
static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2CMD_NB] = {0};
#elif defined(SERVOS_UAVCAN2_NB)
#define UAVCAN2_TELEM_NB SERVOS_UAVCAN2_NB
static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB] = {0};
#endif

/* UNUSED value for CMD */
#define UAVCAN_CMD_UNUSED (MIN_PPRZ-1)

Expand Down Expand Up @@ -112,8 +128,8 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) {

// Find the next set telemetry
uint8_t offset = 0;
#ifdef SERVOS_UAVCAN1_NB
for(uint8_t i = esc_idx - offset; i < SERVOS_UAVCAN1_NB; i++) {
#ifdef UAVCAN1_TELEM_NB
for(uint8_t i = esc_idx - offset; i < UAVCAN1_TELEM_NB; i++) {
if(uavcan1_telem[i].set) {
old_idx = i + offset;
esc_idx = i + offset + add_idx;
Expand All @@ -122,10 +138,10 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) {
esc_idx = i + offset + 1;
}
}
offset += SERVOS_UAVCAN1_NB;
offset += UAVCAN1_TELEM_NB;
#endif
#ifdef SERVOS_UAVCAN2_NB
for(uint8_t i = esc_idx - offset; i < SERVOS_UAVCAN2_NB; i++) {
#ifdef UAVCAN2_TELEM_NB
for(uint8_t i = esc_idx - offset; i < UAVCAN2_TELEM_NB; i++) {
if(uavcan2_telem[i].set) {
old_idx = i + offset;
esc_idx = i + offset + add_idx;
Expand All @@ -134,7 +150,7 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) {
esc_idx = i + offset + 1;
}
}
offset += SERVOS_UAVCAN2_NB;
offset += UAVCAN2_TELEM_NB;
#endif

// Going round or no telemetry found
Expand Down Expand Up @@ -173,16 +189,16 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR

struct actuators_uavcan_telem_t *telem = NULL;
uint8_t max_id = 0;
#ifdef SERVOS_UAVCAN1_NB
#ifdef UAVCAN1_TELEM_NB
if (iface == &uavcan1) {
telem = uavcan1_telem;
max_id = SERVOS_UAVCAN1_NB;
max_id = UAVCAN1_TELEM_NB;
}
#endif
#ifdef SERVOS_UAVCAN2_NB
#ifdef UAVCAN2_TELEM_NB
if (iface == &uavcan2) {
telem = uavcan2_telem;
max_id = SERVOS_UAVCAN2_NB;
max_id = UAVCAN2_TELEM_NB;
}
#endif

Expand All @@ -206,37 +222,53 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR
#if UAVCAN_ACTUATORS_USE_CURRENT
// Update total current
electrical.current = 0;
#ifdef SERVOS_UAVCAN1_NB
for (uint8_t i = 0; i < SERVOS_UAVCAN1_NB; ++i) {
#ifdef UAVCAN1_TELEM_NB
for (uint8_t i = 0; i < UAVCAN1_TELEM_NB; ++i) {
electrical.current += uavcan1_telem[i].current;
}
#endif
#ifdef SERVOS_UAVCAN2_NB
for (uint8_t i = 0; i < SERVOS_UAVCAN2_NB; ++i) {
#ifdef UAVCAN2_TELEM_NB
for (uint8_t i = 0; i < UAVCAN2_TELEM_NB; ++i) {
electrical.current += uavcan2_telem[i].current;
}
#endif
#endif

// Feedback ABI RPM messages
#ifdef SERVOS_UAVCAN1_NB
#ifdef UAVCAN1_TELEM_NB
if (iface == &uavcan1) {
struct act_feedback_t feedback;
feedback.idx = SERVOS_UAVCAN1_OFFSET + esc_idx;
struct act_feedback_t feedback = {0};
feedback.rpm = telem[esc_idx].rpm;
feedback.set.rpm = true;

#ifdef SERVOS_UAVCAN1_NB
feedback.idx = get_servo_idx_UAVCAN1(esc_idx);
#endif
#ifdef SERVOS_UAVCAN1CMD_NB
if(esc_idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[esc_idx] != UAVCAN_CMD_UNUSED) {
feedback.idx = get_servo_idx_UAVCAN1CMD(esc_idx);
}
#endif

// Send ABI message
AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
}
#endif
#ifdef SERVOS_UAVCAN2_NB
#ifdef UAVCAN2_TELEM_NB
if (iface == &uavcan2) {
struct act_feedback_t feedback;
feedback.idx = SERVOS_UAVCAN2_OFFSET + esc_idx;
struct act_feedback_t feedback = {0};
feedback.rpm = telem[esc_idx].rpm;
feedback.set.rpm = true;

#ifdef SERVOS_UAVCAN2_NB
feedback.idx = get_servo_idx_UAVCAN2(esc_idx);
#endif
#ifdef SERVOS_UAVCAN2CMD_NB
if(esc_idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[esc_idx] != UAVCAN_CMD_UNUSED) {
feedback.idx = get_servo_idx_UAVCAN2CMD(esc_idx);
}
#endif

// Send ABI message
AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
}
Expand All @@ -253,16 +285,16 @@ static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, Ca

struct actuators_uavcan_telem_t *telem = NULL;
uint8_t max_id = 0;
#ifdef SERVOS_UAVCAN1_NB
#ifdef UAVCAN1_TELEM_NB
if (iface == &uavcan1) {
telem = uavcan1_telem;
max_id = SERVOS_UAVCAN1_NB;
max_id = UAVCAN1_TELEM_NB;
}
#endif
#ifdef SERVOS_UAVCAN2_NB
#ifdef UAVCAN2_TELEM_NB
if (iface == &uavcan2) {
telem = uavcan2_telem;
max_id = SERVOS_UAVCAN2_NB;
max_id = UAVCAN2_TELEM_NB;
}
#endif

Expand All @@ -276,25 +308,41 @@ static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, Ca
canardDecodeScalar(transfer, 8, 16, true, (void *)&tmp_float);
telem[actuator_idx].position = canardConvertFloat16ToNativeFloat(tmp_float);

#ifdef SERVOS_UAVCAN1_NB
#ifdef UAVCAN1_TELEM_NB
if (iface == &uavcan1) {
struct act_feedback_t feedback;
feedback.idx = SERVOS_UAVCAN1_OFFSET + actuator_idx;
struct act_feedback_t feedback = {0};
feedback.position = telem[actuator_idx].position;
feedback.set.position = true;

#ifdef SERVOS_UAVCAN1_NB
feedback.idx = get_servo_idx_UAVCAN1(actuator_idx);
#endif
#ifdef SERVOS_UAVCAN1CMD_NB
if(actuator_idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[actuator_idx] != UAVCAN_CMD_UNUSED) {
feedback.idx = get_servo_idx_UAVCAN1CMD(actuator_idx);
}
#endif

// Send ABI message
AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
}
#endif

#ifdef SERVOS_UAVCAN2_NB
#ifdef UAVCAN2_TELEM_NB
if (iface == &uavcan2) {
struct act_feedback_t feedback;
feedback.idx = SERVOS_UAVCAN2_OFFSET + actuator_idx;
struct act_feedback_t feedback = {0};
feedback.position = telem[actuator_idx].position;
feedback.set.position = true;

#ifdef SERVOS_UAVCAN2_NB
feedback.idx = get_servo_idx_UAVCAN2(actuator_idx);
#endif
#ifdef SERVOS_UAVCAN2CMD_NB
if(actuator_idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[actuator_idx] != UAVCAN_CMD_UNUSED) {
feedback.idx = get_servo_idx_UAVCAN2CMD(actuator_idx);
}
#endif

// Send ABI message
AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
}
Expand All @@ -311,16 +359,16 @@ static void actuators_uavcan_device_temperature_cb(struct uavcan_iface_t *iface,

struct actuators_uavcan_telem_t *telem = NULL;
uint8_t max_id = 0;
#ifdef SERVOS_UAVCAN1_NB
#ifdef UAVCAN1_TELEM_NB
if (iface == &uavcan1) {
telem = uavcan1_telem;
max_id = SERVOS_UAVCAN1_NB;
max_id = UAVCAN1_TELEM_NB;
}
#endif
#ifdef SERVOS_UAVCAN2_NB
#ifdef UAVCAN2_TELEM_NB
if (iface == &uavcan2) {
telem = uavcan2_telem;
max_id = SERVOS_UAVCAN2_NB;
max_id = UAVCAN2_TELEM_NB;
}
#endif

Expand Down