Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-05 10:41:00 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[hybrid] add basic support for Heewing T1 Ranger VTOL (#3464)
Some checks failed
Issues due date / Add labels to issues (push) Has been cancelled
Details
Doxygen / build (push) Has been cancelled
Details
Docker update / build_docker_image (push) Has been cancelled
Details

Browse Source 
The Ranger T1 is a tilt-rotor hybrid plane. As a first basic approach,
two separated controls are used for hovering (INDI) and forward flight
(PID). The mixing is done by a dedicated module and the autopilot is
customized to support the specific modes.
A simple PID controller is added to the rotorcraft firmware as it is not
possible to directly reuse the one from fixedwing firmware.
In a future work, a full INDI version might be used, but this approach
can still serves as an example of customized autopilot.
A simulation model for JSBSim is also provided.
This commit is contained in:
Gautier Hattenberger
2025-06-24 15:02:36 +02:00
committed by GitHub
parent 982c6947a3
commit 4b6c185516
13 changed files with 2193 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

356
conf/airframes/ENAC/hybrid/ranger_1.xml Normal file
View File

@@ -0,0 +1,356 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<airframe name="RANGER_T1">
<description>
HEEWING Ranger T1
Hybrid Quad/Plane with tilt motors
Tawaki v1.0 Chibios
Xbee API
Ublox
SBUS Futaba
</description>
<firmware name="rotorcraft">
<autopilot name="heewing_vtol"/>
<configure name="PERIODIC_FREQUENCY" value="500"/>
<target name="ap" board="tawaki_2.0">
<module name="radio_control" type="sbus">
<define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
</module>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
</target>
<target name="nps" board="pc">
<module name="fdm" type="jsbsim"/>
<module name="radio_control" type="datalink"/>
<define name="TAWAKIV2_IMU_ROT" value=""/>
<define name="TAWAKIV2_MAG_ROT" value=""/>
</target>
<module name="telemetry" type="xbee_api"/>
<module name="sys_mon"/>
<module name="flight_recorder"/>
<module name="logger" type="control_effectiveness">
<define name="LOGGER_CONTROL_EFFECTIVENESS_COMMANDS" value="TRUE"/>
<define name="LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS" value="TRUE"/>
<define name="LOGGER_CONTROL_EFFECTIVENESS_POS" value="TRUE"/>
<define name="LOGGER_CONTROL_EFFECTIVENESS_SPEED" value="TRUE"/>
<define name="LOGGER_CONTROL_EFFECTIVENESS_AIRSPEED" value="TRUE"/>
</module>
<module name="shell">
<configure name="SHELL_PORT" value="uart3"/>
</module>
<module name="actuators" type="pwm">
<define name="USE_PWM_TIM3" value="TRUE"/>
<define name="USE_PWM5" value="TRUE"/>
<define name="USE_PWM6" value="TRUE"/>
<define name="USE_PWM7" value="TRUE"/>
<define name="USE_PWM8" value="TRUE"/>
</module>
<!--Use an airspeed sensor and get the measured airspeed in the messages-->
<module name="airspeed" type="sdp3x">
<define name="SDP3X_i2C_DEV" value="i2c2"/>
<define name="SDP3X_PRESSURE_SCALE" value="16.0"/>
<define name="SDP3X_PRESSURE_OFFSET" value="-0.1"/>
<define name="USE_AIRSPEED_SDP3X" value="TRUE"/>
</module>
<module name="board" type="tawaki_2.0"/>
<!--configure name="MAG_LIS3MDL_I2C_DEV" value="i2c2"/-->
<module name="mag" type="rm3100">
<configure name="MAG_RM3100_I2C_DEV" value="i2c2"/>
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
<!--define name="MODULE_RM3100_SYNC_SEND" value="TRUE"/-->
<define name="RM3100_CHAN_X" value="1"/>
<define name="RM3100_CHAN_Y" value="0"/>
<define name="RM3100_CHAN_X_SIGN" value="-"/>
</module>
<module name="air_data">
<!--define name="USE_AIRSPEED_AIR_DATA" value="TRUE"/-->
</module>
<module name="stabilization" type="indi">
<configure name="INDI_NUM_ACT" value="4"/> <!-- only rotorcraft part for now -->
<define name="WLS_N_U_MAX" value = "8"/>
<define name="WLS_N_V_MAX" value = "4"/>
</module>
<module name="guidance" type="indi"/>
<module name="nav" type="hybrid"/>
<module name="ins" type="ekf2">
</module>
<!--module name="ins"/>
<module name="ahrs" type="int_cmpl_quat"/-->
<module name="gps" type="ublox">
<configure name="GPS_BAUD" value="B115200"/>
<define name="GPS_FIX_TIMEOUT" value="0.5"/>
</module>
<module name="power_switch"/>
</firmware>
<servos driver="Pwm">
<servo name="MOTOR_RIGHT" no="6" min="1000" neutral="1100" max="1800"/>
<servo name="MOTOR_LEFT" no="5" min="1000" neutral="1100" max="1800"/>
<servo name="MOTOR_TAIL" no="7" min="1000" neutral="1100" max="1800"/>
<servo name="AILERON_RIGHT" no="8" min="1800" neutral="1550" max="1200"/>
<servo name="AILERON_LEFT" no="3" min="1800" neutral="1450" max="1200"/>
<servo name="ELEVATOR" no="4" min="1100" neutral="1500" max="1900"/>
<servo name="TILT_RIGHT" no="2" min="900" neutral="900" max="2000"/>
<servo name="TILT_LEFT" no="1" min="2000" neutral="2000" max="900"/>
</servos>
<commands>
<axis name="MOTOR_RIGHT" group="REAL" failsafe_value="-9600"/>
<axis name="MOTOR_LEFT" group="REAL" failsafe_value="-9600"/>
<axis name="MOTOR_TAIL" group="REAL" failsafe_value="-9600"/>
<axis name="PITCH" group="VIRTUAL" failsafe_value="0"/>
<axis name="ROLL" group="VIRTUAL" failsafe_value="0"/>
<axis name="YAW" group="VIRTUAL" failsafe_value="0"/>
<axis name="THRUST" group="VIRTUAL" failsafe_value="0"/>
<axis name="TILT" group="VIRTUAL" failsafe_value="0"/>
</commands>
<command_laws>
<set servo="MOTOR_RIGHT" value="@MOTOR_RIGHT"/>
<set servo="MOTOR_LEFT" value="@MOTOR_LEFT"/>
<set servo="MOTOR_TAIL" value="@MOTOR_TAIL"/>
<set servo="AILERON_RIGHT" value="@ROLL"/>
<set servo="AILERON_LEFT" value="@ROLL"/>
<set servo="ELEVATOR" value="@PITCH"/>
<set servo="TILT_RIGHT" value="@TILT + @YAW"/>
<set servo="TILT_LEFT" value="@TILT - @YAW"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="GYRO_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="24"/>
<field name="calibrated" type="struct">
<field name="neutral" value="false"/>
<field name="scale" value="false"/>
<field name="rotation" value="true"/>
</field>
<field value="TAWAKIV2_IMU_ROT"/>
</field>
</define>
<define name="ACCEL_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="24"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
<field name="rotation" value="true"/>
</field>
<field name="neutral" value="-15,-2,55" type="int[]"/>
<field name="scale" value="{{6605,5456,56432},{2691,2225,22965}}"/>
<field value="TAWAKIV2_IMU_ROT"/>
</field>
</define>
<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="false"/>
</field>
<field name="neutral" value="-135,-5,-136" type="int[]"/>
<field name="scale" value="{{16177,28166,11486},{28445,47907,20609}}"/>
</field>
<field type="struct">
<field name="abi_id" value="3"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
<field name="rotation" value="false"/>
</field>
<!--field name="neutral" value="-3414,2416,-826" type="int[]"/>
<field name="scale" value="{{19909,4364,21663},{28395,6749,30542}}"/-->
<field name="neutral" value="-397,-161,-5045" type="int[]"/>
<field name="scale" value="{{51767,39473,6973},{59286,49477,8529}}"/>
<field value="TAWAKIV2_MAG_ROT"/>
</field>
</define>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="1000"/>
</section>
<section name="BAT">
<define name="MilliAmpereOfAdc(_adc)" value="_adc*1."/>
<define name="CATASTROPHIC_BAT_LEVEL" value="12.4" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="13.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="14.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="17.0" unit="V"/>
<define name="BAT_NB_CELLS" value="4"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="40." unit="deg"/>
<define name="SP_MAX_THETA" value="30." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- MR ML MT Y -->
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1" type="matrix">
<field value="{ -16., 16., 0., 0. }"/>
<field value="{ 8., 8., -12., 0. }"/>
<field value="{ 0., 0., 0., 20.0 }"/>
<field value="{ -0.85, -0.85, -0.85, 0.}"/>
</define>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{0, 0, 0, 0}"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="100"/>
<define name="REF_ERR_Q" value="80"/>
<define name="REF_ERR_R" value="70"/>
<define name="REF_RATE_P" value="20."/>
<define name="REF_RATE_Q" value="18.0"/>
<define name="REF_RATE_R" value="11.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
<define name="FILT_CUTOFF" value="5.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_FREQ" value="{12.66, 12.66, 12.66, 25.6}"/>
<define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 170}"/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 1}"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="STABILIZATION_PLANE" prefix="STABILIZATION_PLANE_">
<define name="PHI_PGAIN" value="10000"/>
<define name="PHI_IGAIN" value="50"/>
<define name="PHI_DGAIN" value="600"/>
<define name="THETA_PGAIN" value="8000"/>
<define name="THETA_IGAIN" value="30"/>
<define name="THETA_DGAIN" value="1000"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="180"/>
<define name="DGAIN" value="180"/>
<define name="AGAIN" value="0"/>
<define name="IGAIN" value="30"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.35"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_INDI">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.8"/>
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.6"/>
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.5"/>
<define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
<define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="0.6"/>
<define name="GUIDANCE_H_REF_MAX_SPEED" value="16.0"/> <!--not used-->
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="2000"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1000"/>
<define name="GUIDANCE_INDI_MIN_PITCH" value="-80."/>
<define name="GUIDANCE_INDI_MAX_PITCH" value="25."/>
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>
<define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="18.0"/>
<define name="GUIDANCE_INDI_PITCH_OFFSET_GAIN" value="0.06"/>
<define name="GUIDANCE_INDI_FILTER_CUTOFF" value="1.5"/>
<define name="GUIDANCE_INDI_LINE_GAIN" value="0.7"/>
<define name="FWD_SIDESLIP_GAIN" value="0.20"/>
<!--Flap effectiveness on lift-->
<define name="FE_LIFT_A_PITCH" value="0.00018"/>
<define name="FE_LIFT_B_PITCH" value="0.00072"/>
<define name="FE_LIFT_A_AS" value="0.0008"/>
<define name="FE_LIFT_B_AS" value="0.00009"/>
</section>
<section name="GUIDANCE_PLANE" prefix="GUIDANCE_PLANE_">
<define name="COURSE_KP" value="1.0"/>
<define name="COURSE_KD" value="0.1"/>
<define name="CLIMB_KP" value="0.1"/>
<define name="PITCH_KP" value="0.5"/>
<define name="PITCH_KD" value="0.1"/>
<define name="PITCH_KI" value="0."/>
<define name="THROTTLE_KP" value="0.1"/>
<define name="THROTTLE_KD" value="0.1"/>
<define name="THROTTLE_KI" value="0."/>
<define name="NOMINAL_THROTTLE" value="0.5"/>
<define name="PITCH_TRIM" value="2." unit="deg"/>
</section>
<section name="NAV">
<define name="NAV_CLIMB_VSPEED" value="1.5"/>
<define name="NAV_DESCEND_VSPEED" value="-1.5"/>
<define name="NAV_TAKEOFF_HEIGHT" value="10."/>
<define name="NAV_LANDING_AF_HEIGHT" value="20."/>
<define name="NAV_LANDING_FLARE_HEIGHT" value="10."/>
<define name="NAV_LANDING_DESCEND_SPEED" value="-2.0"/>
<define name="SURVEY_HYBRID_MAX_SWEEP_BACK" value="1"/>
<!--define name="MISSION_ALT_PROXIMITY" value="4."/-->
<define name="NAV_HYBRID_LINE_GAIN" value="0.8"/>
</section>
<include href="conf/mag/toulouse_muret.xml"/>
<section name="MISC">
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="FALSE"/>
<!--define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/-->
<define name="COORDINATED_TURN_AIRSPEED" value="18.0"/>
<define name="MISSION_PATH_SKIP_GOTO" value="TRUE"/>
<define name="BARO_PERIODIC_FREQUENCY" value="50"/>
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="ARRIVED_AT_WAYPOINT" value="5.0"/> <!-- For outdoor -->
<define name="DEFAULT_CIRCLE_RADIUS" value="60"/> <!-- For outdoor -->
<define name="CARROT" value="3.0"/>
<define name="POWER_SWITCH_GPIO" value="GPIOA,GPIO3"/>
</section>
<section name="SIMULATOR" prefix="NPS_"> <!-- no model yet -->
<define name="JSBSIM_MODEL" value="ranger_t1" type="string"/>
<define name="USE_COMMANDS" value="TRUE"/>
</section>
<section name="GCS">
<define name="ALT_SHIFT_PLUS_PLUS" value="3"/>
<define name="ALT_SHIFT_PLUS" value="1"/>
<define name="ALT_SHIFT_MINUS" value="-1"/>
</section>
</airframe>

146
conf/autopilot/heewing_vtol.xml Normal file
View File

@@ -0,0 +1,146 @@
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
<autopilot name="Heewing VTOL">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<modules>
<module name="control_mixing" type="heewing"/>
<module name="stabilization" type="plane_pid"/>
<module name="guidance" type="plane_pid_rotorcraft"/>
</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_ATTITUDE_Z_HOLD" cond="ifndef MODE_AUTO1"/-->
<define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
<define name="DLModeAtt()" value="(autopilot_mode_auto2 == 0)"/>
<define name="DLModeNav()" value="(autopilot_mode_auto2 == 1)"/>
</includes>
<settings>
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
<dl_setting var="autopilot_mode_auto2" min="0" step="1" max="1" module="autopilot" values="ATT|NAV"/>
</settings>
<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</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 throttle mode -->
<mode name="KILL">
<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)"/>
<!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>
<!-- Glide (same as KILL ?) -->
<mode name="FAILSAFE" shortname="FAIL">
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call fun="autopilot_set_motors_on(false)"/>
</on_enter>
<control>
<call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
<call fun="th_sp_from_thrust_i(0, THRUST_AXIS_Z)" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
</mode>
<mode name="MANUAL" shortname="MAN">
<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)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="control_mixing_heewing_manual()"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="FORWARD" shortname="FWD">
<on_enter>
<call fun="control_mixing_heewing_attitude_plane_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="control_mixing_heewing_attitude_plane()"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="ATTITUDE" shortname="ATT">
<on_enter>
<call fun="control_mixing_heewing_attitude_direct_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="control_mixing_heewing_attitude_direct()"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="NAV">
<select cond="$DEFAULT_MODE"/>
<on_enter>
<call fun="control_mixing_heewing_nav_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="control_mixing_heewing_nav_run()"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>
<mode name="HOME">
<on_enter>
<call fun="control_mixing_heewing_nav_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call fun="control_mixing_heewing_nav_run()"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
</state_machine>
</autopilot>

23
conf/modules/control_mixing_heewing.xml Normal file
View File

@@ -0,0 +1,23 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="control_mixing_heewing" dir="ctrl" task="control">
<doc>
<description>Control mixing specific to the Heewing T1 Ranger</description>
</doc>
<header>
<file name="control_mixing_heewing.h"/>
</header>
<init fun="control_mixing_heewing_init()"/>
<makefile>
<file name="control_mixing_heewing.c"/>
<test firmware="rotorcraft">
<define name="INDI_OUTPUTS" value="4"/>
<define name="INDI_NUM_ACT" value="4"/>
<define name="COMMAND_MOTOR_RIGHT" value="0"/>
<define name="COMMAND_MOTOR_LEFT" value="1"/>
<define name="COMMAND_MOTOR_TAIL" value="2"/>
<define name="COMMAND_TILT" value="3"/>
<define name="ACTUATORS_NB" value="4"/>
<define name="PERIODIC_FREQUENCY" value="500"/>
</test>
</makefile>
</module>

58
conf/modules/guidance_plane_pid_rotorcraft.xml Normal file
View File

@@ -0,0 +1,58 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="guidance_plane_pid_rotorcraft" dir="guidance" task="control">
<doc>
<description>
Basic guidance code for fixedwing planes using the rotorcraft firmware
with PID control, no airspeed control
</description>
<section name="GUIDANCE_PLANE" prefix="GUIDANCE_PLANE_">
<define name="MAX_BANK" value="20" description="maximum commanded bank angle" unit="deg"/>
<define name="COURSE_KP" value="79" description="feedback horizontal control P gain"/>
<define name="COURSE_KD" value="100" description="feedback horizontal control D gain"/>
<define name="ALT_KP" value="283" description="feedback vertical control P gain"/>
<define name="MAX_CLIMB" value="1.5" description="maximum commanded vertical speed" unit="m/s"/>
<define name="PITCH_KP" value="283" description="feedback vertical control P gain"/>
<define name="PITCH_KD" value="82" description="feedback vertical control D gain"/>
<define name="PITCH_KI" value="13" description="feedback vertical control I gain"/>
<define name="THROTTLE_KP" value="283" description="feedback vertical control P gain"/>
<define name="THROTTLE_KD" value="82" description="feedback vertical control D gain"/>
<define name="THROTTLE_KI" value="13" description="feedback vertical control I gain"/>
<define name="NOMINAL_THROTTLE" value="0.5" description="nominal throttle at hover (between 0 and 1)"/>
</section>
</doc>
<settings target="ap|nps">
<dl_settings>
<dl_settings NAME="guidance plane">
<dl_setting var="guidance_plane.roll_max_setpoint" min="10." step="1." max="60." shortname="max bank" param="GUIDANCE_PLANE_MAX_BANK" module="guidance/guidance_plane" unit="rad" alt_unit="deg"/>
<dl_setting var="guidance_plane.pitch_max_setpoint" min="10." step="1." max="45." shortname="max pitch" param="GUIDANCE_PLANE_MAX_PITCH" unit="rad" alt_unit="deg"/>
<dl_setting var="guidance_plane.pitch_min_setpoint" min="-30." step="1." max="-10." shortname="min pitch" param="GUIDANCE_PLANE_MIN_PITCH" unit="rad" alt_unit="deg"/>
<dl_setting var="guidance_plane.pitch_trim" min="-10." step="1." max="10." shortname="pitch trim" param="GUIDANCE_PLANE_PITCH_TRIM" unit="rad" alt_unit="deg"/>
<dl_setting var="guidance_plane.cruise_throttle" min="0." step="0.01" max="1." shortname="cruise throttle" param="GUIDANCE_PLANE_NOMINAL_THROTTLE"/>
<dl_setting var="guidance_plane.climb_max_setpoint" min="0.5" step="0.1" max="5." shortname="max climb" param="GUIDANCE_PLANE_MAX_CLIMB" unit="m/s"/>
<dl_setting var="guidance_plane.course_kp" min="0." step="0.01" max="10." shortname="course kp" param="GUIDANCE_PLANE_COURSE_KP"/>
<dl_setting var="guidance_plane.course_kd" min="0." step="0.01" max="10." shortname="course kd" param="GUIDANCE_PLANE_COURSE_KD"/>
<dl_setting var="guidance_plane.climb_kp" min="0." step="0.01" max="10." shortname="climb kp" param="GUIDANCE_PLANE_CLIMB_KP"/>
<dl_setting var="guidance_plane.climb_throttle_increment" min="0." step="0.01" max="0.5" shortname="throttle increment" param="GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT"/>
<dl_setting var="guidance_plane.pitch_of_vz" min="0." step="0.1" max="20." shortname="pitch of vz" param="GUIDANCE_PLANE_PITCH_OF_VZ" unit="rad" alt_unit="deg"/>
<dl_setting var="guidance_plane.p_kp" min="0." step="0.1" max="10." shortname="pitch kp" param="GUIDANCE_PLANE_PITCH_KP"/>
<dl_setting var="guidance_plane.p_kd" min="0." step="0.1" max="10." shortname="pitch kd" param="GUIDANCE_PLANE_PITCH_KD"/>
<dl_setting var="guidance_plane.p_ki" min="0." step="0.1" max="10." shortname="pitch ki" param="GUIDANCE_PLANE_PITCH_KI"/>
<dl_setting var="guidance_plane.t_kp" min="0." step="0.1" max="10." shortname="throttle kp" param="GUIDANCE_PLANE_THROTTLE_KP"/>
<dl_setting var="guidance_plane.t_kd" min="0." step="0.1" max="10." shortname="throttle kd" param="GUIDANCE_PLANE_THROTTLE_KD"/>
<dl_setting var="guidance_plane.t_ki" min="0." step="0.1" max="10." shortname="throttle ki" param="GUIDANCE_PLANE_THROTTLE_KI"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>@stabilization,guidance_rotorcraft</depends>
<provides>guidance,attitude_command</provides>
</dep>
<header>
<file name="guidance_plane.h"/>
</header>
<init fun="guidance_plane_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="guidance_plane.c" dir="$(SRC_FIRMWARE)/guidance"/>
</makefile>
</module>

42
conf/modules/stabilization_plane_pid.xml Normal file
View File

@@ -0,0 +1,42 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="stabilization_plane_pid" dir="stabilization" task="control">
<doc>
<description>
Basic fixed-wing PID controller for rotorcraft firmware using float euler implementation
</description>
<section name="STABILIZATION_PLANE" prefix="STABILIZATION_PLANE_">
<define name="SP_MAX_PHI" value="45." description="max setpoint for roll angle" unit="deg"/>
<define name="SP_MAX_THETA" value="45." description="max setpoint for pitch angle" unit="deg"/>
<define name="PHI_PGAIN" value="400" description="feedback roll P gain"/>
<define name="PHI_DGAIN" value="300" description="feedback roll D gain"/>
<define name="PHI_IGAIN" value="100" description="feedback roll I gain"/>
<define name="THETA_PGAIN" value="400" description="feedback pitch P gain"/>
<define name="THETA_DGAIN" value="300" description="feedback pitch D gain"/>
<define name="THETA_IGAIN" value="100" description="feedback pitch I gain"/>
</section>
</doc>
<settings target="ap|nps">
<dl_settings>
<dl_settings NAME="Att Loop">
<dl_setting var="stab_plane_gains.p.x" min="1" step="1" max="15000" shortname="pgain phi" param="STABILIZATION_PLANE_PHI_PGAIN" persistent="true" module="stabilization/stabilization_attitude_plane_pid"/>
<dl_setting var="stab_plane_gains.i.x" min="0" step="1" max="800" shortname="igain phi" param="STABILIZATION_PLANE_PHI_IGAIN" persistent="true"/>
<dl_setting var="stab_plane_gains.d.x" min="1" step="1" max="4000" shortname="dgain p" param="STABILIZATION_PLANE_PHI_DGAIN" persistent="true"/>
<dl_setting var="stab_plane_gains.p.y" min="1" step="1" max="15000" shortname="pgain theta" param="STABILIZATION_PLANE_THETA_PGAIN" persistent="true"/>
<dl_setting var="stab_plane_gains.i.y" min="0" step="1" max="800" shortname="igain theta" param="STABILIZATION_PLANE_THETA_IGAIN" persistent="true"/>
<dl_setting var="stab_plane_gains.d.y" min="1" step="1" max="4000" shortname="dgain q" param="STABILIZATION_PLANE_THETA_DGAIN" persistent="true"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>stabilization_rotorcraft,@attitude_command</depends>
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_attitude_plane_pid.h"/>
</header>
<init fun="stabilization_attitude_plane_pid_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_attitude_plane_pid.c" dir="$(SRC_FIRMWARE)/stabilization"/>
</makefile>
</module>

204
conf/simulator/jsbsim/aircraft/Systems/aerodynamics_ranger_t1.xml Normal file
View File

@@ -0,0 +1,204 @@
<aerodynamics>
<axis name="LIFT">
<function name="aero/force/Lift_alpha">
<description>Lift due to alpha</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<table>
<independentVar lookup="row">aero/alpha-rad</independentVar>
<tableData>
-1.57 0.
-0.20 -0.750
0.00 0.250
0.23 1.400
0.60 0.710
1.57 0.
</tableData>
</table>
</product>
</function>
</axis>
<axis name="DRAG">
<function name="aero/force/Drag_basic">
<description>Drag at zero lift</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<value> 0.15 </value>
</product>
</function>
<function name="aero/force/Drag_induced">
<description>Induced drag</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>aero/cl-squared</property>
<value> 0.3 </value>
</product>
</function>
</axis>
<axis name="SIDE">
<function name="aero/force/Side_beta">
<description>Side force due to beta</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>aero/beta-rad</property>
<value>-0.1</value>
</product>
</function>
</axis>
<axis name="ROLL">
<function name="aero/moment/Roll_beta">
<description>Roll moment due to beta</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/bw-ft</property>
<sin>
<property>aero/beta-rad</property>
</sin>
<value>-0.01</value>
</product>
</function>
<function name="aero/moment/Roll_damp">
<description>Roll moment due to roll rate</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/bw-ft</property>
<property>aero/bi2vel</property>
<property>velocities/p-aero-rad_sec</property>
<value>-0.4</value>
</product>
</function>
<function name="aero/moment/Roll_yaw">
<description>Roll moment due to yaw rate</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/bw-ft</property>
<property>aero/bi2vel</property>
<property>velocities/r-aero-rad_sec</property>
<!--value>0.15</value-->
<value>0.</value>
</product>
</function>
<function name="aero/moment/Roll_aileron">
<description>Roll moment due to aileron</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/bw-ft</property>
<property>fcs/aileron_lag</property>
<value> 0.17 </value>
</product>
</function>
</axis>
<axis name="PITCH">
<function name="aero/moment/Pitch_alpha">
<description>Pitch moment due to alpha</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/cbarw-ft</property>
<property>aero/alpha-rad</property>
<value>-0.5</value>
<table>
<independentVar lookup="row">aero/alpha-rad</independentVar>
<tableData>
-1.57 0.
-0.60 0.3
0.00 0.
0.60 -0.3
1.57 0.
</tableData>
</table>
</product>
</function>
<function name="aero/moment/Pitch_elevator">
<description>Pitch moment due to elevator</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/cbarw-ft</property>
<property>fcs/elevator_lag</property>
<value>1.1</value>
</product>
</function>
<function name="aero/moment/Pitch_damp">
<description>Pitch moment due to pitch rate</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/cbarw-ft</property>
<property>aero/ci2vel</property>
<property>velocities/q-aero-rad_sec</property>
<value>-12</value>
</product>
</function>
</axis>
<axis name="YAW">
<function name="aero/moment/Yaw_beta">
<description>Yaw moment due to beta</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/bw-ft</property>
<sin>
<property>aero/beta-rad</property>
</sin>
<value>0.12</value>
</product>
</function>
<function name="aero/moment/Yaw_damp">
<description>Yaw moment due to yaw rate</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/bw-ft</property>
<property>aero/bi2vel</property>
<property>velocities/r-aero-rad_sec</property>
<value>-0.15</value>
</product>
</function>
<function name="aero/moment/Yaw_aileron">
<description>Adverse yaw</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/bw-ft</property>
<property>fcs/aileron_lag</property>
<value>-0.01</value>
</product>
</function>
</axis>
</aerodynamics>

442
conf/simulator/jsbsim/aircraft/ranger_t1.xml Normal file
View File

@@ -0,0 +1,442 @@
<?xml version="1.0"?>
<?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?>
<fdm_config name="QUAD COMPLETE EXT" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd">
<fileheader>
<author>ENAC</author>
<filecreationdate>23-01-2025</filecreationdate>
<version>1.0</version>
<description>Heewing Ranger T1 VTOL</description>
</fileheader>
<metrics>
<wingarea unit="M2"> 0.09 </wingarea>
<wingspan unit="M"> 0.73 </wingspan>
<chord unit="M"> 0.15 </chord>
<htailarea unit="M2"> 0.029 </htailarea>
<htailarm unit="M"> 0.4 </htailarm>
<vtailarea unit="M2"> 0.01 </vtailarea>
<vtailarm unit="M"> 0.4 </vtailarm>
<wing_incidence unit="DEG"> 0 </wing_incidence>
<location name="AERORP" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="EYEPOINT" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="VRP" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</metrics>
<mass_balance>
<ixx unit="KG*M2"> 0.005 </ixx>
<iyy unit="KG*M2"> 0.005 </iyy>
<izz unit="KG*M2"> 0.015 </izz>
<ixy unit="KG*M2"> 0. </ixy>
<ixz unit="KG*M2"> 0. </ixz>
<iyz unit="KG*M2"> 0. </iyz>
<emptywt unit="KG"> 0.67 </emptywt>
<location name="CG" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</mass_balance>
<ground_reactions>
<contact type="STRUCTURE" name="CONTACT_FRONT">
<location unit="M">
<x> -0.12 </x>
<y> 0.0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_BACK">
<location unit="M">
<x> 0.12</x>
<y> 0.0</y>
<z> -0.1</z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_RIGHT">
<location unit="M">
<x> 0.0 </x>
<y> 0.12</y>
<z> -0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_LEFT">
<location unit="M">
<x> 0.0 </x>
<y>-0.12</y>
<z> -0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
</ground_reactions>
<flight_control name="actuator_dynamics">
<property value="30.">fcs/motor_lag</property>
<property value="54.">fcs/servo_lag</property>
<property value="0.">fcs/tilt_min_rad</property>
<property value="1.73329">fcs/tilt_max_rad</property> <!-- 99.3 deg -->
<channel name="filtering">
<!--First order filter represents actuator dynamics-->
<lag_filter name="motor_right_lag">
<input> fcs/MOTOR_RIGHT </input>
<clipto>
<min>0</min>
<max>1</max>
</clipto>
<c1> fcs/motor_lag </c1>
<output> fcs/motor_right_lag</output>
</lag_filter>
<lag_filter name="motor_left_lag">
<input> fcs/MOTOR_LEFT </input>
<clipto>
<min>0</min>
<max>1</max>
</clipto>
<c1> fcs/motor_lag </c1>
<output> fcs/motor_left_lag</output>
</lag_filter>
<lag_filter name="motor_tail_lag">
<input> fcs/MOTOR_TAIL </input>
<clipto>
<min>0</min>
<max>1</max>
</clipto>
<c1> fcs/motor_lag </c1>
<output> fcs/motor_tail_lag</output>
</lag_filter>
<lag_filter name="aileron_lag">
<input> fcs/ROLL </input>
<c1> fcs/servo_lag </c1>
<output> fcs/aileron_lag</output>
</lag_filter>
<lag_filter name="elevator_lag">
<input> fcs/PITCH </input>
<c1> fcs/servo_lag </c1>
<output> fcs/elevator_lag</output>
</lag_filter>
</channel>
<channel name="tilt">
<fcs_function name="tilt_right">
<function>
<sum>
<property>fcs/TILT</property>
<property>fcs/YAW</property>
</sum>
</function>
<output> fcs/tilt_right </output>
</fcs_function>
<aerosurface_scale name="tilt_right_dyn">
<input> fcs/tilt_right </input>
<zero_centered> false </zero_centered>
<domain>
<min> 0. </min>
<max> 1. </max>
</domain>
<range>
<min> 0. </min>
<max> 1.73329 </max>
</range>
<output> fcs/tilt_right_rad </output>
</aerosurface_scale >
<actuator name="tilt_right_output">
<input> fcs/tilt_right_rad </input>
<rate_limit> 3.5 </rate_limit> <!-- 200 degrees/sec -->
<clipto>
<min> fcs/tilt_min_rad </min>
<max> fcs/tilt_max_rad </max>
</clipto>
<output> fcs/tilt_right_lag </output>
</actuator>
<fcs_function name="tilt_right_dir_x">
<function>
<cos><property> -fcs/tilt_right_lag </property></cos>
</function>
<output> external_reactions/motor_right/x </output>
</fcs_function>
<fcs_function name="tilt_right_dir_z">
<function>
<sin><property> -fcs/tilt_right_lag </property></sin>
</function>
<output> external_reactions/motor_right/z </output>
</fcs_function>
<fcs_function name="torque_right_dir_x">
<function>
<cos><property> -fcs/tilt_right_lag </property></cos>
</function>
<output> external_reactions/motor_right_torque/l </output>
</fcs_function>
<fcs_function name="torque_right_dir_z">
<function>
<sin><property> -fcs/tilt_right_lag </property></sin>
</function>
<output> external_reactions/motor_right_torque/n </output>
</fcs_function>
<fcs_function>
<function>
<sum>
<property>fcs/TILT</property>
<property>-fcs/YAW</property>
</sum>
</function>
<output> fcs/tilt_left </output>
</fcs_function>
<aerosurface_scale name="tilt_left">
<input> fcs/tilt_left </input>
<zero_centered> false </zero_centered>
<domain>
<min> 0. </min>
<max> 1. </max>
</domain>
<range>
<min> 0. </min>
<max> 1.73329 </max>
</range>
<output> fcs/tilt_left_rad </output>
</aerosurface_scale >
<actuator name="tilt_left_output">
<input> fcs/tilt_left_rad </input>
<rate_limit> 3.5 </rate_limit> <!-- 200 degrees/sec -->
<clipto>
<min> fcs/tilt_min_rad </min>
<max> fcs/tilt_max_rad </max>
</clipto>
<output> fcs/tilt_left_lag </output>
</actuator>
<fcs_function name="tilt_left_dir_x">
<function>
<cos><property> -fcs/tilt_left_rad </property></cos>
</function>
<output> external_reactions/motor_left/x </output>
</fcs_function>
<fcs_function name="tilt_left_dir_z">
<function>
<sin><property> -fcs/tilt_left_rad </property></sin>
</function>
<output> external_reactions/motor_left/z </output>
</fcs_function>
<fcs_function name="torque_left_dir_x">
<function>
<product>
<value>-1.</value>
<cos><property> -fcs/tilt_left_rad </property></cos>
</product>
</function>
<output> external_reactions/motor_left_torque/l </output>
</fcs_function>
<fcs_function name="torque_left_dir_z">
<function>
<product>
<value>-1.</value>
<sin><property> -fcs/tilt_left_rad </property></sin>
</product>
</function>
<output> external_reactions/motor_left_torque/n </output>
</fcs_function>
</channel>
</flight_control>
<external_reactions>
<property>fcs/MOTOR_RIGHT</property>
<property>fcs/MOTOR_LEFT</property>
<property>fcs/MOTOR_TAIL</property>
<property>fcs/PITCH</property>
<property>fcs/ROLL</property>
<property>fcs/YAW</property>
<property>fcs/THRUST</property>
<property>fcs/TILT</property>
<property>fcs/motor_right_lag</property>
<property>fcs/motor_left_lag</property>
<property>fcs/motor_tail_lag</property>
<property>fcs/aileron_lag</property>
<property>fcs/elevator_lag</property>
<property>fcs/tilt_left_lag</property>
<property>fcs/tilt_right_lag</property>
<property value="3.5">fcs/motor_max_thrust</property> <!-- 0.67 kg hovering at 62% throttle with 3 motors -->
<!--property value="0.00001">fcs/motor_max_torque</property-->
<property value="0.">fcs/motor_max_torque</property>
<!-- First the lift forces produced by each propeller -->
<force name="motor_right" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/motor_right_lag</property>
<property>fcs/motor_max_thrust</property>
<value>0.224808943</value> <!-- N to LBS -->
</product>
</function>
<location unit="M">
<x> -0.09 </x>
<y> 0.12 </y>
<z> 0. </z>
</location>
<direction>
<x> 1 </x>
<y> 0 </y>
<z> 0 </z>
</direction>
</force>
<force name="motor_left" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/motor_left_lag</property>
<property>fcs/motor_max_thrust</property>
<value>0.224808943</value> <!-- N to LBS -->
</product>
</function>
<location unit="M">
<x> -0.09 </x>
<y> -0.12 </y>
<z> 0 </z>
</location>
<direction>
<x> 1 </x>
<y> 0 </y>
<z> 0 </z>
</direction>
</force>
<force name="motor_tail" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/motor_tail_lag</property>
<property>fcs/motor_max_thrust</property>
<value>0.224808943</value> <!-- N to LBS -->
</product>
</function>
<location unit="M">
<x> 0.25 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<!-- Then the Moment Couples -->
<moment name="motor_right_torque" frame="BODY">
<function>
<product>
<property>fcs/motor_right_lag</property>
<property>fcs/motor_max_torque</property>
<value> 0.738 </value> <!-- N.m to FT.LBS -->
</product>
</function>
<location unit="M">
<x> 0.09 </x>
<y> 0.12 </y>
<z> 0. </z>
</location>
<direction>
<x> 1 </x>
<y> 0 </y>
<z> 0 </z>
</direction>
</moment>
<moment name="motor_left_torque" frame="BODY">
<function>
<product>
<property>fcs/motor_left_lag</property>
<property>fcs/motor_max_torque</property>
<value> 0.738 </value> <!-- N.m to FT.LBS -->
</product>
</function>
<location unit="M">
<x> 0.09 </x>
<y> -0.12 </y>
<z> 0. </z>
</location>
<direction>
<x> -1 </x>
<y> 0 </y>
<z> 0 </z>
</direction>
</moment>
<moment name="motor_tail_torque" frame="BODY">
<function>
<product>
<property>fcs/motor_tail_lag</property>
<property>fcs/motor_max_torque</property>
<value> 0.738 </value> <!-- N.m to FT.LBS -->
</product>
</function>
<location unit="M">
<x>-0.25</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</moment>
</external_reactions>
<propulsion/>
<flight_control name="FGFCS"/>
<aerodynamics file="Systems/aerodynamics_ranger_t1.xml"/>
</fdm_config>

284
sw/airborne/firmwares/rotorcraft/guidance/guidance_plane.c Normal file
View File

@@ -0,0 +1,284 @@
/*
* Copyright (C) 2024 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 firmwares/rotorcraft/guidance/guidance_plane.c
* Guidance controller for planes with PID
* compatible with the rotorcraft firmware
* no airspeed control
*
*/
#include "firmwares/rotorcraft/guidance/guidance_plane.h"
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "modules/nav/waypoints.h"
#include "state.h"
#ifndef GUIDANCE_PLANE_MAX_BANK
#define GUIDANCE_PLANE_MAX_BANK RadOfDeg(45.f)
#endif
#ifndef GUIDANCE_PLANE_MAX_PITCH
#define GUIDANCE_PLANE_MAX_PITCH RadOfDeg(30.f)
#endif
#ifndef GUIDANCE_PLANE_MIN_PITCH
#define GUIDANCE_PLANE_MIN_PITCH RadOfDeg(-20.f)
#endif
#ifndef GUIDANCE_PLANCE_COURSE_PRE_BANK
#define GUIDANCE_PLANCE_COURSE_PRE_BANK 1.f
#endif
#ifndef GUIDANCE_PLANE_MAX_CLIMB
#define GUIDANCE_PLANE_MAX_CLIMB 2.f
#endif
#ifndef GUIDANCE_PLANE_PITCH_OF_VZ
#define GUIDANCE_PLANE_PITCH_OF_VZ RadOfDeg(5.f)
#endif
#ifndef GUIDANCE_PLANE_PITCH_TRIM
#define GUIDANCE_PLANE_PITCH_TRIM RadOfDeg(0.f)
#endif
#ifndef GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT
#define GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT 0.1f
#endif
/*
* external variables
*/
struct GuidancePlane guidance_plane;
/*
* internal variables
*/
static float pitch_sum_err;
#define GUIDANCE_PLANE_PITCH_MAX_SUM_ERR (RadOfDeg(10.))
static float throttle_sum_err;
#define GUIDANCE_PLANE_THROTTLE_MAX_SUM_ERR 0.4
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
#endif
void guidance_plane_init(void)
{
guidance_plane.roll_max_setpoint = GUIDANCE_PLANE_MAX_BANK;
guidance_plane.course_setpoint = 0.f;
guidance_plane.course_kp = GUIDANCE_PLANE_COURSE_KP;
guidance_plane.course_kd = GUIDANCE_PLANE_COURSE_KD;
guidance_plane.course_pre_bank_correction = GUIDANCE_PLANCE_COURSE_PRE_BANK;
guidance_plane.roll_cmd = 0.f;
guidance_plane.pitch_max_setpoint = GUIDANCE_PLANE_MAX_PITCH;
guidance_plane.pitch_min_setpoint = GUIDANCE_PLANE_MIN_PITCH;
guidance_plane.climb_max_setpoint = GUIDANCE_PLANE_MAX_CLIMB;
guidance_plane.altitude_setpoint = 0.f;
guidance_plane.climb_setpoint = 0.f;
guidance_plane.climb_kp = GUIDANCE_PLANE_CLIMB_KP;
guidance_plane.pitch_trim = GUIDANCE_PLANE_PITCH_TRIM;
guidance_plane.pitch_of_vz = GUIDANCE_PLANE_PITCH_OF_VZ;
guidance_plane.climb_throttle_increment = GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT;
guidance_plane.p_kp = GUIDANCE_PLANE_PITCH_KP;
guidance_plane.p_kd = GUIDANCE_PLANE_PITCH_KD;
guidance_plane.p_ki = GUIDANCE_PLANE_PITCH_KI;
guidance_plane.t_kp = GUIDANCE_PLANE_THROTTLE_KP;
guidance_plane.t_kd = GUIDANCE_PLANE_THROTTLE_KD;
guidance_plane.t_ki = GUIDANCE_PLANE_THROTTLE_KI;
guidance_plane.cruise_throttle = GUIDANCE_PLANE_NOMINAL_THROTTLE;
guidance_plane.pitch_cmd = 0.f;
guidance_plane.throttle_cmd = 0;
pitch_sum_err = 0.f;
throttle_sum_err = 0.f;
#if PERIODIC_TELEMETRY
#endif
}
/**
* run horizontal control loop for position and speed control
*/
struct StabilizationSetpoint guidance_plane_attitude_from_nav(bool in_flight UNUSED)
{
struct FloatEulers att_sp;
// course control loop
if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_NONE || nav.horizontal_mode == NAV_HORIZONTAL_MODE_GUIDED) {
guidance_plane.roll_cmd = 0.f;
guidance_plane.pitch_cmd = 0.f;
} else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ATTITUDE) {
if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
struct FloatEulers e;
float_eulers_of_quat(&e, &nav.quat);
guidance_plane.roll_cmd = e.phi;
guidance_plane.pitch_cmd = e.theta;
}
else {
guidance_plane.roll_cmd = nav.roll;
guidance_plane.pitch_cmd = nav.pitch;
}
} else {
// update carrot for GCS display and convert ENU float -> NED int
// even if sp is changed later
// FIXME really needed here ?
guidance_h.sp.pos.x = POS_BFP_OF_REAL(nav.carrot.y);
guidance_h.sp.pos.y = POS_BFP_OF_REAL(nav.carrot.x);
switch (nav.setpoint_mode) {
case NAV_SETPOINT_MODE_POS:
guidance_plane.course_setpoint = atan2f(nav.carrot.x - stateGetPositionEnu_f()->x, nav.carrot.y - stateGetPositionNed_f()->y);
break;
case NAV_SETPOINT_MODE_SPEED:
guidance_plane.course_setpoint = atan2f(nav.speed.x, nav.speed.y);
break;
case NAV_SETPOINT_MODE_ACCEL:
// not supported, flying towards HOME waypoint
guidance_plane.course_setpoint = atan2f(waypoint_get_x(WP_HOME) - stateGetPositionEnu_f()->x,
waypoint_get_y(WP_HOME) - stateGetPositionEnu_f()->y);
break;
default:
// nothing to do for other cases at the moment
break;
}
/* final attitude setpoint */
// Ground path error
static float last_err = 0.f;
float err = guidance_plane.course_setpoint - stateGetHorizontalSpeedDir_f();
NormRadAngle(err);
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
//float course_pre_bank = 0.f; // TODO: compute if flying a circle -> pb is that it is coming from nav
// guidance_plane.course_pre_bank_correction * course_pre_bank
guidance_plane.roll_cmd = guidance_plane.course_kp * err + guidance_plane.course_kd * d_err;
BoundAbs(guidance_plane.roll_cmd, guidance_plane.roll_max_setpoint);
}
att_sp.phi = guidance_plane.roll_cmd;
att_sp.theta = guidance_plane.pitch_cmd;
att_sp.psi = stateGetNedToBodyEulers_f()->psi;
return stab_sp_from_eulers_f(&att_sp);
}
static void guidance_plane_set_pitch(bool in_flight)
{
static float last_err = 0.f;
if (!in_flight) {
pitch_sum_err = 0.f;
}
// Compute errors
float err = guidance_plane.climb_setpoint - stateGetSpeedEnu_f()->z;
float d_err = err - last_err;
last_err = err;
if (guidance_plane.p_ki > 0.) {
pitch_sum_err += err * (1. / PERIODIC_FREQUENCY);
BoundAbs(pitch_sum_err, GUIDANCE_PLANE_PITCH_MAX_SUM_ERR / guidance_plane.p_ki);
}
// PID loop + feedforward ctl
guidance_plane.pitch_cmd = nav.pitch
+ guidance_plane.pitch_trim
+ guidance_plane.pitch_of_vz * guidance_plane.climb_setpoint
+ guidance_plane.p_kp * err
+ guidance_plane.p_kd * d_err
+ guidance_plane.p_ki * pitch_sum_err;
}
static void guidance_plane_set_throttle(bool in_flight)
{
static float last_err = 0.;
if (!in_flight) {
throttle_sum_err = 0;
}
// Compute errors
float err = guidance_plane.climb_setpoint - stateGetSpeedEnu_f()->z;
float d_err = err - last_err;
last_err = err;
if (guidance_plane.t_ki > 0.) {
throttle_sum_err += err * (1. / PERIODIC_FREQUENCY);
BoundAbs(throttle_sum_err, GUIDANCE_PLANE_THROTTLE_MAX_SUM_ERR / guidance_plane.t_ki);
}
// PID loop + feedforward ctl
float th_cmd = guidance_plane.cruise_throttle
+ guidance_plane.climb_throttle_increment * guidance_plane.climb_setpoint
+ guidance_plane.t_kp * err
+ guidance_plane.t_kd * d_err
+ guidance_plane.t_ki * throttle_sum_err;
guidance_plane.throttle_cmd = TRIM_UPPRZ(MAX_PPRZ * th_cmd);
}
/**
* run vertical control loop for position and speed control
*/
struct ThrustSetpoint guidance_plane_thrust_from_nav(bool in_flight)
{
if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) {
guidance_plane.throttle_cmd = (int32_t) nav.throttle;
} else {
if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
guidance_plane.altitude_setpoint = nav.nav_altitude;
float alt_err = guidance_plane.altitude_setpoint - stateGetPositionEnu_f()->z;
guidance_plane.climb_setpoint = guidance_plane.climb_kp * alt_err;
} else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
guidance_plane.climb_setpoint = nav.climb;
} else {
guidance_plane.climb_setpoint = 0.f;
}
BoundAbs(guidance_plane.climb_setpoint, guidance_plane.climb_max_setpoint);
guidance_plane_set_pitch(in_flight);
guidance_plane_set_throttle(in_flight);
}
return th_sp_from_thrust_i(guidance_plane.throttle_cmd, THRUST_AXIS_X);
}
void guidance_plane_enter(void)
{
/* set nav_heading to current heading */
//nav.heading = stateGetNedToBodyEulers_f()->psi;
pitch_sum_err = 0.f;
throttle_sum_err = 0.f;
}

86
sw/airborne/firmwares/rotorcraft/guidance/guidance_plane.h Normal file
View File

@@ -0,0 +1,86 @@
/*
* Copyright (C) 2025 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 firmwares/rotorcraft/guidance/guidance_plane.h
* Guidance controller for planes in rotorcraft firmware
* using basic PID controller
* no airspeed control
*
*/
#ifndef GUIDANCE_PLANE_H
#define GUIDANCE_PLANE_H
#ifdef __cplusplus
extern "C" {
#endif
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
struct GuidancePlane {
// horizontal
float roll_max_setpoint;
float pitch_max_setpoint;
float pitch_min_setpoint;
float course_setpoint;
// horizontal gains
float course_kp;
float course_kd;
float course_pre_bank_correction;
// vertical
float altitude_setpoint;
float climb_max_setpoint;
float climb_setpoint;
float pitch_trim;
// vertical gains, pitch and throttle
float cruise_throttle;
float climb_kp;
float pitch_of_vz;
float climb_throttle_increment;
float p_kp;
float p_kd;
float p_ki;
float t_kp;
float t_kd;
float t_ki;
// outputs
float roll_cmd;
float pitch_cmd;
int32_t throttle_cmd; // Throttle command (in pprz_t)
};
/** Guidance PID structyre
*/
extern struct GuidancePlane guidance_plane;
extern void guidance_plane_init(void);
extern void guidance_plane_enter(void);
extern struct StabilizationSetpoint guidance_plane_attitude_from_nav(bool in_flight);
extern struct ThrustSetpoint guidance_plane_thrust_from_nav(bool in_flight);
#ifdef __cplusplus
}
#endif
#endif /* GUIDANCE_PLANE_H */

149
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_plane_pid.c Normal file
View File

@@ -0,0 +1,149 @@
/*
* Copyright (C) 2024 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 stabilization_attitude_plane_pid.c
*
* Basic fixed-wing attitude stabilization in euler float version.
*/
#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_plane_pid.h"
#include "std.h"
#include "paparazzi.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
struct PlaneAttitudeGains stab_plane_gains;
struct FloatEulers stab_plane_att_sum_err;
static struct FloatEulers stab_att_sp_euler;
float stab_plane_att_cmd[COMMANDS_NB];
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_att(struct transport_tx *trans, struct link_device *dev)
{
struct FloatRates *body_rate = stateGetBodyRates_f();
struct FloatEulers *att = stateGetNedToBodyEulers_f();
float foo = 0.0;
pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stab_plane_att_sum_err.phi,
&stab_plane_att_sum_err.theta,
&stab_plane_att_sum_err.psi,
&stab_plane_att_cmd[COMMAND_ROLL],
&stab_plane_att_cmd[COMMAND_PITCH],
&foo, &foo, &foo, &foo,
&stabilization.cmd[COMMAND_ROLL],
&stabilization.cmd[COMMAND_PITCH],
&stabilization.cmd[COMMAND_YAW],
&foo, &foo, &foo);
}
#endif
void stabilization_attitude_plane_pid_init(void)
{
VECT3_ASSIGN(stab_plane_gains.p,
STABILIZATION_PLANE_PHI_PGAIN,
STABILIZATION_PLANE_THETA_PGAIN,
0.f);
VECT3_ASSIGN(stab_plane_gains.d,
STABILIZATION_PLANE_PHI_DGAIN,
STABILIZATION_PLANE_THETA_DGAIN,
0.f);
VECT3_ASSIGN(stab_plane_gains.i,
STABILIZATION_PLANE_PHI_IGAIN,
STABILIZATION_PLANE_THETA_IGAIN,
0.f);
FLOAT_EULERS_ZERO(stab_plane_att_sum_err);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att);
#endif
}
void stabilization_attitude_plane_pid_enter(void)
{
FLOAT_EULERS_ZERO(stab_plane_att_sum_err);
}
#define MAX_SUM_ERR 200
void stabilization_attitude_plane_pid_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
{
/* Compute feedback */
/* attitude error */
stab_att_sp_euler = stab_sp_to_eulers_f(sp);
struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
struct FloatEulers att_err;
EULERS_DIFF(att_err, stab_att_sp_euler, *att_float);
att_err.psi = 0.f;
/* update integrator */
if (in_flight) {
EULERS_ADD(stab_plane_att_sum_err, att_err);
EULERS_BOUND_CUBE(stab_plane_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
} else {
FLOAT_EULERS_ZERO(stab_plane_att_sum_err);
}
/* rate error */
struct FloatRates *rate_float = stateGetBodyRates_f();
struct FloatRates rate_sp, rate_err;
FLOAT_RATES_ZERO(rate_sp);
RATES_DIFF(rate_err, rate_sp, *rate_float);
/* PID */
stab_plane_att_cmd[COMMAND_ROLL] =
stab_plane_gains.p.x * att_err.phi +
stab_plane_gains.d.x * rate_err.p +
stab_plane_gains.i.x * stab_plane_att_sum_err.phi;
stab_plane_att_cmd[COMMAND_PITCH] =
stab_plane_gains.p.y * att_err.theta +
stab_plane_gains.d.y * rate_err.q +
stab_plane_gains.i.y * stab_plane_att_sum_err.theta;
cmd[COMMAND_ROLL] = (int32_t)stab_plane_att_cmd[COMMAND_ROLL];
cmd[COMMAND_PITCH] = (int32_t)stab_plane_att_cmd[COMMAND_PITCH];
cmd[COMMAND_YAW] = 0;
cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_X);
/* bound the result */
BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
}

45
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_plane_pid.h Normal file
View File

@@ -0,0 +1,45 @@
/*
* Copyright (C) 2024 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 stabilization_attitude_plane_pid.h
*
* Basic fixed-wing attitude stabilization in euler float version.
*/
#ifndef STABILIZATION_ATTITUDE_PLANE_PID_H
#define STABILIZATION_ATTITUDE_PLANE_PID_H
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
struct PlaneAttitudeGains {
struct FloatVect3 p;
struct FloatVect3 i;
struct FloatVect3 d;
};
extern void stabilization_attitude_plane_pid_init(void);
extern void stabilization_attitude_plane_pid_enter(void);
extern void stabilization_attitude_plane_pid_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd);
extern struct PlaneAttitudeGains stab_plane_gains;
extern struct FloatEulers stab_plane_att_sum_err;
#endif /* STABILIZATION_ATTITUDE_PLANE_PID_H */

308
sw/airborne/modules/ctrl/control_mixing_heewing.c Normal file
View File

@@ -0,0 +1,308 @@
/*
* Copyright (C) 2024 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 "modules/ctrl/control_mixing_heewing.c"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Control mixing specific to the Heewing T1 Ranger
*/
#include "modules/ctrl/control_mixing_heewing.h"
#include "modules/radio_control/radio_control.h"
#include "generated/modules.h"
#include "modules/core/commands.h"
#include "modules/actuators/actuators.h"
#include "autopilot.h"
#include "state.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/guidance/guidance_plane.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_plane_pid.h"
// Tilt position in forward flight
#ifndef CMH_TILT_FORWARD
#define CMH_TILT_FORWARD 0
#endif
// Tilt vertical position for hovering
#ifndef CMH_TILT_VERTICAL
#define CMH_TILT_VERTICAL 8700
#endif
// Max tilt differential for yaw
#ifndef CMH_TILT_DIFF_MAX
#define CMH_TILT_DIFF_MAX (MAX_PPRZ-CMH_TILT_VERTICAL)
#endif
// Motor idle position
#ifndef CMH_MOTOR_IDLE
#define CMH_MOTOR_IDLE 800
#endif
// Time for forward transition
#ifndef CMH_TRANSITION_TIME
#define CMH_TRANSITION_TIME 4.f
#endif
// Transition airspeed, set a negative value to disable
#ifndef CMH_TRANSITION_AIRSPEED
#define CMH_TRANSITION_AIRSPEED 10.f
#endif
#define TRANSITION_TO_HOVER false
#define TRANSITION_TO_FORWARD true
static float transition_ratio;
static const float transition_increment = 1.f / (CMH_TRANSITION_TIME * PERIODIC_FREQUENCY);
static void transition_run(bool to_forward) {
if (to_forward && transition_ratio < 1.f) {
if (stateIsAirspeedValid()) {
if ((stateGetAirspeed_f() < CMH_TRANSITION_AIRSPEED && transition_ratio < 0.4f)
|| stateGetAirspeed_f() >= CMH_TRANSITION_AIRSPEED) {
transition_ratio += transition_increment;
}
else if (stateGetAirspeed_f() < CMH_TRANSITION_AIRSPEED && transition_ratio > 0.4f) {
transition_ratio -= transition_increment;
}
}
} else if (!to_forward && transition_ratio > 0.f) {
transition_ratio = 0.f; // immediately switch back to hover
}
Bound(transition_ratio, 0.f, 1.f);
}
static int32_t command_from_transition(int32_t hover_cmd, int32_t forward_cmd) {
Bound(transition_ratio, 0.f, 1.f);
return (int32_t) (hover_cmd + transition_ratio * (forward_cmd - hover_cmd));
}
void control_mixing_heewing_init(void)
{
transition_ratio = 0.f; // 0. for hover to 1. for forward flight
}
void control_mixing_heewing_manual(void)
{
transition_run(TRANSITION_TO_FORWARD);
commands[COMMAND_ROLL] = radio_control_get(RADIO_ROLL);
commands[COMMAND_PITCH] = radio_control_get(RADIO_PITCH);
commands[COMMAND_YAW] = 0;
commands[COMMAND_TILT] = CMH_TILT_FORWARD;
commands[COMMAND_MOTOR_RIGHT] = radio_control_get(RADIO_THROTTLE);
commands[COMMAND_MOTOR_LEFT] = radio_control_get(RADIO_THROTTLE);
commands[COMMAND_MOTOR_TAIL] = MIN_PPRZ;
commands[COMMAND_THRUST] = (commands[COMMAND_MOTOR_RIGHT] + commands[COMMAND_MOTOR_LEFT]) / 2;
autopilot.throttle = commands[COMMAND_THRUST];
}
void control_mixing_heewing_attitude_direct(void)
{
transition_run(TRANSITION_TO_HOVER);
commands[COMMAND_TILT] = CMH_TILT_VERTICAL;
commands[COMMAND_ROLL] = 0;
commands[COMMAND_PITCH] = 0;
struct ThrustSetpoint th_sp = guidance_v_run(autopilot_in_flight());
stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &th_sp, stabilization.cmd);
if (autopilot_get_motors_on()) {
commands[COMMAND_MOTOR_RIGHT] = actuators_pprz[CMH_ACT_MOTOR_RIGHT];
commands[COMMAND_MOTOR_LEFT] = actuators_pprz[CMH_ACT_MOTOR_LEFT];
commands[COMMAND_MOTOR_TAIL] = actuators_pprz[CMH_ACT_MOTOR_TAIL];
if (autopilot_in_flight()) {
commands[COMMAND_YAW] = actuators_pprz[CMH_ACT_YAW];
} else {
commands[CMH_ACT_YAW] = 0;
}
commands[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST];
} else {
commands[COMMAND_MOTOR_RIGHT] = -MAX_PPRZ;
commands[COMMAND_MOTOR_LEFT] = -MAX_PPRZ;
commands[COMMAND_MOTOR_TAIL] = -MAX_PPRZ;
commands[COMMAND_YAW] = 0;
commands[COMMAND_THRUST] = 0;
}
autopilot.throttle = commands[COMMAND_THRUST];
}
void control_mixing_heewing_attitude_direct_enter(void)
{
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
stabilization_mode_changed(STABILIZATION_MODE_NONE, STABILIZATION_ATT_SUBMODE_HEADING); // force mode change to always reset heading
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING);
}
void stabilization_indi_set_wls_settings(void)
{
// Calculate the min and max increments
wls_stab_p.u_min[CMH_ACT_MOTOR_RIGHT] = CMH_MOTOR_IDLE;
wls_stab_p.u_max[CMH_ACT_MOTOR_RIGHT] = MAX_PPRZ;
wls_stab_p.u_pref[CMH_ACT_MOTOR_RIGHT] = act_pref[CMH_ACT_MOTOR_RIGHT];
wls_stab_p.u_min[CMH_ACT_MOTOR_LEFT] = CMH_MOTOR_IDLE;
wls_stab_p.u_max[CMH_ACT_MOTOR_LEFT] = MAX_PPRZ;
wls_stab_p.u_pref[CMH_ACT_MOTOR_LEFT] = act_pref[CMH_ACT_MOTOR_LEFT];
wls_stab_p.u_min[CMH_ACT_MOTOR_TAIL] = CMH_MOTOR_IDLE;
wls_stab_p.u_max[CMH_ACT_MOTOR_TAIL] = MAX_PPRZ;
wls_stab_p.u_pref[CMH_ACT_MOTOR_TAIL] = act_pref[CMH_ACT_MOTOR_TAIL];
wls_stab_p.u_min[CMH_ACT_YAW] = -CMH_TILT_DIFF_MAX;
wls_stab_p.u_max[CMH_ACT_YAW] = CMH_TILT_DIFF_MAX;
wls_stab_p.u_pref[CMH_ACT_YAW] = act_pref[CMH_ACT_YAW];
}
void control_mixing_heewing_attitude_plane_enter(void)
{
// don't use forward submode to avoid pitch offset on RC input
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING);
stabilization_attitude_plane_pid_enter();
}
void control_mixing_heewing_attitude_plane(void)
{
transition_run(TRANSITION_TO_FORWARD);
if (transition_ratio < 0.5f) {
struct ThrustSetpoint hover_th_sp = guidance_v_run(autopilot_in_flight());
stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &hover_th_sp, stabilization.cmd);
}
struct ThrustSetpoint th_sp = th_sp_from_thrust_i(radio_control_get(RADIO_THROTTLE), THRUST_AXIS_X);
struct FloatEulers rc_sp = {
.phi = STABILIZATION_ATTITUDE_SP_MAX_PHI * radio_control_get(RADIO_ROLL) / MAX_PPRZ,
.theta = STABILIZATION_ATTITUDE_SP_MAX_THETA * radio_control_get(RADIO_PITCH) / MAX_PPRZ,
.psi = 0.f
};
struct StabilizationSetpoint stab_sp = stab_sp_from_eulers_f(&rc_sp);
stabilization_attitude_plane_pid_run(transition_ratio < 0.8 ? false : autopilot_in_flight(), &stab_sp, &th_sp, stabilization.cmd);
commands[COMMAND_ROLL] = stabilization.cmd[COMMAND_ROLL];
commands[COMMAND_PITCH] = stabilization.cmd[COMMAND_PITCH];
commands[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST];
if (autopilot_in_flight()) {
if (transition_ratio < 0.5f) {
// only hover stabilization
commands[COMMAND_MOTOR_RIGHT] = actuators_pprz[CMH_ACT_MOTOR_RIGHT];
commands[COMMAND_MOTOR_LEFT] = actuators_pprz[CMH_ACT_MOTOR_LEFT];
commands[COMMAND_MOTOR_TAIL] = actuators_pprz[CMH_ACT_MOTOR_TAIL];
} else {
// blend with plane control
commands[COMMAND_MOTOR_RIGHT] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_RIGHT], stabilization.cmd[COMMAND_THRUST]);
commands[COMMAND_MOTOR_LEFT] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_LEFT], stabilization.cmd[COMMAND_THRUST]);
commands[COMMAND_MOTOR_TAIL] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_TAIL], 0);
}
} else {
commands[COMMAND_MOTOR_RIGHT] = stabilization.cmd[COMMAND_THRUST];
commands[COMMAND_MOTOR_LEFT] = stabilization.cmd[COMMAND_THRUST];
commands[COMMAND_MOTOR_TAIL] = 0;
}
commands[COMMAND_TILT] = command_from_transition(CMH_TILT_VERTICAL, CMH_TILT_FORWARD);
commands[COMMAND_YAW] = 0;
autopilot.throttle = commands[COMMAND_THRUST];
}
void control_mixing_heewing_nav_enter(void)
{
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING);
}
void control_mixing_heewing_nav_run(void)
{
if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ROUTE ||
nav.horizontal_mode == NAV_HORIZONTAL_MODE_CIRCLE) {
transition_run(TRANSITION_TO_FORWARD);
struct ThrustSetpoint th_sp = guidance_plane_thrust_from_nav(transition_ratio < 0.8f ? false : autopilot_in_flight());
if (transition_ratio < 0.8f) {
guidance_plane.pitch_cmd = 0.f;
}
struct StabilizationSetpoint stab_sp = guidance_plane_attitude_from_nav(autopilot_in_flight());
if (transition_ratio < 0.5f) {
stabilization_attitude_plane_pid_run(transition_ratio < 0.8f ? false : autopilot_in_flight(), &stab_sp, &th_sp, stabilization.cmd);
struct ThrustSetpoint hover_th_sp = guidance_v_run(autopilot_in_flight());
struct FloatEulers eulers_sp = { .phi = 0.f , .theta = 0.f, .psi = stateGetNedToBodyEulers_f()->psi };
struct StabilizationSetpoint hover_stab_sp = stab_sp_from_eulers_f(&eulers_sp);
stabilization_run(autopilot_in_flight(), &hover_stab_sp, &hover_th_sp, stabilization.cmd); // will overwrite COMMAND_THRUST
} else {
stabilization_attitude_plane_pid_run(transition_ratio < 0.8f ? false : autopilot_in_flight(), &stab_sp, &th_sp, stabilization.cmd);
}
nav.heading = stateGetNedToBodyEulers_f()->psi; // overwrite nav heading to avoid problems when transition to hover
commands[COMMAND_TILT] = command_from_transition(CMH_TILT_VERTICAL, CMH_TILT_FORWARD);
commands[COMMAND_ROLL] = stabilization.cmd[COMMAND_ROLL];
commands[COMMAND_PITCH] = stabilization.cmd[COMMAND_PITCH];
commands[COMMAND_YAW] = command_from_transition(actuators_pprz[CMH_ACT_YAW], 0);
if (autopilot_get_motors_on()) {
if (transition_ratio < 0.5f) {
// only hover stabilization
commands[COMMAND_MOTOR_RIGHT] = actuators_pprz[CMH_ACT_MOTOR_RIGHT];
commands[COMMAND_MOTOR_LEFT] = actuators_pprz[CMH_ACT_MOTOR_LEFT];
commands[COMMAND_MOTOR_TAIL] = actuators_pprz[CMH_ACT_MOTOR_TAIL];
} else {
// blend with plane control
commands[COMMAND_MOTOR_RIGHT] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_RIGHT], stabilization.cmd[COMMAND_THRUST]);
commands[COMMAND_MOTOR_LEFT] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_LEFT], stabilization.cmd[COMMAND_THRUST]);
commands[COMMAND_MOTOR_TAIL] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_TAIL], 0);
}
commands[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST];
} else {
commands[COMMAND_THRUST] = 0;
commands[COMMAND_MOTOR_RIGHT] = MIN_PPRZ;
commands[COMMAND_MOTOR_LEFT] = MIN_PPRZ;
commands[COMMAND_MOTOR_TAIL] = 0;
}
} else {
// all other nav modes are in rotorcraft flight mode
transition_run(TRANSITION_TO_HOVER);
commands[COMMAND_TILT] = command_from_transition(CMH_TILT_VERTICAL, CMH_TILT_FORWARD);
commands[COMMAND_ROLL] = 0;
commands[COMMAND_PITCH] = 0;
struct ThrustSetpoint th_sp = guidance_v_run(autopilot_in_flight());
struct StabilizationSetpoint stab_sp = guidance_h_run(autopilot_in_flight());
stabilization_run(autopilot_in_flight(), &stab_sp, &th_sp, stabilization.cmd);
if (autopilot_get_motors_on()) {
commands[COMMAND_MOTOR_RIGHT] = actuators_pprz[CMH_ACT_MOTOR_RIGHT];
commands[COMMAND_MOTOR_LEFT] = actuators_pprz[CMH_ACT_MOTOR_LEFT];
commands[COMMAND_MOTOR_TAIL] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_TAIL], 0);
if (autopilot_in_flight()) {
commands[COMMAND_YAW] = actuators_pprz[CMH_ACT_YAW];
} else {
commands[CMH_ACT_YAW] = 0;
}
commands[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST];
} else {
commands[COMMAND_MOTOR_RIGHT] = -MAX_PPRZ;
commands[COMMAND_MOTOR_LEFT] = -MAX_PPRZ;
commands[COMMAND_MOTOR_TAIL] = -MAX_PPRZ;
commands[COMMAND_YAW] = 0;
commands[COMMAND_THRUST] = 0;
}
}
autopilot.throttle = commands[COMMAND_THRUST];
}

50
sw/airborne/modules/ctrl/control_mixing_heewing.h Normal file
View File

@@ -0,0 +1,50 @@
/*
* Copyright (C) 2024 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 "modules/ctrl/control_mixing_heewing.h"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Control mixing specific to the Heewing T1 Ranger
*/
#ifndef CONTROL_MIXING_HEEWING_H
#define CONTROL_MIXING_HEEWING_H
// INDI actuators output indexes
#define CMH_ACT_MOTOR_RIGHT 0
#define CMH_ACT_MOTOR_LEFT 1
#define CMH_ACT_MOTOR_TAIL 2
#define CMH_ACT_YAW 3
extern void control_mixing_heewing_init(void);
/** Direct manual control in plane style flight
*/
extern void control_mixing_heewing_manual(void);
/** Stabilization in attitude direct mode
*/
extern void control_mixing_heewing_attitude_direct(void);
extern void control_mixing_heewing_attitude_direct_enter(void);
extern void control_mixing_heewing_attitude_plane(void);
extern void control_mixing_heewing_attitude_plane_enter(void);
extern void control_mixing_heewing_nav_enter(void);
extern void control_mixing_heewing_nav_run(void);
#endif // CONTROL_MIXING_HEEWING_H