mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Cyfoam, ctrl effectiveness scheduling, JSBsim (#2145)
* Cyfoam, ctrl effectiveness scheduling, JSBsim - Added Cyfoam airframe file - Added control effectiveness scheduling function - More advanced scheduling function should be added in the future - Added simple JSBsim files for Cyclone for a crude simulation * only calculate pseudo inverse when using it
This commit is contained in:
Ewoud Smeur
committed by
Gautier Hattenberger
Gautier Hattenberger
parent
dab1bf2fcf
commit
95e60fe50d
@@ -65,6 +65,17 @@
|
||||
settings_modules="modules/rotorcraft_cam.xml modules/switch_servo.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
|
||||
gui_color="#3cf2d51335a7"
|
||||
/>
|
||||
<aircraft
|
||||
name="CYFOAM"
|
||||
ac_id="6"
|
||||
airframe="airframes/ENAC/cyfoam.xml"
|
||||
radio="radios/T10CG_SBUS.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/guidance_indi.xml modules/stabilization_indi.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/air_data.xml modules/imu_common.xml modules/airspeed_ms45xx_i2c.xml"
|
||||
gui_color="#ffff7f7f0000"
|
||||
/>
|
||||
<aircraft
|
||||
name="FJ2"
|
||||
ac_id="8"
|
||||
|
||||
@@ -0,0 +1,305 @@
|
||||
<!--Foam Cyclone Airframe
|
||||
Chimera AP
|
||||
Xbee API
|
||||
Ublox M8N
|
||||
SBUS Futaba -->
|
||||
|
||||
<airframe name="foam cylone">
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="chimera_1.0">
|
||||
<module name="radio_control" type="sbus">
|
||||
<!-- Put the mode on channel AUX1-->
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
|
||||
</module>
|
||||
|
||||
<module name="ctrl_effectiveness_scheduling">
|
||||
<define name="SQUARED_ROLL_EFF" value="0.0018"/>
|
||||
<define name="PITCH_EFF_AT_60" value="4.0"/>
|
||||
<define name="YAW_EFF_AT_60" value="8.0"/>
|
||||
<!--function of the form: A + B*airspeed^2-->
|
||||
<define name="CE_PITCH_A" value="2.4169"/>
|
||||
<define name="CE_PITCH_B" value="0.0307"/>
|
||||
<define name="CE_YAW_A" value="5.631"/>
|
||||
<define name="CE_YAW_B" value="0.0515"/>
|
||||
</module>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<module name="udp"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="logger_file">
|
||||
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
|
||||
</module>
|
||||
</target>
|
||||
|
||||
<module name="actuators" type="pwm">
|
||||
<define name="SERVO_HZ" value="400"/>
|
||||
<!--define name="USE_SERVOS_7AND8"/-->
|
||||
</module>
|
||||
|
||||
<module name="mag" type="hmc58xx">
|
||||
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
|
||||
<define name="MODULE_HMC58XX_UPDATE_AHRS"/>
|
||||
|
||||
<define name="HMC58XX_CHAN_X" value="1"/>
|
||||
<define name="HMC58XX_CHAN_Y" value="0"/>
|
||||
<define name="HMC58XX_CHAN_Z" value="2"/>
|
||||
|
||||
<define name="HMC58XX_CHAN_X_SIGN" value="-"/>
|
||||
<define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
|
||||
<define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
|
||||
|
||||
<!--run the chimera mag once every 64 seconds to avoid message spam-->
|
||||
<define name="MPU9250_MAG_PRESCALER" value="32768"/>
|
||||
</module>
|
||||
|
||||
<module name="send_imu_mag_current">
|
||||
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000"/>
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="30000"/>
|
||||
<define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.5"/>
|
||||
</module>
|
||||
<module name="sys_mon"/>
|
||||
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
<module name="imu" type="chimera">
|
||||
<define name="IMU_MPU9250_GYRO_RANGE" value="MPU9250_GYRO_RANGE_2000" />
|
||||
<define name="IMU_MPU9250_ACCEL_RANGE" value="MPU9250_ACCEL_RANGE_16G" />
|
||||
</module>
|
||||
|
||||
<!--<module name="servo_tester"/>-->
|
||||
|
||||
<module name="air_data">
|
||||
<define name="USE_AIRSPEED_AIR_DATA" value="FALSE"/>
|
||||
</module>
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B115200"/>
|
||||
</module>
|
||||
<module name="stabilization" type="indi"/>
|
||||
<module name="guidance" type="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="1.0"/>
|
||||
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.5"/>
|
||||
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.8"/>
|
||||
<define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
|
||||
<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="4000"/>
|
||||
<define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1500"/>
|
||||
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>
|
||||
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
|
||||
<!--<define name="KNIFE_EDGE_TEST" value="TRUE"/>-->
|
||||
<!--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"/>
|
||||
</module>
|
||||
|
||||
<!--<module name="ahrs" type="int_cmpl_quat">-->
|
||||
<!--<configure name="USE_MAGNETOMETER" value="TRUE"/>-->
|
||||
<!--[>Use external magnetometer<]-->
|
||||
<!--<define name="AHRS_ICQ_MAG_ID" value="MAG_HMC58XX_SENDER_ID"/>-->
|
||||
<!--[><define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" value="TRUE"/><]-->
|
||||
<!--[><define name="AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION" value="TRUE"/><]-->
|
||||
<!--</module>-->
|
||||
|
||||
<!--<module name="ins"/>-->
|
||||
<module name="ins" type="float_invariant">
|
||||
<define name="INS_PROPAGATE_FREQUENCY" value="500"/>
|
||||
<define name="INS_FINV_MAG_ID" value="MAG_HMC58XX_SENDER_ID"/>
|
||||
</module>
|
||||
|
||||
<!--<module name="gps" type="ubx_ucenter"/>-->
|
||||
<module name="tlsf"/>
|
||||
<module name="pprzlog">
|
||||
<define name="SDLOG_START_DELAY" value="10"/>
|
||||
</module>
|
||||
<module name="logger" type="sd_chibios"/>
|
||||
<!--<module name="flight_recorder"/>-->
|
||||
<!--<module name="logger_spi_link"/>-->
|
||||
|
||||
<!--Use an airspeed sensor and get the measured airspeed in the messages-->
|
||||
<module name="airspeed" type="ms45xx_i2c">
|
||||
<define name="MS45XX_I2C_DEV" value="i2c1"/>
|
||||
<define name="MS45XX_AIRSPEED_SCALE" value="2.4"/>
|
||||
<define name="USE_AIRSPEED_MS45XX" value="TRUE"/>
|
||||
</module>
|
||||
|
||||
<!--<module name="pwm_meas"/>-->
|
||||
<!--<module name="AOA_pwm">-->
|
||||
<!--<define name="AOA_PWM_CHANNEL" value="PWM_INPUT1"/>-->
|
||||
<!--</module>-->
|
||||
|
||||
<module name="pwm_meas">
|
||||
<define name="USE_PWM_INPUT1" value="PWM_PULSE_TYPE_ACTIVE_LOW"/>
|
||||
<define name="USE_PWM_INPUT2" value="PWM_PULSE_TYPE_ACTIVE_LOW"/>
|
||||
</module>
|
||||
|
||||
</firmware>
|
||||
|
||||
<servos driver="Pwm">
|
||||
<servo name="ELEVON_LEFT" no="0" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="ELEVON_RIGHT" no="1" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="RM" no="2" min="1000" neutral="1100" max="2000"/>
|
||||
<servo name="LM" no="3" min="1000" neutral="1100" max="2000"/>
|
||||
<!--<servo name="SERVO_TEST" no="4" min="1000" neutral="1500" max="2000"/>-->
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<command_laws>
|
||||
<set servo="ELEVON_LEFT" value="autopilot.motors_on ? actuators_pprz[0] : 0"/>
|
||||
<set servo="ELEVON_RIGHT" value="autopilot.motors_on ? actuators_pprz[1] : 0"/>
|
||||
<set servo="RM" value="autopilot.motors_on ? actuators_pprz[2] : -MAX_PPRZ"/>
|
||||
<set servo="LM" value="autopilot.motors_on ? actuators_pprz[3] : -MAX_PPRZ"/>
|
||||
<!--<set servo="RM" value="autopilot_motors_on ? -MAX_PPRZ : -MAX_PPRZ"/>-->
|
||||
<!--<set servo="LM" value="autopilot_motors_on ? -MAX_PPRZ : -MAX_PPRZ"/>-->
|
||||
</command_laws>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- IMU mag calibration, make sure to calibrate the IMU properly before flight, see the wiki for more info-->
|
||||
<!--Mag of gps-->
|
||||
<define name="MAG_X_NEUTRAL" value="195"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="-47"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="-25"/>
|
||||
<define name="MAG_X_SENS" value="3.68245686199" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="3.81802043484" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="4.01084591146" integer="16"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="180." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="LOW_BAT_LEVEL" value="10.5" units="V"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="CTRL_EFF_SCHEDULING" prefix="FWD_">
|
||||
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
|
||||
<define name="G1_ROLL" value="{ 0, 0, -13.0, 13.0}"/>
|
||||
<define name="G1_PITCH" value="{-12.0, 12.0, 0, 0}"/>
|
||||
<define name="G1_YAW" value="{-20.0, -20.0, 0.0, 0.0}"/>
|
||||
<define name="G1_THRUST" value="{ 0, 0, -0.9, -0.9}"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="60." unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="60." 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_">
|
||||
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
|
||||
<define name="G1_ROLL" value="{ 0, 0, -13.3, 13.3}"/>
|
||||
<define name="G1_PITCH" value="{-2.1, 2.1, 0, 0}"/>
|
||||
<define name="G1_YAW" value="{-2.0, -2.0, 0.0, 0.0}"/>
|
||||
<define name="G1_THRUST" value="{ 0, 0, -1.1, -1.1}"/>
|
||||
<!--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="107.0"/>
|
||||
<define name="REF_ERR_Q" value="200.0"/>
|
||||
<define name="REF_ERR_R" value="200.0"/>
|
||||
<define name="REF_RATE_P" value="14.0"/>
|
||||
<define name="REF_RATE_Q" value="15.0"/>
|
||||
<define name="REF_RATE_R" value="15.0"/>
|
||||
|
||||
<!--Maxium yaw rate, to avoid instability-->
|
||||
<define name="MAX_R" value="100.0" unit="deg/s"/>
|
||||
|
||||
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
|
||||
<define name="FILT_CUTOFF" value="5.0"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN" value="{0.1, 0.1, 0.045, 0.045}"/>
|
||||
<define name="ACT_RATE_LIMIT" value="{170, 170, 9600, 9600}"/>
|
||||
<define name="ACT_IS_SERVO" value="{1, 1, 0, 0}"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<!-- Gains for vertical navigation -->
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="200"/>
|
||||
<define name="HOVER_KD" value="175"/>
|
||||
<define name="HOVER_KI" value="72"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value ="0.4"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
||||
</section>
|
||||
|
||||
<section name="NAV">
|
||||
<define name="NAV_CLIMB_VSPEED" value="1.5"/>
|
||||
<define name="NAV_DESCEND_VSPEED" value="-0.8"/>
|
||||
</section>
|
||||
|
||||
<!--<section name="AHRS" prefix="AHRS_">-->
|
||||
<!--<define name="H_X" value="0.5138"/>-->
|
||||
<!--<define name="H_Y" value="0.00019"/>-->
|
||||
<!--<define name="H_Z" value="0.8578"/>-->
|
||||
<!--<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>-->
|
||||
<!--</section>-->
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="H_X" value="0.5138"/>
|
||||
<define name="H_Y" value="0.00019"/>
|
||||
<define name="H_Z" value="0.8578"/>
|
||||
</section>
|
||||
|
||||
<!-- Gains for horizontal navigation-->
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="PGAIN" value="100"/>
|
||||
<define name="DGAIN" value="100"/>
|
||||
<define name="IGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<!--The Quadshot uses (when TRUE) a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
|
||||
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
|
||||
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
|
||||
<define name="TRANSITION_MAX_OFFSET" value="-75.0" unit="deg"/>
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
<define name="COORDINATED_TURN_AIRSPEED" value="18.0"/>
|
||||
|
||||
<define name="BARO_PERIODIC_FREQUENCY" value="50"/>
|
||||
<define name="GUIDANCE_H_MAX_BANK" value="60" unit="deg"/>
|
||||
|
||||
<define name="FWD_SIDESLIP_GAIN" value="0.32"/>
|
||||
|
||||
<define name="EFF_SCHED_USE_FUNCTION" value="TRUE"/>
|
||||
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
|
||||
<define name="USE_AIRSPEED" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="ele_left, ele_right, mot_right, mot_left" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="cyclone" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
<define name="NO_MOTOR_MIXING" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,22 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="ctrl_effectiveness_scheduling" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Interpolation of control effectivenss matrix.
|
||||
This is necessary if the vehicle has different operating points,
|
||||
with significantly different control effectivenss.
|
||||
|
||||
If instead using online adaptation is an option, be sure to
|
||||
not use this module at the same time!
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="ctrl_effectiveness_scheduling.h"/>
|
||||
</header>
|
||||
<init fun="ctrl_eff_scheduling_init()"/>
|
||||
<periodic fun="ctrl_eff_scheduling_periodic()" freq="20"/>
|
||||
<makefile>
|
||||
<file name="ctrl_effectiveness_scheduling.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,94 @@
|
||||
<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.87 -0.35
|
||||
-1.57 0.0
|
||||
-1.27 0.35
|
||||
-0.80 0.25
|
||||
0.00 0.0
|
||||
0.80 0.2
|
||||
1.27 0.2
|
||||
1.57 0.0
|
||||
1.87 -0.2
|
||||
</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>
|
||||
<table>
|
||||
<independentVar lookup="row">aero/alpha-rad</independentVar>
|
||||
<tableData>
|
||||
-1.57 0.4
|
||||
-1.27 0.6
|
||||
0.00 1.500
|
||||
1.27 0.6
|
||||
1.57 0.4
|
||||
</tableData>
|
||||
</table>
|
||||
</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.8</value>
|
||||
</product>
|
||||
</function>
|
||||
|
||||
<!--<function name="aero/force/Drag_beta">-->
|
||||
<!--<description>Drag due to sideslip</description>-->
|
||||
<!--<product>-->
|
||||
<!--<property>aero/qbar-psf</property>-->
|
||||
<!--<property>metrics/Sw-sqft</property>-->
|
||||
<!--<table>-->
|
||||
<!--<independentVar lookup="row">aero/beta-rad</independentVar>-->
|
||||
<!--<tableData>-->
|
||||
<!---1.57 1.230-->
|
||||
<!---0.26 0.050-->
|
||||
<!--0.00 0.000-->
|
||||
<!--0.26 0.050-->
|
||||
<!--1.57 1.230-->
|
||||
<!--</tableData>-->
|
||||
<!--</table>-->
|
||||
<!--</product>-->
|
||||
<!--</function>-->
|
||||
|
||||
</axis>
|
||||
|
||||
<axis name="SIDE">
|
||||
|
||||
<!--Destabalizing side force to reduce the sidewards drag a bit-->
|
||||
<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.02</value>
|
||||
</product>
|
||||
</function>
|
||||
|
||||
</axis>
|
||||
|
||||
</aerodynamics>
|
||||
@@ -0,0 +1,384 @@
|
||||
<?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>Ewoud Smeur</author>
|
||||
<filecreationdate>07-03-2017</filecreationdate>
|
||||
<version>Version 0.9 - beta</version>
|
||||
<description>Cyclone</description>
|
||||
</fileheader>
|
||||
|
||||
<metrics>
|
||||
<wingarea unit="IN2"> 115 </wingarea>
|
||||
<wingspan unit="IN"> 35.4 </wingspan>
|
||||
<chord unit="IN"> 7.09 </chord>
|
||||
<htailarea unit="FT2"> 0 </htailarea>
|
||||
<htailarm unit="FT"> 0 </htailarm>
|
||||
<vtailarea unit="FT2"> 0 </vtailarea>
|
||||
<vtailarm unit="FT"> 0 </vtailarm>
|
||||
<wing_incidence unit="DEG"> 90 </wing_incidence>
|
||||
<location name="AERORP" unit="IN">
|
||||
<x> 0 </x>
|
||||
<y> 0 </y>
|
||||
<z> 0 </z>
|
||||
</location>
|
||||
<location name="EYEPOINT" unit="IN">
|
||||
<x> 0 </x>
|
||||
<y> 0 </y>
|
||||
<z> 0 </z>
|
||||
</location>
|
||||
<location name="VRP" unit="IN">
|
||||
<x> 0 </x>
|
||||
<y> 0 </y>
|
||||
<z> 0 </z>
|
||||
</location>
|
||||
</metrics>
|
||||
|
||||
<mass_balance>
|
||||
<ixx unit="SLUG*FT2"> 0.0132 </ixx>
|
||||
<iyy unit="SLUG*FT2"> 0.00250 </iyy>
|
||||
<izz unit="SLUG*FT2"> 0.0150 </izz>
|
||||
<ixy unit="SLUG*FT2"> 0. </ixy>
|
||||
<ixz unit="SLUG*FT2"> 0. </ixz>
|
||||
<iyz unit="SLUG*FT2"> 0. </iyz>
|
||||
<emptywt unit="LBS"> 2.4 </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.15 </x>
|
||||
<y> 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.15</x>
|
||||
<y> 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. </x>
|
||||
<y> 0.15</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. </x>
|
||||
<y>-0.15</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">
|
||||
<channel name="filtering">
|
||||
|
||||
<!--First order filter represents actuator dynamics-->
|
||||
<lag_filter name="ele_left_lag">
|
||||
<input> fcs/ele_left </input>
|
||||
<c1> 54 </c1>
|
||||
<output> fcs/ele_left_lag</output>
|
||||
</lag_filter>
|
||||
<lag_filter name="ele_right_lag">
|
||||
<input> fcs/ele_right </input>
|
||||
<c1> 54 </c1>
|
||||
<output> fcs/ele_right_lag</output>
|
||||
</lag_filter>
|
||||
<lag_filter name="mot_right_lag">
|
||||
<input> fcs/mot_right </input>
|
||||
<c1> 31.7 </c1>
|
||||
<output> fcs/mot_right_lag</output>
|
||||
</lag_filter>
|
||||
<lag_filter name="mot_left_lag">
|
||||
<input> fcs/mot_left </input>
|
||||
<c1> 31.7 </c1>
|
||||
<output> fcs/mot_left_lag</output>
|
||||
</lag_filter>
|
||||
</channel>
|
||||
</flight_control>
|
||||
|
||||
<external_reactions>
|
||||
|
||||
<property>fcs/ele_left</property>
|
||||
<property>fcs/ele_left_lag</property>
|
||||
<property>fcs/ele_right</property>
|
||||
<property>fcs/ele_right_lag</property>
|
||||
<property>fcs/mot_right</property>
|
||||
<property>fcs/mot_right_lag</property>
|
||||
<property>fcs/mot_left</property>
|
||||
<property>fcs/mot_left_lag</property>
|
||||
|
||||
|
||||
<!-- First the lift forces produced by each propeller -->
|
||||
|
||||
<force name="mot_right" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/mot_right_lag</property>
|
||||
<value> 2.2 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<x>0</x>
|
||||
<y>9.4</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="mot_left" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/mot_left_lag</property>
|
||||
<value> 2.2 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<x>0</x>
|
||||
<y>-9.4</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<!-- Then the Moment Couples -->
|
||||
|
||||
<!--Pitch moments-->
|
||||
|
||||
<force name="ele_left_pitch1" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/ele_left_lag</property>
|
||||
<value> -0.5 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<!-- Necessary arm in IN to produce a moment ten times
|
||||
"weaker" then the force when both are measured in the SI is 1.9685 in. -->
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>1</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>-1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="ele_left_pitch2" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/ele_left_lag</property>
|
||||
<value> -0.5 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="ele_right_pitch1" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/ele_right_lag</property>
|
||||
<value> 0.5 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<!-- Necessary arm in IN to produce a moment ten times
|
||||
"weaker" then the force when both are measured in the SI is 1.9685 in. -->
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>1</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>-1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="ele_right_pitch2" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/ele_right_lag</property>
|
||||
<value> 0.5 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<!--Yaw moments-->
|
||||
|
||||
<force name="ele_left_yaw1" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/ele_left_lag</property>
|
||||
<value> 4.2 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<!-- Necessary arm in IN to produce a moment ten times
|
||||
"weaker" then the force when both are measured in the SI is 1.9685 in. -->
|
||||
<x>0</x>
|
||||
<y>1</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="ele_left_yaw2" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/ele_left_lag</property>
|
||||
<value> 4.2 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>-1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="ele_right_yaw1" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/ele_right_lag</property>
|
||||
<value> 4.2 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<!-- Necessary arm in IN to produce a moment ten times
|
||||
"weaker" then the force when both are measured in the SI is 1.9685 in. -->
|
||||
<x>0</x>
|
||||
<y>1</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="ele_right_yaw2" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/ele_right_lag</property>
|
||||
<value> 4.2 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="IN">
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>-1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
</external_reactions>
|
||||
|
||||
<propulsion/>
|
||||
|
||||
<flight_control name="FGFCS"/>
|
||||
|
||||
<!--<aerodynamics>-->
|
||||
<!--<axis name="DRAG">-->
|
||||
<!--<function name="aero/coefficient/CD">-->
|
||||
<!--<description>Drag</description>-->
|
||||
<!--<product>-->
|
||||
<!--<property>aero/qbar-psf</property>-->
|
||||
<!--<value>47.9</value> [> Conversion to pascals <]-->
|
||||
<!--<value>0.0151</value> [> CD x Area (m^2) <]-->
|
||||
<!--<value>0.224808943</value> [> N to LBS <]-->
|
||||
<!--</product>-->
|
||||
<!--</function>-->
|
||||
<!--</axis>-->
|
||||
<!--</aerodynamics>-->
|
||||
<aerodynamics file="Systems/aerodynamics_cyfoam.xml"/>
|
||||
|
||||
</fdm_config>
|
||||
@@ -45,14 +45,8 @@
|
||||
#include "wls/wls_alloc.h"
|
||||
#include <stdio.h>
|
||||
|
||||
//only 4 actuators supported for now
|
||||
#define INDI_NUM_ACT 4
|
||||
// outputs: roll, pitch, yaw, thrust
|
||||
#define INDI_OUTPUTS 4
|
||||
// Factor that the estimated G matrix is allowed to deviate from initial one
|
||||
#define INDI_ALLOWED_G_FACTOR 2.0
|
||||
// Scaling for the control effectiveness to make it readible
|
||||
#define INDI_G_SCALING 1000.0
|
||||
|
||||
float du_min[INDI_NUM_ACT];
|
||||
float du_max[INDI_NUM_ACT];
|
||||
@@ -660,8 +654,10 @@ void lms_estimation(void)
|
||||
float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
|
||||
float_vect_copy(g2, g2_est, INDI_NUM_ACT);
|
||||
|
||||
#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
|
||||
// Calculate the inverse of (G1+G2)
|
||||
calc_g1g2_pseudo_inv();
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -26,8 +26,16 @@
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
|
||||
|
||||
//only 4 actuators supported for now
|
||||
#define INDI_NUM_ACT 4
|
||||
// outputs: roll, pitch, yaw, thrust
|
||||
#define INDI_OUTPUTS 4
|
||||
// Scaling for the control effectiveness to make it readible
|
||||
#define INDI_G_SCALING 1000.0
|
||||
|
||||
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
|
||||
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
|
||||
extern float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
|
||||
|
||||
extern bool indi_use_adaptive;
|
||||
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Ewoud Smeur <ewoud_smeur@msn.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file modules/ctrl/ctrl_effectiveness_scheduling.c
|
||||
* Module that interpolates gainsets in flight based on the transition percentage
|
||||
*/
|
||||
|
||||
#include "modules/ctrl/ctrl_effectiveness_scheduling.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "state.h"
|
||||
#include "subsystems/radio_control.h"
|
||||
|
||||
#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
|
||||
#error "You need to use WLS control allocation for this module"
|
||||
#endif
|
||||
|
||||
|
||||
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {FWD_G1_ROLL,
|
||||
FWD_G1_PITCH, FWD_G1_YAW, FWD_G1_THRUST
|
||||
};
|
||||
|
||||
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
|
||||
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
|
||||
};
|
||||
|
||||
static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
|
||||
|
||||
//Get the specified gains in the gainlibrary
|
||||
void ctrl_eff_scheduling_init(void)
|
||||
{
|
||||
//sum of G1 and G2
|
||||
int8_t i;
|
||||
int8_t j;
|
||||
for (i = 0; i < INDI_OUTPUTS; i++) {
|
||||
for (j = 0; j < INDI_NUM_ACT; j++) {
|
||||
if (i != 2) {
|
||||
g1g2_hover[i][j] = g1g2_hover[i][j] / INDI_G_SCALING;
|
||||
g1g2_forward[i][j] = g1g2_forward[i][j] / INDI_G_SCALING;
|
||||
} else {
|
||||
g1g2_forward[i][j] = (g1g2_forward[i][j] + g2_both[j]) / INDI_G_SCALING;
|
||||
g1g2_hover[i][j] = (g1g2_hover[i][j] + g2_both[j]) / INDI_G_SCALING;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ctrl_eff_scheduling_periodic(void)
|
||||
{
|
||||
// Go from transition percentage to ratio
|
||||
float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;
|
||||
|
||||
int8_t i;
|
||||
int8_t j;
|
||||
for (i = 0; i < INDI_OUTPUTS; i++) {
|
||||
for (j = 0; j < INDI_NUM_ACT; j++) {
|
||||
g1g2[i][j] = (g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Ewoud Smeur <ewoud_smeur@msn.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ctrl/ctrl_effectiveness_scheduling.h
|
||||
*/
|
||||
|
||||
#ifndef CTRL_EFFECTIVENESS_SCHEDULING_H
|
||||
#define CTRL_EFFECTIVENESS_SCHEDULING_H
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
|
||||
/**
|
||||
* Initialises periodic loop;
|
||||
*/
|
||||
extern void ctrl_eff_scheduling_init(void);
|
||||
|
||||
/**
|
||||
* Periodic function that interpolates between gain sets depending on the scheduling variable.
|
||||
*/
|
||||
extern void ctrl_eff_scheduling_periodic(void);
|
||||
|
||||
#endif /* CTRL_EFFECTIVENESS_SCHEDULING_H */
|
||||
|
||||
Reference in New Issue
Block a user