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

feat: add ANDI stabilization controller (#3578)
Issues due date / Add labels to issues (push) Has been cancelled
Details
Doxygen / build (push) Has been cancelled
Details

Browse Source
This commit is contained in:
jpgdubois
2026-02-06 22:02:54 +01:00
committed by GitHub
parent ffaef0e684
commit 4400506292
24 changed files with 5464 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/JPG/jpg_cyclone.xml
+283
View File
@@ -0,0 +1,283 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="Cyclone2">
<description>
Cyclone 2 airframe
+ Model: Cyclone 2 is a tiltbody wing with a ~1m wingspan
+ Autopilot: Cube orange
+ Actuators: Feetech sts3032
<!-- + GNSS Ublox F9P GNSS with RTK -->
+ MAG ΡΜ3100
+ ESC: KISS ECSs
+ RCRX: ELRS RP1
+ AIRSPEED: MS4525DO
+ TELEMETRY: RFDesign 868X
+ CURRENT: Standard FC Power sensor
+ RANGER: None
+ MOTOR: 2 x 1250KV Outrunner Brushless
+ PROP: 2 blade thin electric 8 x 6
+ BAT: Li-Po 4S1P 2300mAh
WARNING: Current PPRZ airspeed sensor driver has a bug,
static and dynamic pressure defaults are swapped,
so the airspeed is negative if you dont change your pitot leads
NOTES:
+ Using oneloop ANDI
+ Servos powered via external BEC (4s to 9 V)
+ Flightcontroller powered via FC Powerboard
+ Telemetry powerd via BEC of FC Powerboard
</description>
<firmware name="rotorcraft">
<autopilot name="jpg_cyclone_andi.xml"/>
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="NAVIGATION_FREQUENCY" value="50"/>
<target name="ap" board="cube_orange">
<module name="radio_control" type="sbus">
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="AP_MODE_SWITCH" value="RADIO_AUX3"/>
</module>
<module name="sys_mon"/>
<module name="flight_recorder"/>
<!-- EKF EKF2 configure inputs -->
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="INS_EKF2_IMU_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_OPTITRACK" value="TRUE"/> <!-- Not sure if it is actually using Optitrack here. Flies just fine with Optitrack Z-up instead of Y-up-->
</target>
<target name="nps" board="pc">
<module name="fdm" type="jsbsim">
<define name="NPS_JSBSIM_MODEL" value="cyclone" type="string"/>
<define name="NPS_SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="NPS_USE_COMMANDS" value="TRUE"/>
<define name="NPS_NO_MOTOR_MIXING" value="TRUE"/>
</module>
<module name="radio_control" type="datalink">
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="AP_MODE_SWITCH" value="RADIO_AUX7"/>
</module>
</target>
<module name="imu" type="cube"/>
<module name="air_data"/>
<module name="ins" type="ekf2">
</module>
<module name="actuators" type="t4"/>
<module name="actuators_t4_uart">
<configure name="ACTUATORS_T4_PORT" value="UART8"/>
<configure name="ACTUATORS_T4_BAUD" value="B921600"/>
</module>
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="gps" type="datalink"/>
<!-- GPS has been removed, dont use this without reinstalling gps-->
<!-- <module name="gps" type="ublox">
<configure name="GPS_BAUD" value="B460800"/>
</module> -->
<module name="mag" type="rm3100">
<configure name="MAG_RM3100_I2C_DEV" value="i2c2"/>
<define name="MODULE_RM3100_SYNC_SEND" value="TRUE"/>
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
<!-- 0:X, 1:Y, 2:Z; -->
<define name="RM3100_CHAN_X" value="1"/>
<define name="RM3100_CHAN_Y" value="0"/>
<define name="RM3100_CHAN_Z" value="2"/>
<define name="RM3100_CHAN_X_SIGN" value="+"/>
<define name="RM3100_CHAN_Y_SIGN" value="+"/>
<define name="RM3100_CHAN_Z_SIGN" value="-"/>
<!--TODO: Set correct angles-->
<!-- <define name="RM3100_MAG_TO_IMU_PHI" value="RadOfDeg\(0.0\)"/>
<define name="RM3100_MAG_TO_IMU_THETA" value="RadOfDeg\(0.0\)"/>
<define name="RM3100_MAG_TO_IMU_PSI" value="RadOfDeg\(0.0\)"/> -->
</module>
<!-- Control -->
<module name="stabilization" type="andi">
<define name="WLS_N_U_MAX" value="4" />
<define name="WLS_N_V_MAX" value="4" />
</module>
<module name="guidance" type="indi_hybrid_tailsitter">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="NAV_HYBRID_POS_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.5"/>
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.8"/>
<define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.20"/>
<define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
<define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/>
<!--not used-->
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="3000"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1500"/>
<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_FREQ" value="23.6"/>
<define name="GUIDANCE_INDI_FILTER_CUTOFF" value="1.5"/>
</module>
<!-- <module name="nav" type="hybrid">
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<define name="NAV_NO_CARROT" value="TRUE"/>
</module> -->
<module name="wls">
<define name="WLS_N_U_MAX" value = "4"/>
<define name="WLS_N_V_MAX" value = "4"/>
</module>
</firmware>
<servos driver="T4">
<servo name="S_ELEVON_LEFT" no="1" min="-5000" neutral="0" max="5000"/> <!-- [50.0 deg]-->
<servo name="S_ELEVON_RIGHT" no="6" min="5000" neutral="0" max="-5000"/> <!-- [50.0 deg]-->
<servo name="S_MOTOR_LEFT" no="13" min="0" neutral="0" max="1999"/> <!-- neutral is value when motor is not spinning, must be 0 -->
<servo name="S_MOTOR_RIGHT" no="14" min="0" neutral="0" max="1999"/>
</servos>
<commands>
<!-- Real actuators commands -->
<axis name="ELEVON_LEFT" group="REAL" failsafe_value="0"/>
<axis name="ELEVON_RIGHT" group="REAL" failsafe_value="0"/>
<axis name="MOTOR_LEFT" group="REAL" failsafe_value="0"/>
<axis name="MOTOR_RIGHT" group="REAL" failsafe_value="0"/>
<!-- Legacy commands-->
<axis name="YAW" group="OTHER" failsafe_value="0"/>
<axis name="THRUST" group="OTHER" failsafe_value="0"/>
</commands>
<!-- Safety critical kill switch implementation, DO NOT REMOVE -->
<command_laws>
<set servo="S_ELEVON_LEFT" value="autopilot_get_motors_on()? commands[COMMAND_ELEVON_LEFT] : 0"/>
<set servo="S_ELEVON_RIGHT" value="autopilot_get_motors_on()? commands[COMMAND_ELEVON_RIGHT] : 0"/>
<set servo="S_MOTOR_LEFT" value="autopilot_get_motors_on()? commands[COMMAND_MOTOR_LEFT] : -MAX_PPRZ"/>
<set servo="S_MOTOR_RIGHT" value="autopilot_get_motors_on()? commands[COMMAND_MOTOR_RIGHT] : -MAX_PPRZ"/>
</command_laws>
<section name="MISC">
<define name="TAILSITTER" value="TRUE"/>
<define name="USE_AIRSPEED" value="FALSE"/>
<define name="STABILIZATION_ATTITUDE_SP_MAX_PHI" value="45." unit="deg" />
<define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" />
<define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/>
<define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" />
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="1500" />
<define name="AUTOPILOT_IN_FLIGHT_MIN_SPEED" value="0" />
<define name="AUTOPILOT_IN_FLIGHT_MIN_ACCEL" value="0" />
</section>
<section name="STABILIZATION_ANDI" prefix="STABILIZATION_ANDI_" >
<define name="SCHEDULE_EFF" value="TRUE"/>
<define name="USE_STATE_DYNAMICS" value="TRUE"/>
<define name="RELAX_OBM" value="1.0f"/>
<!-- | EL | ER | ML^2 | MR^2 | -->
<define name="ACT_IS_SERVO" value="{ 1, 1, 0, 0}"/>
<define name="ACT_DYNAMICS" value="{ 20.0f, 20.0f, 35.0f, 35.0f}"/> <!-- [rad/s]-->
<define name="ACT_DELAY" value="{ 0, 0, 0, 0}"/> <!-- [rad/s]-->
<define name="ACT_USE_MEAS" value="{ true, true, true, true}"/>
<define name="ACT_MIN" value="{ -0.87f, -0.87f, 40000.0f, 40000.0f}"/> <!-- [ rad | (rad/s)^2]-->
<define name="ACT_MAX" value="{ 0.87f, 0.87f, 3534400.0f, 3534400.0f}"/> <!-- [ rad | (rad/s)^2]-->
<define name="ACT_RATE_MIN" value="{ -10.0f, -10.0f, -90000000.0f, -90000000.0f}"/> <!-- [rad/s | (rad/s)^2/s]-->
<define name="ACT_RATE_MAX" value="{ 10.0f, 10.0f, 90000000.0f, 90000000.0f}"/> <!-- [rad/s | (rad/s)^2/s]-->
<define name="ACT_PREF" value="{ 0.00f, 0.00f, 705600.0f, 705600.0f}"/> <!-- [ rad | (rad/s)^2]--> <!-- Used as operating point when not scheduling-->
<define name="WLS_WU" value="{ 0.0f, 0.0f, 0.0f, 0.0f}"/>
<!-- | P | Q | R | Tau | -->
<define name="WLS_WV" value="{ 1000.0f, 100.0f, 1.0f, 10.0f}"/> <!-- Roll is more important than pitch, we do not want to sacrefice roll control in case failing to reach a certain pitch command -->
<!-- | P | Q | R | -->
<define name="RC_RATE_MAX" value="{ 5.0f, 5.0f, 5.0f}"/> <!-- [rad/s]-->
<define name="THRUST_MIN" value="0.6f" /> <!-- [m/s^2]-->
<define name="THRUST_MAX" value="52.0f" /> <!-- [m/s^2]-->
<define name="CUTOFF_FREQ_ACCEL" value=" 20.0f"/> <!-- [rad/s]-->
<define name="CUTOFF_FREQ_VEL" value=" 80.0f"/> <!-- [rad/s]-->
<define name="CUTOFF_FREQ_OMEGA_DOT" value=" 20.0f"/> <!-- [rad/s]-->
<define name="CUTOFF_FREQ_OMEGA" value=" 80.0f"/> <!-- [rad/s]-->
</section>
<section name="BAT">
<define name="MAX_BAT_CAPACITY" value="2300" unit="mAh"/>
<!--<define name="MilliAmpereOfAdc(_adc)" value="(_adc-632)*4.14"/>-->
<define name="VoltageOfAdc(_adc)" value="(0.000611456 * _adc)"/>
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="280" unit="mA"/>
<define name="MILLIAMP_AT_FULL_THROTTLE" value="20000" unit="mA"/>
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="14.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="13.5" unit="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="13.2" unit="V"/>
<define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.0"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="MAG_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="5"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
<!-- <field name="rotation" value="true"/> -->
</field>
<field name="neutral" type="int[]" value="-2044,169,119"/>
<field name="scale" type="int[][]" value="{{5666,15649,36949},{9069,27419,63039}}"/>
<!-- <field name="body_to_sensor" value="{{0, 16384,0, 0, 0, -16384,-16384, 0, 0}}"/> -->
</field>
</define>
<!-- <define name="ACCEL_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="20"/>
<field name="calibrated" type="struct">
<field name="neutral" value="false"/>
<field name="scale" value="false"/>
<field name="rotation" value="true"/>
</field>
<field name="body_to_sensor" value="{{0, 0, -16384, 0, 16384, 0, 0, 16384, 0}}"/>
</field>
</define> -->
<!-- Gyrometer calibration (NOT DONE!) -->
<!-- <define name="GYRO_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="20"/>
<field name="calibrated" type="struct">
<field name="neutral" value="false"/>
<field name="scale" value="false"/>
<field name="rotation" value="true"/>
</field>
<field name="body_to_sensor" value="{{0, 0, -16384, 0, 16384, 0, 0, 16384, 0}}"/>
</field>
</define> -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
</airframe>
conf/autopilot/jpg_cyclone_andi.xml
+194
View File
@@ -0,0 +1,194 @@
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
<autopilot name="ANDI Autopilot Cyclone">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<modules>
<module name="actuators" type="t4_uart"/>
<module name="stabilization" type="andi"/>
</modules>
<includes>
<include name="autopilot_rc_helpers.h"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT" cond="ifndef MODE_MANUAL"/>
<define name="MODE_AUTO1" value="AP_MODE_RATE_DIRECT" cond="ifndef MODE_AUTO1"/>
<define name="MODE_AUTO2" value="AP_MODE_RATE_DIRECT" cond="ifndef MODE_AUTO2"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
</includes>
<settings>
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" values="Resurrect|Kill"/> <!-- handler="KillThrottle" -->
</settings>
<!-- Commit actuator commands. ANDI/INDI perform allocation themselves; retained so COMMAND_THRUST remains set -->
<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</control_block>
<control_block name="run_rate_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_andi_run(true, autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="run_attitude_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_andi_run(false, autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<mode_selection>
<mode_select cond="RCMode0()" mode="MODE_MANUAL"/>
<mode_select cond="RCMode1()" mode="MODE_AUTO1"/>
<mode_select cond="RCMode2()" mode="MODE_AUTO2"/>
</mode_selection>
<!-- <exceptions>
<exception cond="nav.too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions> -->
<!-- KILL: 0 -->
<mode name="KILL">
<select cond="$DEFAULT_MODE"/>
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>
<!-- FAILSAFE: 1 -->
<mode name="FAILSAFE" shortname="FAIL">
<!-- Failsafe does not work needs to be fixed-->
</mode>
<!-- HOME: 2 (UNUSED) -->
<mode name="HOME">
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
<!-- RATE_DIRECT: 3 -->
<mode name="RATE_DIRECT" shortname="RATE">
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call fun="stabilization_andi_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="run_rate_control"/>
<!-- <call_block name="set_commands"/> -->
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATITUDE_DIRECT: 4 -->
<mode name="ATTITUDE_DIRECT" shortname="ATT">
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call fun="stabilization_andi_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="run_attitude_control"/>
<!-- <call fun="actuator_debug(true, false, 2)"/> For debugging actuators -->
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- RATE_RC_CLIMB: 5 (UNUSED) -->
<mode name="RATE_RC_CLIMB" shortname="RRCC">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATTITUDE_RC_CLIMB: 6 (UNUSED) -->
<mode name="ATTITUDE_RC_CLIMB" shortname="ATTITUDE_RC_CLIMB">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATTITUDE_CLIMB: 7 (UNUSED) -->
<mode name="ATTITUDE_CLIMB" shortname="ATTITUDE_CLIMB">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- RATE_Z_HOLD: 8 (UNUSED) -->
<mode name="RATE_Z_HOLD" shortname="RATE_Z_HOLD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATTITUDE_Z_HOLD: (UNUSED) 9 -->
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- HOVER_DIRECT: 10 (UNUSED) -->
<mode name="HOVER_DIRECT" shortname="HOVER_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- HOVER_CLIMB: 11 (UNUSED) -->
<mode name="HOVER_CLIMB" shortname="HOVER_CLIMB">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- HOVER_Z_HOLD: 12 (UNUSED) -->
<mode name="HOVER_Z_HOLD" shortname="HOVER_Z_HOLD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- NAV: 13 -->
<mode name="NAV" shortname="NAV">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- RC_DIRECT: 14 (UNUSED) -->
<mode name="RC_DIRECT" shortname="RC_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- CARE_FREE_DIRECT: 15 (UNUSED) -->
<mode name="CARE_FREE_DIRECT" shortname="CARE_FREE_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- FORWARD: 16 (UNUSED) -->
<mode name="FORWARD" shortname="FORWARD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- MODULE: 17 (UNUSED) -->
<mode name="MODULE" shortname="MODULE">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- FLIP: 18 (UNUSED) -->
<mode name="FLIP" shortname="FLIP">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- GUIDED: 19 (UNUSED) -->
<mode name="GUIDED">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATTITUDE_DIRECT_FWD: 20 (UNUSED) -->
<mode name="ATTITUDE_DIRECT_FWD" shortname="ATT_Fwd">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
</state_machine>
</autopilot>
conf/flight_plans/JPG/jpg_cyberzoo_basic.xml
+32
View File
@@ -0,0 +1,32 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="3.5" ground_alt="2" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Cyberzoo (Delft) no GPS" security_height="0.3">
<header>
#include "autopilot.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="CLIMB" x="1.2" y="-0.6"/>
<waypoint name="GOAL" x="2.0" y="2.0"/>
<waypoint name="STDBY" x="-0.7" y="-0.8"/>
<waypoint name="TD" x="0.8" y="-1.7"/>
<waypoint lat="51.9906213" lon="4.3768628" name="FA1"/>
<waypoint lat="51.9905874" lon="4.3767766" name="FA2"/>
<waypoint lat="51.9906409" lon="4.3767226" name="FA3"/>
<waypoint lat="51.9906737" lon="4.3768074" name="FA4"/>
</waypoints>
<sectors>
<sector color="red" name="Flight_Area">
<corner name="FA4"/>
<corner name="FA3"/>
<corner name="FA2"/>
<corner name="FA1"/>
</sector>
</sectors>
<blocks>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/modules/stabilization_andi.xml
+170
View File
@@ -0,0 +1,170 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="stabilization_andi" dir="stabilization" task="control">
<doc>
<description>
Full ANDI stabilization controller for rotorcraft
</description>
<configure name="ANDI_OUTPUTS" default="4"/>
<configure name="ANDI_NUM_ACT" default="4"/>
<section name="STABILIZATION_ATTITUDE_ANDI" prefix="STABILIZATION_ANDI_">
<define name="SCHEDULE_EFF" value="0" description="1 to schedule control effectiveness based on throttle command. 0 uses ACT_PREF to compute constant control effectiveness"/>
<define name="USE_STATE_DYNAMICS" value="0" description="1 to use obm state derivative term, 0 to neglect this term."/>
<define name="RELAX_OBM" value="1.0f" description="relaxation factor for obm state derivative contribution."/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 0}" description="1 for every actuator that is a servo"/>
<define name="ACT_DYNAMICS" description="actuator bandwidth [rad/s]"/>
<define name="ACT_DELAY" description="actuator transport delay [samples]"/>
<define name="ACT_MAX" description="maximum actuator value [SI units]"/>
<define name="ACT_MIN" description="maximum actuator value [SI units]"/>
<define name="ACT_RATE_MAX" description="minimum actuator rate [SI units]"/>
<define name="ACT_RATE_MIN" description="minimum actuator rate [SI units]"/>
<define name="ACT_PREF" value="{0.0, 0.0, 0.0, 0.0}" description="preferred actuator value used for unscheduled control effectiveness"/>
<define name="WLS_WV" value="{1000, 1000, 1, 100}" description="WLS control objective priorities: roll, pitch, yaw, thrust (size of ANDI_OUTPUTS)"/>
<define name="WLS_WU" value="{1, 1, 1, 1}" description="WLS actuator cost (size of ANDI_NUM_ACT)"/>
</section>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="andi">
<dl_setting var="andi_p_rate_rm.omega_a.x" min="0" step="0.25" max="100" shortname="pole_rate_rm/omega_a.x" param="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_X"/>
<dl_setting var="andi_p_rate_rm.omega_a.y" min="0" step="0.25" max="100" shortname="pole_rate_rm/omega_a.y" param="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Y"/>
<dl_setting var="andi_p_rate_rm.omega_a.z" min="0" step="0.25" max="100" shortname="pole_rate_rm/omega_a.z" param="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Z"/>
<dl_setting var="andi_p_rate_rm.zeta.x" min="0" step="0.25" max="5" shortname="pole_rate_rm/zeta.x" param="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_X"/>
<dl_setting var="andi_p_rate_rm.zeta.y" min="0" step="0.25" max="5" shortname="pole_rate_rm/zeta.y" param="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Y"/>
<dl_setting var="andi_p_rate_rm.zeta.z" min="0" step="0.25" max="5" shortname="pole_rate_rm/zeta.z" param="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Z"/>
<dl_setting var="andi_p_att_rm.omega_n.x" min="0" step="0.25" max="100" shortname="pole_att_rm/omega_n.x" param="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_X"/>
<dl_setting var="andi_p_att_rm.omega_n.y" min="0" step="0.25" max="100" shortname="pole_att_rm/omega_n.y" param="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Y"/>
<dl_setting var="andi_p_att_rm.omega_n.z" min="0" step="0.25" max="100" shortname="pole_att_rm/omega_n.z" param="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Z"/>
<dl_setting var="andi_p_att_rm.zeta.x" min="0" step="0.25" max="5" shortname="pole_att_rm/zeta.x" param="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_X"/>
<dl_setting var="andi_p_att_rm.zeta.y" min="0" step="0.25" max="5" shortname="pole_att_rm/zeta.y" param="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Y"/>
<dl_setting var="andi_p_att_rm.zeta.z" min="0" step="0.25" max="5" shortname="pole_att_rm/zeta.z" param="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Z"/>
<dl_setting var="andi_p_att_rm.omega_a.x" min="0" step="0.25" max="100" shortname="pole_att_rm/p1.x" param="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_X"/>
<dl_setting var="andi_p_att_rm.omega_a.y" min="0" step="0.25" max="100" shortname="pole_att_rm/p1.y" param="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Y"/>
<dl_setting var="andi_p_att_rm.omega_a.z" min="0" step="0.25" max="100" shortname="pole_att_rm/p1.z" param="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Z"/>
<dl_setting var="andi_p_thrust_rm" min="0" step="0.25" max="100" shortname="pole_thrust_rm" param="STABILIZATION_ANDI_POLE_THRUST_RM"/>
<dl_setting var="andi_p_rate_ec.omega_a.x" min="0" step="0.25" max="100" shortname="pole_rate_ec/omega_a.x" param="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_X"/>
<dl_setting var="andi_p_rate_ec.omega_a.y" min="0" step="0.25" max="100" shortname="pole_rate_ec/omega_a.y" param="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Y"/>
<dl_setting var="andi_p_rate_ec.omega_a.z" min="0" step="0.25" max="100" shortname="pole_rate_ec/omega_a.z" param="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Z"/>
<dl_setting var="andi_p_rate_ec.zeta.x" min="0" step="0.25" max="5" shortname="pole_rate_ec/zeta.x" param="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_X"/>
<dl_setting var="andi_p_rate_ec.zeta.y" min="0" step="0.25" max="5" shortname="pole_rate_ec/zeta.y" param="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Y"/>
<dl_setting var="andi_p_rate_ec.zeta.z" min="0" step="0.25" max="5" shortname="pole_rate_ec/zeta.z" param="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Z"/>
<dl_setting var="andi_p_att_ec.omega_n.x" min="0" step="0.25" max="100" shortname="pole_att_ec/omega_n.x" param="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_X"/>
<dl_setting var="andi_p_att_ec.omega_n.y" min="0" step="0.25" max="100" shortname="pole_att_ec/omega_n.y" param="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Y"/>
<dl_setting var="andi_p_att_ec.omega_n.z" min="0" step="0.25" max="100" shortname="pole_att_ec/omega_n.z" param="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Z"/>
<dl_setting var="andi_p_att_ec.zeta.x" min="0" step="0.25" max="5" shortname="pole_att_ec/zeta.x" param="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_X"/>
<dl_setting var="andi_p_att_ec.zeta.y" min="0" step="0.25" max="5" shortname="pole_att_ec/zeta.y" param="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Y"/>
<dl_setting var="andi_p_att_ec.zeta.z" min="0" step="0.25" max="5" shortname="pole_att_ec/zeta.z" param="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Z"/>
<dl_setting var="andi_p_att_ec.omega_a.x" min="0" step="0.25" max="100" shortname="pole_att_ec/p1.x" param="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_X"/>
<dl_setting var="andi_p_att_ec.omega_a.y" min="0" step="0.25" max="100" shortname="pole_att_ec/p1.y" param="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Y"/>
<dl_setting var="andi_p_att_ec.omega_a.z" min="0" step="0.25" max="100" shortname="pole_att_ec/p1.z" param="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Z"/>
<dl_setting var="andi_p_thrust_ec" min="0" step="0.25" max="100" shortname="pole_thrust_ec" param="STABILIZATION_ANDI_POLE_THRUST_EC"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>stabilization_rotorcraft,@attitude_command,wls</depends>
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_andi.h"/>
</header>
<init fun="stabilization_andi_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_andi.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="obm_cyclone.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<configure name="ANDI_OUTPUTS" default="4"/>
<configure name="ANDI_NUM_ACT" default="4"/>
<define name="ANDI_OUTPUTS" value="$(ANDI_OUTPUTS)"/>
<define name="ANDI_NUM_ACT" value="$(ANDI_NUM_ACT)"/>
<define name="USE_STABILIZATION_RATE"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_X" default="35.0"/> <!-- Equivalent actuator bandwidth -->
<configure name="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Y" default="20.0"/> <!-- Equivalent actuator bandwidth -->
<configure name="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Z" default="20.0"/> <!-- Equivalent actuator bandwidth -->
<configure name="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_X" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Y" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Z" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_X" default="35.0"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Y" default="20.0"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Z" default="20.0"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_X" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Y" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Z" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_X" default="7.0"/> <!-- rad/s-->
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Y" default="7.0"/> <!-- rad/s-->
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Z" default="7.0"/> <!-- rad/s-->
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_X" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Y" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Z" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_X" default="35.0"/> <!-- Equivalent actuator bandwidth -->
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Y" default="20.0"/> <!-- Equivalent actuator bandwidth -->
<configure name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Z" default="20.0"/> <!-- Equivalent actuator bandwidth -->
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_X" default="7.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Y" default="7.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Z" default="7.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_X" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Y" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Z" default="1.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_X" default="35.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Y" default="20.0"/>
<configure name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Z" default="20.0"/>
<configure name="STABILIZATION_ANDI_POLE_THRUST_EC" default="35.0"/> <!-- Equivalent actuator bandwidth -->
<configure name="STABILIZATION_ANDI_POLE_THRUST_RM" default="35.0"/> <!-- Equivalent actuator bandwidth -->
<define name="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_X" value="$(STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_X)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Y" value="$(STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Y)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Z" value="$(STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Z)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_X" value="$(STABILIZATION_ANDI_POLE_RATE_EC_ZETA_X)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Y" value="$(STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Y)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Z" value="$(STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Z)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_X" value="$(STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_X)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Y" value="$(STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Y)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Z" value="$(STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Z)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_X" value="$(STABILIZATION_ANDI_POLE_RATE_RM_ZETA_X)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Y" value="$(STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Y)"/>
<define name="STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Z" value="$(STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Z)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_X" value="$(STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_X)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Y" value="$(STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Y)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Z" value="$(STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Z)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_X" value="$(STABILIZATION_ANDI_POLE_ATT_EC_ZETA_X)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Y" value="$(STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Y)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Z" value="$(STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Z)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_X" value="$(STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_X)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Y" value="$(STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Y)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Z" value="$(STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Z)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_X" value="$(STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_X)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Y" value="$(STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Y)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Z" value="$(STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Z)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_X" value="$(STABILIZATION_ANDI_POLE_ATT_RM_ZETA_X)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Y" value="$(STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Y)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Z" value="$(STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Z)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_X" value="$(STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_X)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Y" value="$(STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Y)"/>
<define name="STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Z" value="$(STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Z)"/>
<define name="STABILIZATION_ANDI_POLE_THRUST_EC" value="$(STABILIZATION_ANDI_POLE_THRUST_EC)"/>
<define name="STABILIZATION_ANDI_POLE_THRUST_RM" value="$(STABILIZATION_ANDI_POLE_THRUST_RM)"/>
</makefile>
</module>
conf/radios/JPG/jpg_elrs.xml
+27
View File
@@ -0,0 +1,27 @@
<!--
The order of the list below is of importance if you do not define a
"no=" (order in the PPM frame) parameter.
If you do not define this then the order of the PPM is the one of
the order of the functon in the list
-->
<!-- for example below function="AUX1" means that it defines RADIO_AUX1 as 4 (0 based counting)-->
<!DOCTYPE radio SYSTEM "../radio.dtd">
<radio name="Generic SBUS" data_min="986" data_max="2190" sync_min="5000" sync_max="15000" pulse_type="POSITIVE">
<channel function="THROTTLE" min="986" neutral="987" max="1900" average="0"/>
<channel function="ROLL" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="PITCH" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="YAW" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX1" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="MODE" min="1100" neutral="1500" max="1900" average="3"/>
<channel function="AUX2" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX3" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX4" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX5" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX6" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX7" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX8" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX9" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX10" min="1100" neutral="1500" max="1900" average="0"/>
<channel function="AUX11" min="1100" neutral="1500" max="1900" average="0"/>
</radio>
conf/telemetry/JPG/jpg_cyclone.xml
+252
View File
@@ -0,0 +1,252 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
<telemetry>
<process name="Main">
<mode name="default" key_press="d">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CAM" period="1."/>
<message name="GPS_INT" period=".25"/>
<message name="INS" period=".25"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="VISION_POSITION_ESTIMATE" period="0.1"/>
<message name="LOGGER_STATUS" period="5.1"/>
<message name="LIDAR" period="1.2"/>
<message name="INS_EKF2" period=".25"/>
<message name="WIND_INFO_RET" period="1."/>
<message name="AHRS_REF_QUAT" period="0.5"/>
<message name="STAB_ATTITUDE" period=".1"/>
<!-- <message name="STAB_THRUST" period=".1"/> -->
<!-- <message name="STAB_PSEUDO_COMMAND" period=".1"/> -->
<message name="GUIDANCE" period=".25"/>
<message name="ACTUATORS_T4_IN" period="0.1"/>
<message name="ACTUATORS_T4_OUT" period="0.1"/>
<message name="PPM" period="0.5"/>
<message name="PAYLOAD_FLOAT" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="WLS_V" period="0.1"/>
<message name="WLS_U" period="0.1"/>
<message name="SYS_MON" period="0.25"/>
<message name="EXTERNAL_POSE_DOWN" period="0.1"/>
<message name="AHRS_BIAS" period="1.0"/>
<!-- <message name="DEBUG_VECT" period="0.1"/> -->
<message name="COMMANDS" period="0.2"/>
</mode>
<mode name="ppm">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="BEBOP_ACTUATORS" period="0.2"/>
</mode>
<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="IMU_MAG_CURRENT_CALIBRATION" period=".05"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
<message name="AIRSPEED_RAW" period="0.1"/>
</mode>
<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>
<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<message name="AHRS_QUAT_INT" period=".25"/>
<message name="AHRS_EULER_INT" period=".1"/>
<message name="AHRS_EULER" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>
<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>
<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>
<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE" period=".25"/>
<!-- <message name="STAB_THRUST" period=".25"/> -->
<message name="EFF_MAT_STAB" period="2.0"/>
</mode>
<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VFF_EXTENDED" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS_Z" period=".05"/>
<message name="INS" period=".11"/>
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>
<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
<message name="GUIDANCE_H_REF_INT" period="0.062"/>
<message name="STAB_ATTITUDE_INT" period="0.4"/>
<message name="STAB_ATTITUDE_FLOAT" period="0.4"/>
<!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>
<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>
<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RELPOS" period="1"/>
</mode>
</process>
<process name="FlightRecorder">
<mode name="default">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="GPS" period="11.1"/>
<!-- <message name="DL_VALUE" period="2.5"/> -->
<message name="ROTORCRAFT_STATUS" period="0.02"/>
<message name="ROTORCRAFT_FP" period="0.002"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.20"/>
<message name="COMMANDS" period="0.002"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period=".1"/>
<message name="INS" period=".02"/>
<message name="INS_Z" period=".1"/>
<!-- <message name="I2C_ERRORS" period="4.1"/> -->
<!-- <message name="UART_ERRORS" period="3.1"/> -->
<message name="ENERGY" period="0.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="0.05"/>
<!-- <message name="SURVEY" period="2.5"/> -->
<message name="IMU_GYRO" period="0.002"/>
<message name="IMU_GYRO_SCALED" period="0.002"/>
<message name="IMU_ACCEL" period="0.002"/>
<message name="IMU_MAG_SCALED" period=".0125"/>
<!-- <message name="LIDAR" period="0.05"/> -->
<message name="INS_EKF2" period="0.05"/>
<message name="INS_EKF2_EXT" period="0.1"/>
<!-- <message name="WIND_INFO_RET" period="0.2"/> -->
<message name="AHRS_BIAS" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.002"/>
<message name="GUIDANCE_H_REF_INT" period="0.02"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.2"/>
<message name="HYBRID_GUIDANCE" period="0.02"/>
<message name="ESC" period="0.02"/>
<message name="STAB_ATTITUDE" period="0.01"/>
<!-- <message name="STAB_THRUST" period="0.01"/> -->
<!-- <message name="STAB_PSEUDO_COMMAND" period="0.01"/> -->
<!-- <message name="ON_BOARD_MODEL" period="0.01"/> -->
<message name="ACTUATORS_T4_IN" period="0.01"/>
<message name="ACTUATORS_T4_OUT" period="0.01"/>
<message name="GPS_RELPOS" period="0.1"/>
<message name="IMU_HEATER" period="6.2"/>
<!-- <message name="GUIDANCE" period="0.002"/> -->
<!-- <message name="EXTERNAL_POSE_DOWN" period="0.002"/> -->
<!-- <message name="DOUBLET" period="0.002"/> -->
<message name="DEBUG" period="0.02"/> <!-- For the parachute module -->
<message name="WLS_V" period="0.01"/>
<message name="WLS_U" period="0.01"/>
<message name="DEBUG_VECT" period="0.1"/>
<message name="PPM" period="0.1"/>
<message name="EXTERNAL_POSE_DOWN" period="0.02"/>
<message name="AHRS_BIAS" period="0.5"/>
</mode>
</process>
</telemetry>
conf/userconf/JPG/jpg_conf.xml
+13
View File
@@ -0,0 +1,13 @@
<conf>
<aircraft
name="Cyclone"
ac_id="44"
airframe="airframes/JPG/jpg_cyclone.xml"
radio="radios/JPG/jpg_elrs.xml"
telemetry="telemetry/JPG/jpg_cyclone.xml"
flight_plan="flight_plans/JPG/jpg_cyberzoo_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/guidance_indi_hybrid.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_rotorcraft.xml modules/stabilization_andi.xml"
gui_color="#b300c400ba00"
/>
</conf>
conf/userconf/JPG/jpg_control_panel.xml
+208
View File
@@ -0,0 +1,208 @@
<control_panel name="paparazzi control panel">
<section name="programs">
<!-- Examples of custom configuration. -->
<!--program name="Server" command="sw/ground_segment/tmtc/server">
<arg flag="-b" constant="192.168.1.255:2010"/>
</program>
<program name="GCS" command="sw/ground_segment/cockpit/gcs" icon="gcs.svg" favorite="True">
<arg flag="-layout" constant="large_left_col.xml"/>
<arg flag="-osm"/>
</program-->
</section>
<section name="sessions">
<session name="Flight UDP/WiFi">
<program name="Server"/>
<program name="PprgGCS"/>
<program name="Data Link">
<arg flag="-udp"/>
</program>
</session>
<session name="Flight USB-XBee-API@57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/paparazzi/xbee"/>
<arg flag="-transport" constant="xbee"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="PprzGCS"/>
</session>
<session name="Flight USB-serial Redundant">
<program name="Server"/>
<program name="PprzGCS"/>
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-id" constant="1"/>
<arg flag="-redlink"/>
</program>
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB1"/>
<arg flag="-id" constant="2"/>
<arg flag="-redlink"/>
</program>
<program name="Link Combiner"/>
</session>
<session name="Flight USB-serial@115200">
<program name="Messages"/>
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-s" constant="115200"/>
</program>
<program name="Server"/>
<program name="PprzGCS"/>
</session>
<session name="Flight USB-serial@57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="PprzGCS"/>
</session>
<session name="Flight USB-serial@9600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
</program>
<program name="Server"/>
<program name="PprzGCS"/>
</session>
<session name="HITL demo">
<program name="Hardware in the Loop">
<arg flag="-a" constant="@AIRCRAFT"/>
<arg flag="-t" constant="hitl"/>
</program>
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyACM1"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="GCS"/>
</session>
<session name="Messages and Settings">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="Messages"/>
<program name="Settings">
<arg flag="-ac" constant="@AIRCRAFT"/>
</program>
</session>
<session name="Raw Sensors">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="Messages">
<arg flag="-g" constant="300x400+0-220"/>
</program>
<program name="Settings">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="-g" constant="800x200+0-0"/>
</program>
<program name="Real-time Plotter">
<arg flag="-g" constant="1000x250-0+0"/>
<arg flag="-t" constant="ACC"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:ax"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:ay"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:az"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+250"/>
<arg flag="-t" constant="GYRO"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gp"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gq"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gr"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+500"/>
<arg flag="-t" constant="MAG"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:mx"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:my"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:mz"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+750"/>
<arg flag="-t" constant="BARO"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="101325.0"/>
<arg flag="-c" constant="*:telemetry:BARO_RAW:abs"/>
</program>
</session>
<session name="Scaled Sensors">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="Messages">
<arg flag="-g" constant="300x400+0-220"/>
</program>
<program name="Settings">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="-g" constant="800x200+0-0"/>
</program>
<program name="Real-time Plotter">
<arg flag="-g" constant="1000x250-0+0"/>
<arg flag="-t" constant="ACC"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="9.81"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="-9.81"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ax:0.0009766"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ay:0.0009766"/>
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:az:0.0009766"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+250"/>
<arg flag="-t" constant="GYRO"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gp:0.0139882"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gq:0.0139882"/>
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gr:0.0139882"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+500"/>
<arg flag="-t" constant="MAG"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mx:0.0004883"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:my:0.0004883"/>
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mz:0.0004883"/>
</program>
</session>
<session name="SupperBitRF">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyACM0"/>
</program>
<program name="Server"/>
<program name="PprzGCS"/>
<program name="Messages"/>
</session>
<session name="SupperBitRF cable telemetry">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyACM1"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="PprzGCS"/>
<program name="Messages"/>
<program name="Messages"/>
</session>
<session name="messages up down">
<program name="Messages">
<arg flag="-c" constant="ground"/>
</program>
</session>
</section>
</control_panel>
fix_code_style.sh
+32 -5
View File
@@ -10,13 +10,39 @@ if [ $# -eq 0 ]; then
exit 1
fi
ASTYLE_VERSION=`astyle --version 2>&1| awk '{print $4}'`
ASTYLE_VERSION=${ASTYLE_VERSION:0:4}
ASTYLE_VERSION=$(astyle --version 2>&1 | awk '{print $4}' | head -1)
echo "Using astyle version $ASTYLE_VERSION"
set -f
if [ $(bc <<< "$ASTYLE_VERSION >= 2.03") -eq 1 ]; then
# Safe version parsing with defaults
major=${ASTYLE_VERSION%%.*}
minor=$(echo "$ASTYLE_VERSION" | cut -d. -f2)
# Fix unary operator - quote variables and provide defaults
major=${major:-0}
minor=${minor:-0}
if [ "$major" -ge 3 ]; then
echo "Using AStyle 3.x options"
astyle --style=kr \
--indent=spaces=2 \
--convert-tabs \
--indent-switches \
--indent-preproc-block \
--pad-oper \
--pad-header \
--unpad-paren \
--keep-one-line-blocks \
--keep-one-line-statements \
--align-pointer=name \
--suffix=none \
--lineend=linux \
--ignore-exclude-errors-x \
--max-code-length=120 \
"$@"
elif [ "$major" -ge 2 ]; then
echo "Using AStyle 2.x options"
astyle --style=kr \
--indent=spaces=2 \
--convert-tabs \
@@ -33,8 +59,9 @@ if [ $(bc <<< "$ASTYLE_VERSION >= 2.03") -eq 1 ]; then
--add-brackets \
--ignore-exclude-errors-x \
--max-code-length=120 \
$*
"$@"
else
echo "Using AStyle 1.x options"
astyle --style=kr \
--indent=spaces=2 \
--convert-tabs \
@@ -49,5 +76,5 @@ else
--suffix=none \
--lineend=linux \
--add-brackets \
$*
"$@"
fi
sw/airborne/filters/complementary_filter.h
+307
View File
@@ -0,0 +1,307 @@
/*
*
* Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/** @file filters/complementary_filter.h
* @brief Implementation of complementary filters (first order, second order, and Butterworth variants)
*
* Provides structures and functions to initialize, update, reset, and get outputs from
* first order and second order complementary filters, as well as Butterworth complementary filters.
*
* FIXME: This implementation of complementary filters forces user to first integrate or differentiate
* the signals outside of the filter before passing them in. Consider extending the filter to handle integration
* and differentiation internally. Maybe implement cascaded complementary filters for this purpose.
*
* @author Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*/
#ifndef COMPLEMENTARY_FILTER_H
#define COMPLEMENTARY_FILTER_H
#include "std.h"
#include "filters/low_pass_filter.h"
/**
* @brief First order complementary filter structure.
*
* This structure contains two first order low-pass filters,
* one for the high-pass path and one for the low-pass path.
*/
struct FirstOrderComplementary {
struct FirstOrderLowPass x_lp_filter; // Low pass filter instance for high pass path
struct FirstOrderLowPass y_lp_filter; // Low pass filter instance for low pass path
};
/** Initialize the first order complementary filter.
*
* @param filter Complementary filter struct
* @param cut_off Time constant of the low-pass filter
* @param sample_time Sampling period
* @param value Initial value for filter history
*/
static inline void init_first_order_complementary(
struct FirstOrderComplementary *filter,
float cut_off, float sample_time,
float value)
{
init_first_order_low_pass(&filter->x_lp_filter, 1.0f / cut_off, sample_time, value);
init_first_order_low_pass(&filter->y_lp_filter, 1.0f / cut_off, sample_time, value);
}
/** Update the first order complementary filter with new input values.
* @param filter Complementary filter struct
* @param value_x New input value from the high-pass path
* @param value_y New input value from the low-pass path
* @return New filtered output value
*/
static inline float update_first_order_complementary(
struct FirstOrderComplementary *filter,
float value_x, float value_y)
{
float x_lp_output = update_first_order_low_pass(&filter->x_lp_filter, value_x);
float y_lp_output = update_first_order_low_pass(&filter->y_lp_filter, value_y);
return filter->x_lp_filter.last_in - x_lp_output + y_lp_output;
}
/**
* @brief Reset the first order complementary filter to a specific value.
*
* @param filter Complementary filter struct
* @param value Value to reset the filter to
* @return The reset value
*/
static inline float reset_first_order_complementary(
struct FirstOrderComplementary *filter,
float value)
{
reset_first_order_low_pass(&filter->x_lp_filter, value);
reset_first_order_low_pass(&filter->y_lp_filter, value);
return value;
}
/** Get current value of the first order complementary filter.
* @param filter Complementary filter struct
* @return Current output value of the filter
*/
static inline float get_first_order_complementary(const struct FirstOrderComplementary *filter)
{
float x_lp_output = get_first_order_low_pass(&filter->x_lp_filter);
float y_lp_output = get_first_order_low_pass(&filter->y_lp_filter);
return filter->x_lp_filter.last_in - x_lp_output + y_lp_output;
}
/**
* @brief Second order complementary filter structure.
*
* This structure contains two second order low-pass filters,
* one for the high-pass path and one for the low-pass path.
*/
struct SecondOrderComplementary {
struct SecondOrderLowPass x_lp_filter; // Low pass filter instance for high pass path
struct SecondOrderLowPass y_lp_filter; // Low pass filter instance for low pass path
};
/** Initialize the second order complementary filter.
*
* @param filter Complementary filter struct
* @param cut_off Time constant of the low-pass filter
* @param Q Q factor of the low-pass filter
* @param sample_time Sampling period
* @param value Initial value for filter history
*/
static inline void init_second_order_complementary(
struct SecondOrderComplementary *filter,
float cut_off, float Q, float sample_time,
float value)
{
init_second_order_low_pass(&filter->x_lp_filter, 1.0f / cut_off, Q, sample_time, value);
init_second_order_low_pass(&filter->y_lp_filter, 1.0f / cut_off, Q, sample_time, value);
}
/** Update the second order complementary filter with new input values.
*
* @param filter Complementary filter struct
* @param value_x New input value from the high-pass path
* @param value_y New input value from the low-pass path
* @return New filtered output value
*/
static inline float update_second_order_complementary(
struct SecondOrderComplementary *filter,
float value_x, float value_y)
{
float x_lp_output = update_second_order_low_pass(&filter->x_lp_filter, value_x);
float y_lp_output = update_second_order_low_pass(&filter->y_lp_filter, value_y);
return filter->x_lp_filter.i[0] - x_lp_output + y_lp_output;
}
/**
* @brief Reset the second order complementary filter to a specific value.
*
* @param filter Complementary filter struct
* @param value Value to reset the filter to
* @return The reset value
*/
static inline float reset_second_order_complementary(
struct SecondOrderComplementary *filter,
float value)
{
reset_second_order_low_pass(&filter->x_lp_filter, value);
reset_second_order_low_pass(&filter->y_lp_filter, value);
return value;
}
/** Get current value of the second order complementary filter.
*
* @param filter Complementary filter struct
* @return Current output value of the filter
*/
static inline float get_second_order_complementary(const struct SecondOrderComplementary *filter)
{
float x_lp_output = get_second_order_low_pass(&filter->x_lp_filter);
float y_lp_output = get_second_order_low_pass(&filter->y_lp_filter);
return filter->x_lp_filter.i[0] - x_lp_output + y_lp_output;
}
typedef struct SecondOrderComplementary Butterworth2Complementary;
/** Initialize the Butterworth 2nd order low-pass complementary filter.
*
* @param filter Complementary filter struct
* @param cut_off Time constant of the low-pass filter
* @param sample_time Sampling period
* @param value Initial value for filter history
*/
static inline void init_butterworth_2_complementary(
Butterworth2Complementary *filter,
float cut_off, float sample_time,
float value)
{
init_second_order_complementary(
(struct SecondOrderComplementary *)filter,
cut_off, 0.7071, sample_time,
value);
}
/** Update the Butterworth 2nd order low-pass complementary filter with new input values.
*
* @param filter Complementary filter struct
* @param value_x New input value from the high-pass path
* @param value_y New input value from the low-pass path
* @return New filtered output value
*/
static inline float update_butterworth_2_complementary(Butterworth2Complementary *filter, float value_x, float value_y)
{
return update_second_order_complementary((struct SecondOrderComplementary *)filter, value_x, value_y);
}
/**
* @brief Reset the Butterworth 2nd order complementary filter to a specific value.
*
* @param filter Complementary filter struct
* @param value Value to reset the filter to
*/
static inline void reset_butterworth_2_complementary(Butterworth2Complementary *filter, float value)
{
reset_second_order_complementary((struct SecondOrderComplementary *)filter, value);
}
/** Get current value of the Butterworth 2nd order low-pass complementary filter.
*
* @param filter Complementary filter struct
* @return Current output value of the filter
*/
static inline float get_butterworth_2_complementary(const Butterworth2Complementary *filter)
{
return get_second_order_complementary((const struct SecondOrderComplementary *)filter);
}
/**
* @brief 4th order Butterworth complementary filter structure.
*
* This structure contains two 4th order Butterworth low-pass filters,
* one for the high-pass path and one for the low-pass path.
*/
typedef struct {
Butterworth4LowPass x_lp_filter; // Low pass filter instance for high pass path
Butterworth4LowPass y_lp_filter; // Low pass filter instance for low pass path
} Butterworth4Complementary;
/** Initialize the Butterworth 4th order low-pass complementary filter.
*
* @param filter Complementary filter struct
* @param cut_off Time constant of the low-pass filter
* @param sample_time Sampling period
* @param value Initial value for filter history
*/
static inline void init_butterworth_4_complementary(Butterworth4Complementary *filter, float cut_off, float sample_time,
float value)
{
init_butterworth_4_low_pass(&filter->x_lp_filter, 1.0f / cut_off, sample_time, value);
init_butterworth_4_low_pass(&filter->y_lp_filter, 1.0f / cut_off, sample_time, value);
}
/** Update the Butterworth 4th order low-pass complementary filter with new input values.
*
* @param filter Complementary filter struct
* @param value_x New input value from the high-pass path
* @param value_y New input value from the low-pass path
* @return New filtered output value
*/
static inline float update_butterworth_4_complementary(Butterworth4Complementary *filter, float value_x, float value_y)
{
float x_lp_output = update_butterworth_4_low_pass(&filter->x_lp_filter, value_x);
float y_lp_output = update_butterworth_4_low_pass(&filter->y_lp_filter, value_y);
return filter->x_lp_filter.lp1.i[0] - x_lp_output + y_lp_output;
}
/**
* @brief Reset the Butterworth 4th order complementary filter to a specific value.
*
* @param filter Complementary filter struct
* @param value Value to reset the filter to
*/
static inline void reset_butterworth_4_complementary(Butterworth4Complementary *filter, float value)
{
reset_butterworth_4_low_pass(&filter->x_lp_filter, value);
reset_butterworth_4_low_pass(&filter->y_lp_filter, value);
}
/** Get current value of the Butterworth 4th order low-pass complementary filter.
*
* @param filter Complementary filter struct
* @return Current output value of the filter
*/
static inline float get_butterworth_4_complementary(const Butterworth4Complementary *filter)
{
float x_lp_output = get_butterworth_4_low_pass(&filter->x_lp_filter);
float y_lp_output = get_butterworth_4_low_pass(&filter->y_lp_filter);
return filter->x_lp_filter.lp1.i[0] - x_lp_output + y_lp_output;
}
#endif /* COMPLEMENTARY_FILTER_H */
sw/airborne/filters/complementary_filter_types.h
+582
View File
File diff suppressed because it is too large Load Diff
sw/airborne/filters/low_pass_filter.h
+84 -23
View File
@@ -22,6 +22,9 @@
/** @file filters/low_pass_filter.h
* @brief Simple first order low pass filter with bilinear transform
*
* @FIXME: INT and FLOAT implementations are inconsistent, tau and cut_off freq are mixed.
* Suggestion: Change everything to freq.
*
*/
#ifndef LOW_PASS_FILTER_H
@@ -54,7 +57,7 @@ struct FirstOrderLowPass {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time,
static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, const float sample_time,
float value)
{
filter->last_in = value;
@@ -68,7 +71,7 @@ static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, f
* @param value new input value of the filter
* @return new filtered value
*/
static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
{
float out = (value + filter->last_in + (filter->time_const - 1.0f) * filter->last_out) / (1.0f + filter->time_const);
filter->last_in = value;
@@ -76,12 +79,26 @@ static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter
return out;
}
/**
* @brief Reset the first order low-pass filter to a specific value.
*
* @param filter first order low pass filter structure
* @param value Value to reset the filter to
* @return The reset value
*/
static inline float reset_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
{
filter->last_in = value;
filter->last_out = value;
return value;
}
/** Get current value of the first order low pass filter.
*
* @param filter first order low pass filter structure
* @return current value of the filter
*/
static inline float get_first_order_low_pass(struct FirstOrderLowPass *filter)
static inline float get_first_order_low_pass(const struct FirstOrderLowPass *filter)
{
return filter->last_out;
}
@@ -92,7 +109,8 @@ static inline float get_first_order_low_pass(struct FirstOrderLowPass *filter)
* @param tau time constant of the first order low pass filter
* @param sample_time sampling period of the signal
*/
static inline void update_first_order_low_pass_tau(struct FirstOrderLowPass *filter, float tau, float sample_time)
static inline void update_first_order_low_pass_tau(struct FirstOrderLowPass *filter, const float tau,
const float sample_time)
{
filter->time_const = 2.0f * tau / sample_time;
}
@@ -149,7 +167,8 @@ struct SecondOrderLowPass {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time,
static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, const float tau, const float Q,
const float sample_time,
float value)
{
float K = tanf(sample_time / (2.0f * tau));
@@ -161,13 +180,26 @@ static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter,
filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
}
/**
* @brief Reset the second order low-pass filter to a specific value.
*
* @param filter second order low pass filter structure
* @param value Value to reset the filter to
* @return The reset value
*/
static inline float reset_second_order_low_pass(struct SecondOrderLowPass *filter, const float value)
{
filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
return value;
}
/** Update second order low pass filter state with a new value.
*
* @param filter second order low pass filter structure
* @param value new input value of the filter
* @return new filtered value
*/
static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, const float value)
{
float out = filter->b[0] * value
+ filter->b[1] * filter->i[0]
@@ -186,7 +218,7 @@ static inline float update_second_order_low_pass(struct SecondOrderLowPass *filt
* @param filter second order low pass filter structure
* @return current value of the filter
*/
static inline float get_second_order_low_pass(struct SecondOrderLowPass *filter)
static inline float get_second_order_low_pass(const struct SecondOrderLowPass *filter)
{
return filter->o[0];
}
@@ -207,7 +239,8 @@ struct SecondOrderLowPass_int {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q,
static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, const float cut_off,
const float Q,
float sample_time, int32_t value)
{
struct SecondOrderLowPass filter_temp;
@@ -236,7 +269,7 @@ static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int
* @param value new input value of the filter
* @return new filtered value
*/
static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, int32_t value)
static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, const int32_t value)
{
int32_t out = filter->b[0] * value
+ filter->b[1] * filter->i[0]
@@ -256,7 +289,7 @@ static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass
* @param filter second order low pass filter structure
* @return current value of the filter
*/
static inline int32_t get_second_order_low_pass_int(struct SecondOrderLowPass_int *filter)
static inline int32_t get_second_order_low_pass_int(const struct SecondOrderLowPass_int *filter)
{
return filter->o[0];
}
@@ -277,7 +310,8 @@ typedef struct SecondOrderLowPass Butterworth2LowPass;
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time,
const float value)
{
init_second_order_low_pass((struct SecondOrderLowPass *)filter, tau, 0.7071, sample_time, value);
}
@@ -288,19 +322,31 @@ static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, floa
* @param value new input value of the filter
* @return new filtered value
*/
static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
{
return update_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
}
/**
* @brief Reset a Butterworth low-pass filter to a specific value.
*
* @param[out] filter Butterworth2LowPass filter instance to reset.
* @param[in] value Value to reset the filter to.
* @return The reset value.
*/
static inline float reset_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
{
return reset_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
}
/** Get current value of the second order Butterworth low pass filter.
*
* @param filter second order Butterworth low pass filter structure
* @return current value of the filter
*/
static inline float get_butterworth_2_low_pass(Butterworth2LowPass *filter)
static inline float get_butterworth_2_low_pass(const Butterworth2LowPass *filter)
{
return filter->o[0];
return get_second_order_low_pass((const struct SecondOrderLowPass *)filter);
}
/** Second order Butterworth low pass filter(fixed point version).
@@ -319,7 +365,8 @@ typedef struct SecondOrderLowPass_int Butterworth2LowPass_int;
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time,
static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, const float cut_off,
const float sample_time,
int32_t value)
{
init_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, cut_off, 0.7071, sample_time, value);
@@ -331,7 +378,7 @@ static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filt
* @param value new input value of the filter
* @return new filtered value
*/
static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value)
static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, const int32_t value)
{
return update_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, value);
}
@@ -341,7 +388,7 @@ static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int
* @param filter second order Butterworth low pass filter structure
* @return current value of the filter
*/
static inline int32_t get_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter)
static inline int32_t get_butterworth_2_low_pass_int(const Butterworth2LowPass_int *filter)
{
return filter->o[0];
}
@@ -368,7 +415,8 @@ typedef struct {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value)
static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, const float tau, const float sample_time,
const float value)
{
init_second_order_low_pass(&filter->lp1, tau, 1.30651, sample_time, value);
init_second_order_low_pass(&filter->lp2, tau, 0.51184, sample_time, value);
@@ -382,7 +430,7 @@ static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, floa
* @param value new input value of the filter
* @return new filtered value
*/
static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value)
static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, const float value)
{
float tmp = update_second_order_low_pass(&filter->lp1, value);
return update_second_order_low_pass(&filter->lp2, tmp);
@@ -393,11 +441,23 @@ static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, f
* @param filter fourth order Butterworth low pass filter structure
* @return current value of the filter
*/
static inline float get_butterworth_4_low_pass(Butterworth4LowPass *filter)
static inline float get_butterworth_4_low_pass(const Butterworth4LowPass *filter)
{
return filter->lp2.o[0];
}
/**
* @brief Reset a Butterworth low-pass filter to a specific value.
*
* @param[out] filter Butterworth4LowPass filter instance to reset.
* @param[in] value Value to reset the filter to.
*/
static inline void reset_butterworth_4_low_pass(Butterworth4LowPass *filter, const float value)
{
filter->lp1.i[0] = filter->lp1.i[1] = filter->lp1.o[0] = filter->lp1.o[1] = filter->lp2.i[0] = filter->lp2.i[1] =
filter->lp2.o[0] = filter->lp2.o[1] = value;
}
/** Fourth order Butterworth low pass filter(fixed point version).
*
* using two cascaded second order filters
@@ -420,7 +480,8 @@ typedef struct {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, float cut_off, float sample_time,
static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, const float cut_off,
const float sample_time,
int32_t value)
{
init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value);
@@ -435,7 +496,7 @@ static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filt
* @param value new input value of the filter
* @return new filtered value
*/
static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, int32_t value)
static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, const int32_t value)
{
int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value);
return update_second_order_low_pass_int(&filter->lp2, tmp);
@@ -446,7 +507,7 @@ static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int
* @param filter fourth order Butterworth low pass filter structure
* @return current value of the filter
*/
static inline int32_t get_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter)
static inline int32_t get_butterworth_4_low_pass_int(const Butterworth4LowPass_int *filter)
{
return filter->lp2.o[0];
}
sw/airborne/filters/low_pass_filter_types.h
+550
View File
File diff suppressed because it is too large Load Diff
sw/airborne/filters/low_pass_zoh_filter.h
+116
View File
@@ -0,0 +1,116 @@
/*
* Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file low_pass_zoh_filter.h
* @brief A first order low pass filter using Zero Order Hold (ZOH) discretization.
*/
#ifndef LOW_PASS_ZOH_FILTER_H
#define LOW_PASS_ZOH_FILTER_H
#include "std.h"
#include "math/pprz_algebra_int.h"
/**
* @brief Zero Order Hold (ZOH) discrete first order low pass filter structure.
*
* This structure represents a first order low pass filter implemented using
* the Zero Order Hold (ZOH) method for discretization. It maintains the necessary
* state for filtering operations.
*/
struct FirstOrderZOHLowPass {
float discrete_time_constant;
float last_in;
float last_out;
};
/**
* @brief Init first order ZOH low pass filter.
*
* The ZOH discretization method is used to convert a continuous-time first order
* low pass filter into its discrete-time equivalent which matches the continuous-time
* behavior at the sampling instants exactly when holding the input constant between samples.
*
* @param[out] filter first order ZOH low pass filter structure
* @param[in] tau time constant of the first order low pass filter [s]
* @param[in] sample_time sampling period of the signal [s]
* @param[in] value initial value of the filter
*/
static inline void init_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float tau,
const float sample_time,
float value)
{
filter->discrete_time_constant = exp(-sample_time / tau);
filter->last_in = value;
filter->last_out = value;
}
/**
* @brief Update first order ZOH low pass filter state with a new value.
*
* @param[in,out] filter first order ZOH low pass filter structure
* @param[in] value new input value of the filter
* @return new filtered value
*/
static inline float update_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float value)
{
float out = (1.0f - filter->discrete_time_constant) * value + filter->discrete_time_constant * filter->last_out;
filter->last_in = value;
filter->last_out = out;
return out;
}
/**
* @brief Reset the first order ZOH low-pass filter to a specific value.
*
* @param[in,out] filter first order ZOH low pass filter structure
* @param[in] value Value to reset the filter to
* @return The reset value
*/
static inline float reset_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float value)
{
filter->last_in = value;
filter->last_out = value;
return value;
}
/**
* @brief Get current value of the first order ZOH low pass filter.
*
* @param[in] filter first order ZOH low pass filter structure
* @return current value of the filter
*/
static inline float get_first_order_zoh_low_pass(const struct FirstOrderZOHLowPass *filter)
{
return filter->last_out;
}
/** @brief Update time constant (tau parameter) for first order ZOH low pass filter
* @param[in,out] filter first order ZOH low pass filter structure
* @param[in] tau time constant of the first order low pass filter [s]
* @param[in] sample_time sampling period of the signal [s]
*/
static inline void update_first_order_zoh_low_pass_tau(struct FirstOrderZOHLowPass *filter, const float tau,
const float sample_time)
{
filter->discrete_time_constant = exp(-sample_time / tau);
}
#endif // LOW_PASS_ZOH_FILTER_H
sw/airborne/filters/low_pass_zoh_filter_types.h
+97
View File
@@ -0,0 +1,97 @@
/*
* Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file low_pass_zoh_filter.h
* @brief A first order low pass filter using Zero Order Hold (ZOH) discretization.
*/
/** @file filters/low_pass_zoh_filter.h
* @brief Definitions and inline functions for 1st order low-pass ZOH filter vector types
*
* Provides structures and functions to initialize, update, reset, and get outputs from
* 1st order low-pass ZOH filter vectors.
*
* @author Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*/
#ifndef LOW_PASS_ZOH_FILTER_TYPES_H
#define LOW_PASS_ZOH_FILTER_TYPES_H
#include "std.h"
#include "filters/low_pass_zoh_filter.h"
#include "math/pprz_algebra_float.h"
/**
* @brief Initialize a set of first order ZOH low-pass filters to zero for 3D vector data.
* @param[out] filter_array Array of FirstOrderZOHLowPass filters to initialize.
* @param[in] omega Cut-off frequencies for the filters (rad/s).
* @param[in] sample_time Sampling time interval (seconds).
*/
static inline void init_first_order_zoh_low_pass_array(const uint8_t n,
struct FirstOrderZOHLowPass filter_array[restrict n], const float omega[restrict n], const float sample_time)
{
for (uint8_t i = 0; i < n; i++) {
float tau = 1.0f / omega[i];
init_first_order_zoh_low_pass(&filter_array[i], tau, sample_time, 0.0f);
}
}
/**
* @brief Update an array of first order ZOH low-pass filters.
* @param[in] n Number of filters in the array.
* @param[in,out] filter_array Array of FirstOrderZOHLowPass filters to update.
* @param[in] input_array Array of input values for the filters.
*/
static inline void update_first_order_zoh_low_pass_array(const uint8_t n,
struct FirstOrderZOHLowPass filter_array[restrict n], const float input_array[restrict n])
{
for (uint8_t i = 0; i < n; i++) {
update_first_order_zoh_low_pass(&filter_array[i], input_array[i]);
}
}
/**
* @brief Reset an array of first order ZOH low-pass filters to specific values.
* @param[in] n Number of filters in the array.
* @param[out] filter_array Array of FirstOrderZOHLowPass filters to reset.
* @param[in] value_array Array of values to reset the filters to.
*/
static inline void reset_first_order_zoh_low_pass_array(const uint8_t n,
struct FirstOrderZOHLowPass filter_array[restrict n], const float value_array[restrict n])
{
for (uint8_t i = 0; i < n; i++) {
filter_array[i].last_in = value_array[i];
filter_array[i].last_out = value_array[i];
}
}
/**
* @brief Retrieve the filtered outputs from an array of first order ZOH low-pass filters.
* @param[in] n Number of filters in the array.
* @param[in] filter_array Array of FirstOrderZOHLowPass filters.
* @param[out] output_array Array to store the filtered output values.
*/
static inline void get_first_order_zoh_low_pass_array(const uint8_t n,
const struct FirstOrderZOHLowPass filter_array[restrict n], float output_array[restrict n])
{
for (uint8_t i = 0; i < n; i++) {
output_array[i] = filter_array[i].last_out;
}
}
#endif // LOW_PASS_ZOH_FILTER_TYPES_H
sw/airborne/filters/transport_delay.h
+104
View File
@@ -0,0 +1,104 @@
/*
* Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file transport_delay.h
* @brief Transport delay filter implementation.
*/
#ifndef TRANSPORT_DELAY_H
#define TRANSPORT_DELAY_H
#include "paparazzi.h"
#define TRANSPORT_DELAY_BUFFER_SIZE 20
struct TransportDelay {
uint8_t delay_samples; // Number of samples to delay
uint8_t write_index; // Current write index
float buffer[TRANSPORT_DELAY_BUFFER_SIZE];
};
/**
* Initialize a transport delay buffer.
*
* @param[out] td Pointer to the transport_delay_t structure to initialize.
* @param[in] delay_samples Number of samples to delay. If this value exceeds TRANSPORT_DELAY_BUFFER_SIZE,
* it will be clamped to TRANSPORT_DELAY_BUFFER_SIZE.
* @param[in] initial_value Initial value to fill the buffer with.
*
* @note: If delay_samples > TRANSPORT_DELAY_BUFFER_SIZE, it will be clamped to TRANSPORT_DELAY_BUFFER_SIZE
* and the requested delay will not be fully honored.
*/
static inline void init_transport_delay(struct TransportDelay *td, uint8_t delay_samples, const float initial_value)
{
if (delay_samples > TRANSPORT_DELAY_BUFFER_SIZE) {
delay_samples = TRANSPORT_DELAY_BUFFER_SIZE;
}
td->delay_samples = delay_samples;
td->write_index = 0;
for (uint8_t i = 0; i < TRANSPORT_DELAY_BUFFER_SIZE; i++) {
td->buffer[i] = initial_value;
}
}
/**
* Propagate a new input value through the transport delay buffer.
*
* @param[in,out] td Pointer to the transport_delay_t structure.
* @param[in] input New input value to add to the buffer.
* @return Delayed output value from the buffer.
*/
static inline float update_transport_delay(struct TransportDelay *td, const float input)
{
td->buffer[td->write_index] = input;
uint8_t read_index = (td->write_index + TRANSPORT_DELAY_BUFFER_SIZE - td->delay_samples) % TRANSPORT_DELAY_BUFFER_SIZE;
float output = td->buffer[read_index];
td->write_index = (td->write_index + 1) % TRANSPORT_DELAY_BUFFER_SIZE;
return output;
}
/**
* Reset the transport delay buffer to a specific initial value.
*
* @param[in,out] td Pointer to the transport_delay_t structure.
* @param[in] initial_value Value to reset the buffer elements to.
*/
static inline void reset_transport_delay(struct TransportDelay *td, const float initial_value)
{
td->write_index = 0;
for (uint8_t i = 0; i < TRANSPORT_DELAY_BUFFER_SIZE; i++) {
td->buffer[i] = initial_value;
}
}
/**
* Get the current output value from the transport delay buffer without updating it.
*
* @param td Pointer to the transport_delay_t structure.
* @return Current delayed output value from the buffer.
*/
static inline float get_transport_delay(const struct TransportDelay *td)
{
uint8_t read_index = (td->write_index + TRANSPORT_DELAY_BUFFER_SIZE - td->delay_samples - 1) %
TRANSPORT_DELAY_BUFFER_SIZE;
return td->buffer[read_index];
}
#endif // TRANSPORT_DELAY_H
sw/airborne/filters/transport_delay_types.h
+88
View File
@@ -0,0 +1,88 @@
/*
* Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file transport_delay_types.h
* @brief Transport delay filter type definitions and array operations.
*/
#ifndef TRANSPORT_DELAY_TYPES_H
#define TRANSPORT_DELAY_TYPES_H
#include "std.h"
#include "paparazzi.h"
#include "filters/transport_delay.h"
/**
* @brief Initialize an array of TransportDelay structures.
* @param[in] n Number of TransportDelay structures in the array.
* @param[in,out] td_array Array of TransportDelay structures to initialize.
* @param[in] delay_samples Array of delay samples for each TransportDelay structure.
* @param[in] initial_value Array of initial values to fill the buffers.
*/
static inline void init_transport_delay_array(uint8_t n, struct TransportDelay td_array[restrict n],
const uint8_t delay_samples[restrict n], float initial_value[restrict n])
{
for (uint8_t i = 0; i < n; i++) {
init_transport_delay(&td_array[i], delay_samples[i], initial_value[i]);
}
}
/**
* @brief Update an array of TransportDelay structures with input values.
* @param[in] n Number of TransportDelay structures in the array.
* @param[in,out] td_array Array of TransportDelay structures to update.
* @param[in] input_array Array of input values for each TransportDelay structure.
*/
static inline void update_transport_delay_array(uint8_t n, struct TransportDelay td_array[restrict n],
const float input_array[restrict n])
{
for (uint8_t i = 0; i < n; i++) {
update_transport_delay(&td_array[i], input_array[i]);
}
}
/**
* @brief Reset an array of TransportDelay structures to specific initial values.
* @param[in] n Number of TransportDelay structures in the array.
* @param[in,out] td_array Array of TransportDelay structures to reset.
* @param[in] initial_value Array of initial values to reset the buffers to.
*/
static inline void reset_transport_delay_array(const uint8_t n, struct TransportDelay td_array[restrict n],
const float initial_value[restrict n])
{
for (uint8_t i = 0; i < n; i++) {
reset_transport_delay(&td_array[i], initial_value[i]);
}
}
/**
* @brief Get output values from an array of TransportDelay structures.
* @param[in] n Number of TransportDelay structures in the array.
* @param[in] td_array Array of TransportDelay structures to get outputs from.
* @param[out] output_array Array to store the output values for each TransportDelay structure.
*/
static inline void get_transport_delay_array(const uint8_t n, const struct TransportDelay td_array[restrict n],
float output_array[restrict n])
{
for (uint8_t i = 0; i < n; i++) {
output_array[i] = get_transport_delay(&td_array[i]);
}
}
#endif // TRANSPORT_DELAY_TYPES_H
sw/airborne/firmwares/rotorcraft/stabilization/obm_cyclone.c
+378
View File
@@ -0,0 +1,378 @@
#include "firmwares/rotorcraft/stabilization/stabilization_andi.h"
#include<math.h>
#if ANDI_NUM_ACT != 4
#error Cyclone expects 4 actuators
#endif
#if ANDI_OUTPUTS != 4
#error The cyclone model provides 4 outputs
#endif
union CycloneCoefficients {
struct {
// X-axis force coefficients (f_ff_x)
float fx_motor_squared; // Motor thrust squared effect
float fx_speed_forward; // Forward speed effect
// Y-axis force coefficients (f_ff_y)
float fy_speed_lateral; // Lateral speed effect
// Z-axis force coefficients (f_ff_z)
float fz_motor_squared; // Motor thrust squared effect
float fz_speed_forward; // Forward speed effect
float fz_speed_vertical; // Vertical speed effect
float fz_elevon_speed; // Elevon-speed coupling
float fz_elevon_motor; // Elevon-motor coupling
// X-axis moment coefficients (m_ff_x)
float mx_elevon_motor_diff; // (ele_l * motor_l^2 - ele_r * motor_r^2)
float mx_elevon_speed_diff; // (ele_l - ele_r) * speed * v_ff(1)
float mx_angular_drag; // w_ff(1)^2
float mx_angular_coupling; // w_ff(2) * w_ff(3)
// Y-axis moment coefficients (m_ff_y)
float my_speed_forward; // speed * v_ff(1)
float my_speed_vertical; // speed * v_ff(3)
float my_motor_sum; // motor_l^2 + motor_r^2
float my_elevon_motor_sum; // ele_l * motor_l^2 + ele_r * motor_r^2
float my_elevon_speed_sum; // (ele_l + ele_r) * speed * v_ff(1)
float my_angular_sum; // w_ff(1) + w_ff(3)
// Z-axis moment coefficients (m_ff_z)
float mz_speed_lateral; // speed * v_ff(2)
float mz_motor_diff; // motor_l^2 - motor_r^2
float mz_speed_roll; // speed * w_ff(1)
float mz_angular_coupling; // w_ff(1) * w_ff(2)
// Fixme: Move to x-axis moment coefficients
};
float data[22];
};
// Model coefficinets
union CycloneCoefficients obm_coefficients = {
.fx_motor_squared = 0.00000735f,
.fx_speed_forward = -0.03f,
.fy_speed_lateral = -0.008f,
.fz_motor_squared = 0.0f,
.fz_speed_forward = 0.0f,
.fz_speed_vertical = -0.144f,
.fz_elevon_speed = 0.0f,
.fz_elevon_motor = 0.0f,
.mx_elevon_motor_diff = 1.9e-05f,
.mx_elevon_speed_diff = 0.344f,
.mx_angular_drag = -0.4940217f,
.mx_angular_coupling = -2.18f,
.my_speed_forward = 0.0f,
.my_speed_vertical = -0.0888f,
.my_motor_sum = 0.0f,
.my_elevon_motor_sum = -0.0000424f,
.my_elevon_speed_sum = 0.2525f,
.my_angular_sum = 1.262f,
.mz_speed_lateral = -0.00371f,
.mz_motor_diff = 0.000039f,
.mz_speed_roll = -0.0129f,
.mz_angular_coupling = -0.4827f,
};
union CeMatrix {
struct {
float ce_11;
float ce_12;
float ce_13;
float ce_14;
float ce_21;
float ce_22;
float ce_23;
float ce_24;
float ce_31;
float ce_32;
float ce_33;
float ce_34;
float ce_41;
float ce_42;
float ce_43;
float ce_44;
};
float data[16];
};
// Model coefficinets
union CeMatrix ce_mat_tmp = {
.ce_11 = 0.0f, .ce_12 = 0.0f, .ce_13 = 3.9e-5f, .ce_14 = -3.9e-5f, // Roll
.ce_21 = -29.917439f, .ce_22 = -29.917439f, .ce_23 = 0.0f, .ce_24 = 0.0f, // Pitch
.ce_31 = -19.968481f, .ce_32 = 19.968481f, .ce_33 = 0.0f, .ce_34 = 0.0f, // Yaw
.ce_41 = 0.0f, .ce_42 = 0.0f, .ce_43 = 7e-6f, .ce_44 = 7e-6f, // Thrust
};
/* Function Definitions */
static void cyclone_obm_forces(const float body_vel[3], const float u[4],
const float coeff[22], float F_obm_forces[3])
{
float t2;
float t7;
t2 = u[2] + u[3];
t7 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
F_obm_forces[0] = coeff[3] * t2
+ coeff[7] * (u[0] * u[2] + u[1] * u[3])
+ coeff[5] * t7 * body_vel[0]
- coeff[4] * t7 * body_vel[2]
- coeff[6] * t7 * body_vel[2] * (u[0] + u[1]);
F_obm_forces[1] = coeff[2] * t7 * body_vel[1];
F_obm_forces[2] = -coeff[0] * t2 + coeff[1] * t7 * body_vel[2];
}
static void cyclone_obm_moments(const float rates[3],
const float body_vel[3], const float u[4],
const float coeff[22], float F_obm_moments[3])
{
float t2;
float t3;
float t8;
t2 = u[0] * u[2];
t3 = u[1] * u[3];
t8 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
F_obm_moments[0] = coeff[19] * (u[2] - u[3])
+ coeff[18] * t8 * body_vel[1]
- coeff[20] * t8 * rates[2]
- rates[1] * coeff[21] * rates[2];
F_obm_moments[1] = coeff[15] * (t2 + t3)
+ coeff[14] * (u[2] + u[3])
+ coeff[13] * t8 * body_vel[0]
- coeff[12] * t8 * body_vel[2]
- rates[0] * coeff[17] * rates[2]
- coeff[16] * t8 * body_vel[2] * (u[0] + u[1]);
F_obm_moments[2] = -coeff[8] * (t2 - t3)
+ coeff[10] * fabsf(rates[2]) * rates[2]
- rates[0] * coeff[11] * rates[1]
+ coeff[9] * t8 * body_vel[2] * (u[0] - u[1]);
}
static void cyclone_f_stb_u(const float body_vel[3],
const float u[4], const float coeff[22], float F_stb_u[16])
{
float fv[16];
float t6;
float t7;
int i;
int i1;
int i2;
t6 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
t7 = coeff[9] * t6 * body_vel[2];
t6 = coeff[16] * t6 * body_vel[2];
fv[0] = 0.0F;
fv[1] = -t6 + u[2] * coeff[15];
fv[2] = t7 - u[2] * coeff[8];
fv[3] = 0.0F;
fv[4] = 0.0F;
fv[5] = -t6 + u[3] * coeff[15];
fv[6] = -t7 + u[3] * coeff[8];
fv[7] = 0.0F;
fv[8] = coeff[19];
fv[9] = coeff[14] + u[0] * coeff[15];
fv[10] = -coeff[8] * u[0];
fv[11] = coeff[0];
fv[12] = -coeff[19];
fv[13] = coeff[14] + u[1] * coeff[15];
fv[14] = u[1] * coeff[8];
fv[15] = coeff[0];
i = 0;
i1 = 0;
for (i2 = 0; i2 < 16; i2++) {
F_stb_u[i1 + (i << 2)] = fv[i2];
i++;
if (i > 3) {
i = 0;
i1++;
}
}
}
/* Function Definitions */
void cyclone_f_stb_x(const float rates[3], const float ang_accel[3],
const float vel_body[3], const float accel_body[3], const float u[4],
const float coeff[22], float F_stb_x[4])
{
float F_stb_x_tmp;
float b_F_stb_x_tmp;
float c_F_stb_x_tmp;
float d_F_stb_x_tmp;
float et1_tmp;
float t10;
float t12;
float t15;
float t8;
float t9;
t8 = vel_body[0] * vel_body[0];
t9 = vel_body[1] * vel_body[1];
t10 = vel_body[2] * vel_body[2];
t12 = u[0] - u[1];
t15 = sqrtf((t8 + t9) + t10);
et1_tmp = fmaxf(1.0E-8F, t15);
F_stb_x[0] = -(
-accel_body[1] * coeff[18] * t9
- accel_body[0] * coeff[18] * vel_body[0] * vel_body[1]
- accel_body[2] * coeff[18] * vel_body[1] * vel_body[2]
+ accel_body[0] * coeff[20] * vel_body[0] * rates[2]
+ accel_body[1] * coeff[20] * vel_body[1] * rates[2]
+ accel_body[2] * coeff[20] * vel_body[2] * rates[2]
- accel_body[1] * coeff[18] * t15 * et1_tmp
+ coeff[20] * t15 * ang_accel[2] * et1_tmp
+ rates[1] * coeff[21] * ang_accel[2] * et1_tmp
+ rates[2] * coeff[21] * ang_accel[1] * et1_tmp
) / et1_tmp;
t9 = accel_body[2] * coeff[12];
F_stb_x_tmp = accel_body[2] * coeff[16];
b_F_stb_x_tmp = accel_body[0] * coeff[16];
c_F_stb_x_tmp = accel_body[1] * coeff[16];
d_F_stb_x_tmp = F_stb_x_tmp * u[0];
F_stb_x_tmp *= u[1];
F_stb_x[1] = -(
-accel_body[0] * coeff[13] * t8 + t9 * t10
+ accel_body[0] * coeff[12] * vel_body[0] * vel_body[2]
- accel_body[1] * coeff[13] * vel_body[0] * vel_body[1]
+ accel_body[1] * coeff[12] * vel_body[1] * vel_body[2]
- accel_body[1] * coeff[12] * vel_body[1] * vel_body[2]
- accel_body[2] * coeff[13] * vel_body[0] * vel_body[2]
- accel_body[0] * coeff[13] * t15 * et1_tmp
+ t9 * t15 * et1_tmp
+ rates[0] * coeff[17] * ang_accel[2] * et1_tmp
+ rates[2] * coeff[17] * ang_accel[0] * et1_tmp
+ d_F_stb_x_tmp * t10
+ F_stb_x_tmp * t10
+ b_F_stb_x_tmp * u[0] * vel_body[0] * vel_body[2]
+ c_F_stb_x_tmp * u[0] * vel_body[1] * vel_body[2]
+ b_F_stb_x_tmp * u[1] * vel_body[0] * vel_body[2]
+ c_F_stb_x_tmp * u[1] * vel_body[1] * vel_body[2]
+ d_F_stb_x_tmp * t15 * et1_tmp
+ F_stb_x_tmp * t15 * et1_tmp
) / et1_tmp;
F_stb_x[2] = accel_body[2] * (coeff[9] * t12 * t15 + coeff[9] * t10 * t12 / et1_tmp)
- rates[0] * coeff[11] * ang_accel[1]
- rates[1] * coeff[11] * ang_accel[0]
+ 2.0F * coeff[10] * fabsf(rates[2]) * ang_accel[2]
+ accel_body[0] * coeff[9] * t12 * vel_body[0] * vel_body[2] / et1_tmp
+ accel_body[1] * coeff[9] * t12 * vel_body[1] * vel_body[2] / et1_tmp;
F_stb_x[3] = 0.0F;
}
/* End of code generation (cyclone_f_stb_x.c) */
struct FloatVect3 evaluate_obm_forces(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
const float actuator_state[ANDI_NUM_ACT])
{
float vel_body_array[3];
(void)rates;
vel_body_array[0] = vel_body->x;
vel_body_array[1] = vel_body->y;
vel_body_array[2] = vel_body->z;
float forces_array[3];
cyclone_obm_forces(vel_body_array, actuator_state, obm_coefficients.data, forces_array);
struct FloatVect3 forces;
forces.x = forces_array[0];
forces.y = forces_array[1];
forces.z = forces_array[2];
return forces;
}
struct FloatVect3 evaluate_obm_moments(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
const float actuator_state[ANDI_NUM_ACT])
{
float vel_body_array[3];
float rates_array[3];
vel_body_array[0] = vel_body->x;
vel_body_array[1] = vel_body->y;
vel_body_array[2] = vel_body->z;
rates_array[0] = rates->p;
rates_array[1] = rates->q;
rates_array[2] = rates->r;
float moments_array[3];
cyclone_obm_moments(rates_array, vel_body_array, actuator_state, obm_coefficients.data, moments_array);
struct FloatVect3 moments;
moments.x = moments_array[0];
moments.y = moments_array[1];
moments.z = moments_array[2];
return moments;
}
void evaluate_obm_f_stb_u(float fu_mat[ANDI_NUM_ACT * ANDI_OUTPUTS], const struct FloatRates *rates,
const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT])
{
float vel_body_array[3];
(void)rates;
vel_body_array[0] = vel_body->x;
vel_body_array[1] = vel_body->y;
vel_body_array[2] = vel_body->z;
// Bound min motor speed in actuator_state to prevent really low control effectiveness which may result in instabilities.
// This should make "free fall" more stable at the cost of some model inaccuracy at very low thrust.
// FIXME: Rethink this solution.
// FIXME: Apply this bounding directly on the elevon effectiveness terms in the final matrix instead of modifying actuator_state.
// FIXME: Would it be possible to dynamically identify or saturate the control effectiveness?
float actuator_state_bounded[ANDI_NUM_ACT];
actuator_state_bounded[0] = actuator_state[0];
actuator_state_bounded[1] = actuator_state[1];
actuator_state_bounded[2] = fmaxf(actuator_state[2], 360000.0f);
actuator_state_bounded[3] = fmaxf(actuator_state[3], 360000.0f);
cyclone_f_stb_u(vel_body_array, actuator_state_bounded, obm_coefficients.data, fu_mat);
fu_mat = ce_mat_tmp.data; // FIXME: Temporary hack to use ce_mat instead of the real computed matrix
}
void evaluate_obm_f_stb_x(float nu_obm[ANDI_OUTPUTS], const struct FloatRates *rates,
const struct FloatVect3 *vel_body, const struct FloatVect3 *ang_accel, const struct FloatVect3 *accel_body,
const float actuator_state[ANDI_NUM_ACT])
{
float rates_array[3];
float ang_accel_array[3];
float vel_body_array[3];
float accel_body_array[3];
rates_array[0] = rates->p;
rates_array[1] = rates->q;
rates_array[2] = rates->r;
ang_accel_array[0] = ang_accel->x;
ang_accel_array[1] = ang_accel->y;
ang_accel_array[2] = ang_accel->z;
vel_body_array[0] = vel_body->x;
vel_body_array[1] = vel_body->y;
vel_body_array[2] = vel_body->z;
accel_body_array[0] = accel_body->x;
accel_body_array[1] = accel_body->y;
accel_body_array[2] = accel_body->z;
cyclone_f_stb_x(rates_array, ang_accel_array, vel_body_array, accel_body_array, actuator_state, obm_coefficients.data,
nu_obm);
}
float evaluate_obm_thrust_z(const float actuator_state[ANDI_NUM_ACT])
{
return obm_coefficients.fx_motor_squared * (actuator_state[2] + actuator_state[3]);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.c
+1444
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.h
+277
View File
@@ -0,0 +1,277 @@
/*
*
* Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.h
* @brief ANDI stabilization controller for tiltbody rotorcraft.
*
* Provides declarations, configuration parameters, and interfaces for the ANDI controller implemented in stabilization_andi.c.
*
* @see stabilization_andi.c
* @author Justin Dubois <j.p.g.dubois@student.tudelft.nl>
*/
#ifndef STABILIZATION_ANDI_H
#define STABILIZATION_ANDI_H
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#include "generated/airframe.h"
#include <stdio.h>
#include "filters/low_pass_filter.h"
#ifndef ANDI_NUM_ACT
#define ANDI_NUM_ACT COMMANDS_NB_REAL
#endif
#ifndef ANDI_OUTPUTS
#error "You must specify the number of controlled axis (outputs)"
#endif
/**
* @brief Structure representing attitude in quaternion form along with its first three derivatives, used for reference.
*
* This structure encapsulates the attitude represented as a quaternion,
* its first derivative (angular rates), second derivative (angular accelerations),
* and third derivative (angular jerks). It is primarily used for reference
* representation in the ANDI controller.
*/
struct AttQuat {
struct FloatQuat att;
struct FloatRates att_d;
struct FloatVect3 att_2d;
struct FloatVect3 att_3d;
};
/**
* @brief Structure representing attitude in quaternion form along with its first two derivatives, used for state.
*
* This structure encapsulates the attitude represented as a quaternion,
* its first derivative (angular rates), and second derivative (angular accelerations).
* It is primarily used for state representation in the ANDI controller.
*/
struct AttStateQuat {
struct FloatQuat att;
struct FloatRates att_d;
struct FloatVect3 att_2d;
};
/**
* @brief Structure representing linear state with velocity and acceleration vectors.
*
* This structure encapsulates the linear state of the vehicle,
* including its velocity and acceleration as 3D vectors.
*/
struct LinState {
struct FloatVect3 vel;
struct FloatVect3 acc;
};
/**
* @brief Structure representing (specific) thrust reference and its derivative.
*
* This structure encapsulates the thrust reference,
* including the thrust value and its first derivative.
*/
struct ThrustRef {
float thrust;
float thrust_d;
};
/**
* @brief Structure defining third-order pole parameters for vectorized 3D quantities.
*
* This structure holds the natural frequencies, damping ratios,
* and the pseudo actuator dynamics.
*/
struct PolesOrder3Vect3 {
struct FloatVect3 omega_n;
struct FloatVect3 zeta;
struct FloatVect3 omega_a;
};
/**
* @brief Structure defining second-order pole parameters for vectorized 3D quantities.
*
* This structure holds the natural frequencies and damping ratios.
*/
struct PolesOrder2Vect3 {
struct FloatVect3 omega_a;
struct FloatVect3 zeta;
};
/**
* @brief Structure defining second-order gain parameters for vectorized 3D quantities.
*/
struct GainsOrder2Vect3 {
struct FloatVect3 k1;
struct FloatVect3 k2;
};
/**
* @brief Structure defining third-order gain parameters for vectorized 3D quantities.
*/
struct GainsOrder3Vect3 {
struct FloatVect3 k1;
struct FloatVect3 k2;
struct FloatVect3 k3;
};
/* Declaration of Reference Model and Error Controller Poles */
extern struct PolesOrder2Vect3 andi_p_rate_ec;
extern struct PolesOrder2Vect3 andi_p_rate_rm;
extern struct PolesOrder3Vect3 andi_p_att_ec;
extern struct PolesOrder3Vect3 andi_p_att_rm;
extern float andi_p_thrust_ec;
extern float andi_p_thrust_rm;
void stabilization_andi_init(void);
void stabilization_andi_enter(void);
void stabilization_andi_run(bool use_rate_control, bool in_flight, struct StabilizationSetpoint *stab_setpoint,
struct ThrustSetpoint *thrust_setpoint, int32_t *cmd);
// void actuator_debug(bool do_servo_step, bool do_motor_step, float step_rate);
/**
* @brief Evaluate total force acting on the vehicle from the OBM
*
* Computes the full set of forces modeled by the on board model for
* the given airframe as a function of the state and input.
*
* This function is used for complementary filtering in ANDI.
*
* @param[in] rates Current angular rates (p, q, r) used by the model (rad/s).
* @param[in] vel_body Body-frame linear velocity vector (u, v, w) (m/s).
* @param[in] actuator_state Current actuator commands/deflections array (length
* ANDI_NUM_ACT). Units and normalization depend on the
* airframe and actuator type (e.g. radians, throttle fraction).
* @return Computed force vector produced by the actuators as per the OBM.
*
* @note Implementation is airframe-specific.
* @see obm_cyclone.c
*/
struct FloatVect3 evaluate_obm_forces(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
const float actuator_state[ANDI_NUM_ACT]);
/**
* @brief Evaluate total moments acting on the vehicle from the OBM
*
* Computes the full set of moments modeled by the on board model for
* the given airframe as a function of the state and input.
*
* This function is used for complementary filtering in ANDI.
*
* @param[in] rates Current angular rates (p, q, r) used by the model (rad/s).
* @param[in] vel_body Body-frame linear velocity vector (u, v, w) (m/s).
* @param[in] actuator_state Current actuator commands/deflections array (length
* ANDI_NUM_ACT). Units and normalization depend on the
* airframe and actuator type (e.g. radians, throttle fraction).
* @return Computed moment vector produced by the actuators as per the OBM.
*
* @note Implementation is airframe-specific.
* @see obm_cyclone.c
*/
struct FloatVect3 evaluate_obm_moments(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
const float actuator_state[ANDI_NUM_ACT]);
/**
* @brief Evaluate the state-dependent control effectiveness matrix F_u for stabilization.
*
* This function computes the mapping from actuator inputs to aerodynamic/motor outputs
* for the stabilization model. The output is written into the provided, pre-allocated
* array fu_mat of size ANDI_NUM_ACT * ANDI_OUTPUTS.
*
* The produced matrix represents the partial derivatives of the modeled outputs with
* respect to actuator commands (∂f/∂u) evaluated at the current state and actuator
* conditions. It is intended to be implemented per-airframe (see obm_cyclone for an
* example).
*
* @param[out] fu_mat Pre-allocated array (size ANDI_NUM_ACT * ANDI_OUTPUTS) receiving
* the flattened control-effectiveness matrix. Convention: element
* for output i and actuator j should be written to
* fu_mat[i * ANDI_NUM_ACT + j].
* @param[in] rates Current angular rates (typically p, q, r) used by the model (rad/s).
* @param[in] vel_body Body-frame linear velocity vector (u, v, w) (m/s).
* @param[in] actuator_state Current actuator commands/deflections array (length
* ANDI_NUM_ACT). Units and normalization depend on the
* airframe and actuator type (e.g. radians, throttle fraction).
*
* @note Implementations are airframe-specific and must account for the particular
* actuator layout and aerodynamic/motor characteristics of the platform.
* @see obm_cyclone.c
*/
void evaluate_obm_f_stb_u(float fu_mat[ANDI_NUM_ACT * ANDI_OUTPUTS], const struct FloatRates *rates,
const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT]);
/**
* @brief Evaluate the state-dependent contribution F_x * x_dot for stabilization.
*
* Computes nu_obm = F_x * x_dot, the portion of the open-body-model outputs that
* depends on the current state rates/accelerations (i.e. the state-dependent term).
* This is the term that captures how changes in state drive the modeled outputs
* independent of actuator inputs.
*
* @note This contribution is often neglected for INDI, but can be included for ANDI.
*
* @param[out] nu_obm Pre-allocated output vector of length ANDI_OUTPUTS receiving
* the computed F_x * x_dot values.
* @param[in] rates Current angular rates (p, q, r) used by the model (rad/s).
* @param[in] vel_body Body-frame velocity vector (u, v, w) (m/s).
* @param[in] ang_accel Body angular accelerations (p_dot, q_dot, r_dot) (rad/s^2).
* @param[in] accel_body Body-frame linear accelerations (ax, ay, az) (m/s^2).
* @param[in] actuator_state Current actuator commands/deflections array (length
* ANDI_NUM_ACT). Some terms of F_x may be actuator-state
* dependent (e.g. rotor wake effects).
*
* @note The function computes only the state-dependent part of the model. The full
* modeled output is typically nu_obm + F_u * delta_u (where F_u is provided by
* evaluate_obm_f_stb_u). Implementations must be provided per airframe.
* @see obm_cyclone.c
*/
void evaluate_obm_f_stb_x(float nu_obm[ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body,
const struct FloatVect3 *ang_accel, const struct FloatVect3 *accel_body, const float actuator_state[ANDI_NUM_ACT]);
/**
* @brief Compute total thrust produced by the current actuator state.
*
* Returns the aggregate thrust generated by the set of actuators described by
* actuator_state. This value is typically used by the stabilization/obm code when
* converting actuator commands to net force for direct thrust control.
*
* @note This function does not intend to accurately compute the thrust force;
* it only returns an approximation of the total specific thrust in the body z-direction.
* The intended use is as feedback for direct throttle control in ANDI stabilization.
*
* @param[in] actuator_state Current actuator commands/deflections array (length
* ANDI_NUM_ACT). Units and normalization depend on the
* airframe (e.g. rotor collective, throttle fraction).
* @return Total specific thrust produced by the actuators (SI units, m/s^2). If
* the airframe model uses a normalized thrust unit, document that convention
* in the airframe implementation.
*
* @note Implementation is airframe-specific.
* @see obm_cyclone.c
*/
float evaluate_obm_thrust_z(const float actuator_state[ANDI_NUM_ACT]);
#endif // STABILIZATION_ANDI_H
sw/airborne/math/pprz_algebra_float.c
+88 -76
View File
File diff suppressed because it is too large Load Diff
sw/airborne/math/pprz_algebra_float.h
+108 -103
View File
@@ -104,7 +104,7 @@ struct FloatRates {
/*
* Returns the real part of the log of v in base of n
*/
static inline float float_log_n(float v, float n)
static inline float float_log_n(const float v, const float n)
{
if (fabsf(v) < 1e-4) { // avoid inf
return - 1.0E+30;
@@ -131,12 +131,12 @@ static inline float float_log_n(float v, float n)
/* macros also usable if _v is not a FloatVect2, but a different struct with x,y members */
#define FLOAT_VECT2_NORM(_v) sqrtf(VECT2_NORM2(_v))
static inline float float_vect2_norm2(struct FloatVect2 *v)
static inline float float_vect2_norm2(const struct FloatVect2 *v)
{
return v->x * v->x + v->y * v->y;
}
static inline float float_vect2_norm(struct FloatVect2 *v)
static inline float float_vect2_norm(const struct FloatVect2 *v)
{
return sqrtf(float_vect2_norm2(v));
}
@@ -163,12 +163,12 @@ static inline void float_vect2_normalize(struct FloatVect2 *v)
/* macros also usable if _v is not a FloatVect3, but a different struct with x,y,z members */
#define FLOAT_VECT3_NORM(_v) sqrtf(VECT3_NORM2(_v))
static inline float float_vect3_norm2(struct FloatVect3 *v)
static inline float float_vect3_norm2(const struct FloatVect3 *v)
{
return v->x * v->x + v->y * v->y + v->z * v->z;
}
static inline float float_vect3_norm(struct FloatVect3 *v)
static inline float float_vect3_norm(const struct FloatVect3 *v)
{
return sqrtf(float_vect3_norm2(v));
}
@@ -201,14 +201,17 @@ static inline void float_vect3_normalize(struct FloatVect3 *v)
}
extern void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv,
float dt);
extern void float_vect3_integrate_fi(struct FloatVect3 *vec, const struct FloatVect3 *dv,
const float dt);
extern void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr,
float dt);
extern void float_rates_integrate_fi(struct FloatRates *r, const struct FloatRates *dr,
const float dt);
extern void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e,
struct FloatEulers *edot);
extern void float_rates_vect3_integrate_fi(struct FloatRates *r, const struct FloatVect3 *dr,
const float dt);
extern void float_rates_of_euler_dot(struct FloatRates *r, const struct FloatEulers *e,
const struct FloatEulers *edot);
/* defines for backwards compatibility */
#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) WARNING("FLOAT_VECT3_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_vect3_integrate_fi(&(_vo), &(_dv), _dt)
@@ -260,61 +263,61 @@ static inline void float_rmat_identity(struct FloatRMat *rm)
/** Inverse/transpose of a rotation matrix.
* m_b2a = inv(_m_a2b) = transp(_m_a2b)
*/
extern void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b);
extern void float_rmat_inv(struct FloatRMat *m_b2a, const struct FloatRMat *m_a2b);
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
extern void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b,
struct FloatRMat *m_b2c);
extern void float_rmat_comp(struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b,
const struct FloatRMat *m_b2c);
/** Composition (multiplication) of two rotation matrices.
* m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c
*/
extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c,
struct FloatRMat *m_b2c);
extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, const struct FloatRMat *m_a2c,
const struct FloatRMat *m_b2c);
/// Norm of a rotation matrix.
extern float float_rmat_norm(struct FloatRMat *rm);
extern float float_rmat_norm(const struct FloatRMat *rm);
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
*/
extern void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b,
struct FloatVect3 *va);
extern void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b,
const struct FloatVect3 *va);
/** rotate 3D vector by transposed rotation matrix.
* vb = m_b2a^T * va
*/
extern void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a,
struct FloatVect3 *va);
extern void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a,
const struct FloatVect3 *va);
/** rotate angle by rotation matrix.
* rb = m_a2b * ra
*/
extern void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b,
struct FloatEulers *ra);
extern void float_rmat_mult(struct FloatEulers *rb, const struct FloatRMat *m_a2b,
const struct FloatEulers *ra);
/** rotate angle by transposed rotation matrix.
* rb = m_b2a^T * ra
*/
extern void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a,
struct FloatEulers *ra);
extern void float_rmat_transp_mult(struct FloatEulers *rb, const struct FloatRMat *m_b2a,
const struct FloatEulers *ra);
/** rotate anglular rates by rotation matrix.
* rb = m_a2b * ra
*/
extern void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b,
struct FloatRates *ra);
extern void float_rmat_ratemult(struct FloatRates *rb, const struct FloatRMat *m_a2b,
const struct FloatRates *ra);
/** rotate anglular rates by transposed rotation matrix.
* rb = m_b2a^T * ra
*/
extern void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a,
struct FloatRates *ra);
extern void float_rmat_transp_ratemult(struct FloatRates *rb, const struct FloatRMat *m_b2a,
const struct FloatRates *ra);
/** initialises a rotation matrix from unit vector axis and angle */
extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle);
extern void float_rmat_of_axis_angle(struct FloatRMat *rm, const struct FloatVect3 *uv, const float angle);
/** Rotation matrix from 321 Euler angles (float).
* The Euler angles are interpreted as zy'x'' (intrinsic) rotation.
@@ -328,13 +331,13 @@ extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv
* @param[out] rm pointer to rotation matrix
* @param[in] e pointer to Euler angles
*/
extern void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e);
extern void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e);
extern void float_rmat_of_eulers_321(struct FloatRMat *rm, const struct FloatEulers *e);
extern void float_rmat_of_eulers_312(struct FloatRMat *rm, const struct FloatEulers *e);
#define float_rmat_of_eulers float_rmat_of_eulers_321
extern void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q);
extern void float_rmat_of_quat(struct FloatRMat *rm, const struct FloatQuat *q);
/** in place first order integration of a rotation matrix */
extern void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt);
extern void float_rmat_integrate_fi(struct FloatRMat *rm, const struct FloatRates *omega, const float dt);
extern float float_rmat_reorthogonalize(struct FloatRMat *rm);
/* defines for backwards compatibility */
@@ -372,7 +375,7 @@ static inline void float_quat_identity(struct FloatQuat *q)
#define FLOAT_QUAT_NORM2(_q) (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz))
static inline float float_quat_norm(struct FloatQuat *q)
static inline float float_quat_norm(const struct FloatQuat *q)
{
return sqrtf(SQUARE(q->qi) + SQUARE(q->qx) + SQUARE(q->qy) + SQUARE(q->qz));
}
@@ -388,7 +391,7 @@ static inline void float_quat_normalize(struct FloatQuat *q)
}
}
static inline void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
static inline void float_quat_invert(struct FloatQuat *qo, const struct FloatQuat *qi)
{
QUAT_INVERT(*qo, *qi);
}
@@ -406,72 +409,75 @@ static inline void float_quat_wrap_shortest(struct FloatQuat *q)
/** Composition (multiplication) of two quaternions.
* a2c = a2b comp b2c , aka a2c = a2b * b2c
*/
extern void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
extern void float_quat_comp(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c);
/** Composition (multiplication) of two quaternions.
* a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
*/
extern void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
extern void float_quat_comp_inv(struct FloatQuat *a2b, const struct FloatQuat *a2c, const struct FloatQuat *b2c);
/** Composition (multiplication) of two quaternions.
* b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
*/
extern void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
extern void float_quat_inv_comp(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c);
/** Composition (multiplication) of two quaternions with normalization.
* a2c = a2b comp b2c , aka a2c = a2b * b2c
*/
extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, const struct FloatQuat *a2b,
const struct FloatQuat *b2c);
/** Composition (multiplication) of two quaternions with normalization.
* a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
*/
extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, const struct FloatQuat *a2c,
const struct FloatQuat *b2c);
/** Composition (multiplication) of two quaternions with normalization.
* b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
*/
extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b,
const struct FloatQuat *a2c);
/** Quaternion derivative from rotational velocity.
* qd = -0.5*omega(r) * q
* or equally:
* qd = 0.5 * q * omega(r)
*/
extern void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
extern void float_quat_derivative(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q);
/** Quaternion derivative from rotational velocity with Lagrange multiplier.
* qd = -0.5*omega(r) * q
* or equally:
* qd = 0.5 * q * omega(r)
*/
extern void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
extern void float_quat_derivative_lagrange(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q);
/** Delta rotation quaternion with constant angular rates.
*/
extern void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt);
extern void float_quat_differential(struct FloatQuat *q_out, const struct FloatRates *w, const float dt);
/** in place first order quaternion integration with constant rotational velocity */
extern void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt);
extern void float_quat_integrate_fi(struct FloatQuat *q, const struct FloatRates *omega, const float dt);
/** in place quaternion integration with constant rotational velocity */
extern void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt);
extern void float_quat_integrate(struct FloatQuat *q, const struct FloatRates *omega, const float dt);
/** rotate 3D vector by quaternion.
* vb = q_a2b * va * q_a2b^-1
*/
extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in);
extern void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in);
/// Quaternion from Euler angles.
extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e);
extern void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e);
extern void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e);
extern void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e);
extern void float_quat_of_eulers_zxy(struct FloatQuat *q, const struct FloatEulers *e);
extern void float_quat_of_eulers_yxz(struct FloatQuat *q, const struct FloatEulers *e);
/** Quaternion from unit vector and angle.
* Output quaternion is not normalized.
* It will be a unit quaternion only if the input vector is also unitary.
*/
extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle);
extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, const float angle);
/** Quaternion from orientation vector.
* Length/norm of the vector is the angle.
@@ -479,10 +485,10 @@ extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect
extern void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov);
/// Quaternion from rotation matrix.
extern void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm);
extern void float_quat_of_rmat(struct FloatQuat *q, const struct FloatRMat *rm);
/// Tilt twist decomposition of quaternion
extern void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat);
extern void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, const struct FloatQuat *quat);
/* defines for backwards compatibility */
@@ -518,14 +524,14 @@ extern void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twis
#define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
static inline float float_eulers_norm(struct FloatEulers *e)
static inline float float_eulers_norm(const struct FloatEulers *e)
{
return sqrtf(SQUARE(e->phi) + SQUARE(e->theta) + SQUARE(e->psi));
}
extern void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm);
extern void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q);
extern void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q);
extern void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q);
extern void float_eulers_of_rmat(struct FloatEulers *e, const struct FloatRMat *rm);
extern void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q);
extern void float_eulers_of_quat_zxy(struct FloatEulers *e, const struct FloatQuat *q);
extern void float_eulers_of_quat_yxz(struct FloatEulers *e, const struct FloatQuat *q);
/* defines for backwards compatibility */
#define FLOAT_EULERS_OF_RMAT(_e, _rm) WARNING("FLOAT_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") float_eulers_of_rmat(&(_e), &(_rm))
@@ -642,12 +648,12 @@ static inline float float_vect_dot_product(const float *a, const float *b, const
for (__i = 0; __i < _rows; __i++) { _ptr[__i] = &_mat[__i][0]; } \
}
extern void float_mat_invert(float **o, float **mat, int n);
extern void float_mat_exp(float **a, float **o, int n);
extern float float_mat_norm_li(float **o, int m, int n);
extern void float_mat_invert(float **o, float **mat, const int n);
extern void float_mat_exp(float **a, float **o, const int n);
extern float float_mat_norm_li(float **o, const int m, const int n);
/** a = 0 */
static inline void float_mat_zero(float **a, int m, int n)
static inline void float_mat_zero(float **a, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -656,7 +662,7 @@ static inline void float_mat_zero(float **a, int m, int n)
}
/** a = b */
static inline void float_mat_copy(float **a, float **b, int m, int n)
static inline void float_mat_copy(float **a, float **b, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -667,7 +673,7 @@ static inline void float_mat_copy(float **a, float **b, int m, int n)
/** o = a + b */
static inline void float_mat_sum(float **o, float **a, float **b, int m, int n)
static inline void float_mat_sum(float **o, float **a, float **b, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -676,7 +682,7 @@ static inline void float_mat_sum(float **o, float **a, float **b, int m, int n)
}
/** o = a - b */
static inline void float_mat_diff(float **o, float **a, float **b, int m, int n)
static inline void float_mat_diff(float **o, float **a, float **b, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -685,7 +691,7 @@ static inline void float_mat_diff(float **o, float **a, float **b, int m, int n)
}
/** transpose square matrix */
static inline void float_mat_transpose_square(float **a, int n)
static inline void float_mat_transpose_square(float **a, const int n)
{
int i, j;
for (i = 0; i < n; i++) {
@@ -699,7 +705,7 @@ static inline void float_mat_transpose_square(float **a, int n)
/** transpose non-square matrix */
static inline void float_mat_transpose(float **o, float **a, int n, int m)
static inline void float_mat_transpose(float **o, float **a, const int n, const int m)
{
int i, j;
for (i = 0; i < n; i++) {
@@ -715,7 +721,7 @@ static inline void float_mat_transpose(float **o, float **a, int n, int m)
* b: [n x l]
* o: [m x l]
*/
static inline void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
static inline void float_mat_mul(float **o, float **a, float **b, const int m, const int n, const int l)
{
int i, j, k;
for (i = 0; i < m; i++) {
@@ -734,7 +740,7 @@ static inline void float_mat_mul(float **o, float **a, float **b, int m, int n,
* b: [l x n]
* o: [m x l]
*/
static inline void float_mat_mul_transpose(float **o, float **a, float **b, int m, int n, int l)
static inline void float_mat_mul_transpose(float **o, float **a, float **b, const int m, const int n, const int l)
{
int i, j, k;
for (i = 0; i < m; i++) {
@@ -758,7 +764,7 @@ static inline void float_mat_mul_transpose(float **o, float **a, float **b, int
* of the input matrices when this function is used, which is useful for consecutive multiplications
* (e.g. when doing matrix exponentiation), at the cost of some copy overhead.
*/
static inline void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
static inline void float_mat_mul_copy(float **o, float **a, float **b, const int m, const int n, const int l)
{
float temp[m][l];
int i, j, k;
@@ -782,7 +788,7 @@ static inline void float_mat_mul_copy(float **o, float **a, float **b, int m, in
* b: [n x 1]
* o: [m x 1]
*/
static inline void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
static inline void float_mat_vect_mul(float *o, float **a, const float *b, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -798,7 +804,7 @@ static inline void float_mat_vect_mul(float *o, float **a, float *b, int m, int
* a: [m x n]
* k: [1 x 1]
*/
static inline void float_mat_scale(float **a, float k, int m, int n)
static inline void float_mat_scale(float **a, float k, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -814,7 +820,7 @@ static inline void float_mat_scale(float **a, float k, int m, int n)
* b: [m x n]
* k: [1 x 1]
*/
static inline void float_mat_sum_scaled(float **a, float **b, float k, int m, int n)
static inline void float_mat_sum_scaled(float **a, float **b, const float k, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -831,7 +837,7 @@ static inline void float_mat_sum_scaled(float **a, float **b, float k, int m, in
* o: [I(d,d) 0 ]
* [ 0 a(d,m:d,n)]
*/
static inline void float_mat_minor(float **o, float **a, int m, int n, int d)
static inline void float_mat_minor(float **o, float **a, const int m, const int n, const int d)
{
int i, j;
float_mat_zero(o, m, n);
@@ -844,7 +850,7 @@ static inline void float_mat_minor(float **o, float **a, int m, int n, int d)
}
/** o = I - v v^T */
static inline void float_mat_vmul(float **o, float *v, int n)
static inline void float_mat_vmul(float **o, const float *v, const int n)
{
int i, j;
for (i = 0; i < n; i++) {
@@ -858,7 +864,7 @@ static inline void float_mat_vmul(float **o, float *v, int n)
}
/** o = c-th column of matrix a[m x n] */
static inline void float_mat_col(float *o, float **a, int m, int c)
static inline void float_mat_col(float *o, float **a, const int m, const int c)
{
int i;
for (i = 0; i < m; i++) {
@@ -867,7 +873,7 @@ static inline void float_mat_col(float *o, float **a, int m, int c)
}
/** Make an n x n identity matrix (for matrix passed as array) */
static inline void float_mat_diagonal_scal(float **o, float v, int n)
static inline void float_mat_diagonal_scal(float **o, float v, const int n)
{
int i, j;
for (i = 0 ; i < n; i++) {
@@ -883,41 +889,40 @@ static inline void float_mat_diagonal_scal(float **o, float v, int n)
/** Divide a matrix by a scalar */
static inline void float_mat_div_scalar(float **o, float **a, float scalar, int m, int n)
static inline void float_mat_div_scalar(float **o, float **a, const float scalar, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++) {
o[i][j] = a[i][j] / scalar;
}
}
int i, j;
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++) {
o[i][j] = a[i][j] / scalar;
}
}
}
/** Multiply a matrix by a scalar */
static inline void float_mat_mul_scalar(float **o, float **a, float scalar, int m, int n)
static inline void float_mat_mul_scalar(float **o, float **a, const float scalar, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++) {
o[i][j] = a[i][j] * scalar;
}
}
int i, j;
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++) {
o[i][j] = a[i][j] * scalar;
}
}
}
extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
extern bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3]);
extern void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in);
extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);
extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);
extern void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound);
extern void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des);
extern void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound);
extern void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des);
extern bool float_mat_inv_2d(float inv_out[4], const float mat_in[4]);
extern void float_mat2_mult(struct FloatVect2 *vect_out, const float mat[4], const struct FloatVect2 vect_in);
extern bool float_mat_inv_3d(float inv_out[3][3], const float mat_in[3][3]);
extern void float_mat3_mult(struct FloatVect3 *vect_out, const float mat[3][3], const struct FloatVect3 vect_in);
extern bool float_mat_inv_4d(float invOut[4][4], const float mat_in[4][4]);
extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, const float bound);
extern void float_vect3_bound_in_3d(struct FloatVect3 *vect3, const float bound);
extern void float_vect3_scale_in_2d(struct FloatVect3 *vect3, const float norm_des);
extern void float_vect2_bound_in_2d(struct FloatVect2 *vect2, const float bound);
extern void float_vect2_scale_in_2d(struct FloatVect2 *vect2, const float norm_des);
#ifdef __cplusplus
} /* extern "C" */
#endif
sw/airborne/state.h
+8
View File
@@ -1097,6 +1097,14 @@ static inline struct Int32Vect3 *stateGetAccelBody_i(void)
}
/** @}*/
static inline struct FloatVect3 *stateGetAccelBody_f(void)
{
static struct FloatVect3 body_accel_f;
body_accel_f.x = FLOAT_OF_BFP(state.body_accel_i.x, INT32_ACCEL_FRAC);
body_accel_f.y = FLOAT_OF_BFP(state.body_accel_i.y, INT32_ACCEL_FRAC);
body_accel_f.z = FLOAT_OF_BFP(state.body_accel_i.z, INT32_ACCEL_FRAC);
return &body_accel_f;
}
/******************************************************************************
sw/include/std.h
+22 -21
View File
@@ -31,7 +31,7 @@
#include <math.h>
#ifdef SITL
#include <stdio.h> // for debuging in simulation
#include <stdio.h> // for debuging in simulation
#endif
/* some convenience macros to print debug/config messages at compile time */
@@ -44,46 +44,46 @@
#define PTR(_f) &_f
#ifndef FALSE
#define FALSE false
#define FALSE false
#endif
#ifndef TRUE
#define TRUE true
#define TRUE true
#endif
#ifndef NULL
#ifdef __cplusplus
#define NULL 0
#else
#define NULL ((void *)0)
#endif
#ifdef __cplusplus
#define NULL 0
#else
#define NULL ((void *)0)
#endif
#endif
/* Unit (void) values */
typedef uint8_t unit_t;
#ifndef M_PI
#define M_PI 3.14159265358979323846
#define M_PI 3.14159265358979323846
#endif
#ifndef M_PI_6
#define M_PI_6 (M_PI/6)
#define M_PI_6 (M_PI/6)
#endif
#ifndef M_PI_4
#define M_PI_4 (M_PI/4)
#define M_PI_4 (M_PI/4)
#endif
#ifndef M_PI_2
#define M_PI_2 (M_PI/2)
#define M_PI_2 (M_PI/2)
#endif
#ifndef bit_is_set
#define bit_is_set(x, b) ((x >> b) & 0x1)
#define bit_is_set(x, b) ((x >> b) & 0x1)
#endif
#ifndef _BV
#define _BV(bit) (1 << (bit))
#define _BV(bit) (1 << (bit))
#endif
#define SetBit(a, n) a |= (1 << n)
@@ -122,6 +122,7 @@ typedef uint8_t unit_t;
#define DeciDegOfRad(x) ((x) * (1800./ M_PI))
#define RadOfDeg(x) ((x) * (M_PI/180.))
#define RadOfDeciDeg(x) ((x) * (M_PI/1800.))
#define RadOfCentiDeg(x) ((x) * (M_PI/18000.))
#define MOfCm(_x) (((float)(_x))/100.)
#define MOfMm(_x) (((float)(_x))/1000.)
@@ -134,7 +135,7 @@ typedef uint8_t unit_t;
#define MoreThan(_x, _y) ((_x) > (_y))
#ifndef ABS
#define ABS(val) ((val) < 0 ? -(val) : (val))
#define ABS(val) ((val) < 0 ? -(val) : (val))
#endif
#define BoundUpper(_x, _max) { if (_x > (_max)) _x = (_max);}
@@ -270,17 +271,17 @@ static inline bool str_equal(const char *a, const char *b)
}
#ifdef __GNUC__
# define UNUSED __attribute__((__unused__))
# define WEAK __attribute__((weak))
#define UNUSED __attribute__((__unused__))
#define WEAK __attribute__((weak))
#else
# define UNUSED
# define WEAK
#define UNUSED
#define WEAK
#endif
#if __GNUC__ >= 7
# define INTENTIONAL_FALLTHRU __attribute__ ((fallthrough));
#define INTENTIONAL_FALLTHRU __attribute__ ((fallthrough));
#else
# define INTENTIONAL_FALLTHRU
#define INTENTIONAL_FALLTHRU
#endif
#endif /* STD_H */