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

[airframes] RW25kg (#3133)

Browse Source 
* [airframe] RW25kg
This commit is contained in:
Christophe De Wagter
2023-10-09 08:08:32 +02:00
committed by GitHub
parent 650509808d
commit cb97031d47
8 changed files with 621 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_25kg.xml
+431
View File
@@ -0,0 +1,431 @@
<!-- This is a Rotating Wing 25kg
* Airframe: TUD00???
* Autopilot: Cube Orange Plus
* Datalink: Herelink
* GPS: UBlox F9P (2x)
* RC: SBUS Crossfire (diversity)
-->
<airframe name="RotatingWing25Kg">
<description>RotatingWing25Kg</description>
<firmware name="rotorcraft">
<target name="ap" board="cube_orangeplus">
<configure name="PERIODIC_FREQUENCY" value="500"/> <!-- Configure the main periodic frequency to 500Hz -->
<module name="radio_control" type="sbus">
<configure name="SBUS_PORT" value="UART3"/> <!-- On the TELEM2 port -->
</module>
<module name="sys_mon"/>
<module name="flight_recorder"/>
<!-- RC switches -->
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_FMODE" value="RADIO_AUX2"/>
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/>
<!-- EKF2 configure inputs -->
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<!-- Disable current sensing using UAVCAN -->
<define name="UAVCAN_ACTUATORS_USE_CURRENT" value="FALSE"/>
<define name="USE_I2C1"/>
</target>
<target name="nps" board="pc">
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>
<!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
</target>
<!-- Herelink datalink -->
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B460800"/>
</module>
<!-- Sensors -->
<module name="mag" type="rm3100">
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_RM3100_I2C_DEV" value="I2C2"/>
</module>
<module name="airspeed" type="ms45xx_i2c">
<define name="MS45XX_I2C_DEV" value="i2c1"/>
<define name="MS45XX_AIRSPEED_SCALE" value="2.0833"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
</module>
<module name="airspeed" type="uavcan"/>
<module name="air_data"/>
<module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>
<!-- IMU / INS -->
<module name="imu" type="cube"/>
<module name="ins" type="ekf2">
<!-- The Cube is mounted 25cm backwards of the CG -->
<define name="INS_EKF2_IMU_POS_X" value="-0.25"/>
<define name="INS_EKF2_IMU_POS_Y" value="0.0"/>
<define name="INS_EKF2_IMU_POS_Z" value="0.0"/>
<!-- The main GPS is mounted at the tail 1.44m backwards and 20cm above the CG -->
<define name="INS_EKF2_GPS_POS_X" value="-1.44"/>
<define name="INS_EKF2_GPS_POS_Y" value="0.0"/>
<define name="INS_EKF2_GPS_POS_Z" value="0.2"/>
</module>
<!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan">
<configure value="TRUE" name="UAVCAN_USE_CAN1"/>
<configure value="TRUE" name="UAVCAN_USE_CAN2"/>
</module>
<!-- Control -->
<module name="stabilization" type="indi">
<configure name="INDI_NUM_ACT" value="6"/>
<define name="WLS_N_U" value="9"/>
<define name="WLS_N_V" value="5"/>
</module>
<module name="ctrl_eff_sched_rot_wing"/>
<module name="guidance" type="indi_hybrid"/>
<module name="nav_hybrid"/>
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
</firmware>
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="-8191" neutral="-5000" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="-8191" neutral="-5000" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="-8191" neutral="-5000" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="-8191" neutral="-5000" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="-8191" neutral="-8191" max="8191"/>
<servo no="5" name="SERVO_ELEVATOR" min="-8191" neutral="-8191" max="4900"/>
<servo no="6" name="SERVO_RUDDER" min="-8191" neutral="0" max="8191"/>
<servo no="7" name="ROTATION_MECH" min="-8191" neutral="0" max="8191"/>
<servo no="8" name="AIL_RIGHT" min="-8191" neutral="0" max="8191"/>
<servo no="9" name="FLAP_RIGHT" min="-8191" neutral="0" max="8191"/>
<servo no="10" name="AIL_LEFT" min="-8191" neutral="0" max="8191"/>
<servo no="11" name="FLAP_LEFT" min="-8191" neutral="0" max="8191"/>
<servo no="12" name="PARACHUTE" min="-8191" neutral="0" max="8191"/>
</servos>
<!-- Can bus 2 actuators -->
<servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="-8191" neutral="-5000" max="8191"/>
<servo no="1" name="BMOTOR_RIGHT" min="-8191" neutral="-5000" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="-8191" neutral="-5000" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="-8191" neutral="-5000" max="8191"/>
<servo no="4" name="BMOTOR_PUSH" min="-8191" neutral="-8191" max="8191"/>
<servo no="5" name="BSERVO_ELEVATOR" min="-8191" neutral="-8191" max="4900"/>
<servo no="6" name="BSERVO_RUDDER" min="-8191" neutral="0" max="8191"/>
<servo no="7" name="BROTATION_MECH" min="-8191" neutral="0" max="8191"/>
<servo no="8" name="BAIL_RIGHT" min="-8191" neutral="0" max="8191"/>
<servo no="9" name="BFLAP_RIGHT" min="-8191" neutral="0" max="8191"/>
<servo no="10" name="BAIL_LEFT" min="-8191" neutral="0" max="8191"/>
<servo no="11" name="BFLAP_LEFT" min="-8191" neutral="0" max="8191"/>
<servo no="12" name="BPARACHUTE" min="-8191" neutral="0" max="8191"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
<axis name="THRUST_X" failsafe_value="0"/>
</commands>
<auto_rc_commands>
<set VALUE="@AUX4" COMMAND="THRUST_X"/>
</auto_rc_commands>
<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<!-- Main bus -->
<set servo="MOTOR_FRONT" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_RIGHT" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_BACK" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_LEFT" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_PUSH" value="($th_hold? -9600 : actuators_pprz[6])"/>
<set servo="SERVO_ELEVATOR" value="0"/>
<set servo="SERVO_RUDDER" value="0"/>
<set servo="ROTATION_MECH" value="rotation_cmd"/>
<set servo="AIL_RIGHT" value="0"/>
<set servo="FLAP_RIGHT" value="0"/>
<set servo="AIL_LEFT" value="0"/>
<set servo="FLAP_LEFT" value="0"/>
<set servo="PARACHUTE" value="RadioControlValues(RADIO_AUX6)"/>
<!-- Second bus -->
<set servo="BMOTOR_FRONT" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="BMOTOR_RIGHT" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="BMOTOR_BACK" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="BMOTOR_LEFT" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="BMOTOR_PUSH" value="($th_hold? -9600 : actuators_pprz[6])"/>
<set servo="BSERVO_ELEVATOR" value="0"/>
<set servo="BSERVO_RUDDER" value="0"/>
<set servo="BROTATION_MECH" value="rotation_cmd"/>
<set servo="BAIL_RIGHT" value="0"/>
<set servo="BFLAP_RIGHT" value="0"/>
<set servo="BAIL_LEFT" value="0"/>
<set servo="BFLAP_LEFT" value="0"/>
<set servo="BPARACHUTE" value="RadioControlValues(RADIO_AUX6)"/>
</command_laws>
<section PREFIX="SYS_ID_" NAME="SYS_ID">
<define name="DOUBLET_AXES" value="{0,1,2,3,4,5,6}"/>
<define name="DOUBLET_RADIO_CHANNEL" value="6"/>
<define name="AUTO_DOUBLETS_N_ACTUATORS" value="4"/>
<define name="AUTO_DOUBLETS_ACTUATORS" value="{0,1,2,3}"/>
<define name="AUTO_DOUBLETS_AMPLITUDE" value="{1500,1500,1500,1500}"/>
<define name="CHIRP_AXES" value="{0,1,2,3}"/>
<define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section>
<!-- 20.17 kg -->
<!-- <section name="CTRL_EFF_SCED" prefix="ROT_WING_EFF_SCHED_">
<define name="IXX_BODY" value="0.90940"/>
<define name="IYY_BODY" value="7.88418"/>
<define name="IZZ" value="5.91445"/>
<define name="IWING" value="1.06965"/>
<define name="M" value="20.17"/>
<define name="ROLL_ARM" value="0.64"/>
<define name="PITCH_ARM" value="0.556"/>
<define name="HOVER_DF_DPPRZ" value="0.012078"/>
</section> -->
<!-- 25kg -->
<section name="CTRL_EFF_SCED" prefix="ROT_WING_EFF_SCHED_">
<!-- Control Effectiveness Scheduling -->
<define name="IXX_BODY" value="0.2845"/>
<define name="IYY_BODY" value="5.078"/>
<define name="IZZ" value="7.051"/>
<define name="IXX_WING" value="0.712"/>
<define name="IYY_WING" value="1.77"/>
<define name="M" value="25.5"/>
<define name="ROLL_ARM" value="0.64"/>
<define name="PITCH_ARM" value="0.556"/>
<define name="HOVER_DF_DPPRZ" value="0.014007"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.64,1.4}"/>
</section>
<section name="MISC">
<!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 21.314 * adc)"/> <!-- ADC calibration -->
<define name="MilliAmpereOfAdc(adc)" value="((3.3f/65536.0f) * 50000.f * adc)"/> <!-- Has like a -2A offset -->
<define name="MilliAmpereOfAdc2(adc)" value="((3.3f/65536.0f) * 50000.f * adc)"/> <!-- Seems te be ok -->
<define name="ADC_CHANNEL_CURRENT2" value="ADC_4"/>
<!-- Others -->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="150"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
<define name="USE_AIRSPEED" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true},.neutral={-3,3,13}, .scale={{15146,60067,2567},{1551,6124,263}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-26,9,2}, .scale={{32863,18346,55169},{6721,3745,11294}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-96,-292,-95}, .scale={{33136,8764,8709},{59233,15761,15343}}}}"/> <!-- Calibrated 't Harde 17-08-2023 -->
<!-- Define axis in hover frame -->
<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="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- Limits -->
<define name="SP_MAX_PHI" value="45." unit="deg" />
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
<!-- Reference model -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- Gains -->
<define name="PHI_PGAIN" value="500"/>
<define name="PHI_DGAIN" value="230"/>
<define name="PHI_IGAIN" value="10"/>
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_PGAIN" value="500"/>
<define name="THETA_DGAIN" value="230"/>
<define name="THETA_IGAIN" value="10"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_PGAIN" value="700"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<define name="PSI_DDGAIN" value="0"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- G1 and G2 20.17 kg-->
<!-- <define name="G1_ROLL" value="{ 0.0, -8.5, 0.0, 8.5, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 0.75, 0.0, -0.75, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{ -0.25, 0.25, -0.25, 0.25, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/> -->
<!-- G1 and G2 25 kg-->
<define name="G1_ROLL" value="{ 0.0, -9.0, 0.0, 9.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.14, 0.0, -1.14, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{ -0.29, 0.29, -0.29, 0.29, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{ -0.54, -0.54, -0.54, -0.54, 0.0, 0.0}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_DYN" value="{0.024, 0.024, 0.024, 0.024, 0.1, 0.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1}"/>
<!-- Pusher prop -->
<define name="PUSHER_PROP_EFFECTIVENESS" value="0.0009"/>
<define name="PUSHER_PROP_DYN" value="0.047"/>
<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="7.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILT_CUTOFF_P" value="10.0"/>
<define name="FILT_CUTOFF_Q" value="10.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILTER_RATES_SECOND_ORDER" value="TRUE" />
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<define name="OUTPUT_NOTCH_FILTER" value="FALSE"/>
<define name="OUTPUT_NOTCH_FILTER_CUTOFF_F" value="6.5"/>
<define name="OUTPUT_NOTCH_FILTER_BANDWITDH" value="3.0"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
<define value="FALSE" name="USE_ADAPTIVE"/>
<define value="0.001" name="ADAPTIVE_MU"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<!-- Gains -->
<define name="HOVER_KP" value="310"/>
<define name="HOVER_KD" value="130"/>
<define name="HOVER_KI" value="10"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.42"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
<!-- Reference -->
<define name="REF_MIN_ZD" value="-2.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="1.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<!-- Gains -->
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="20"/>
<define name="AGAIN" value="0"/>
</section>
<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<!-- Gains -->
<define name="POS_GAIN" value="0.3"/>
<define name="POS_GAINZ" value="0.5"/>
<define name="SPEED_GAIN" value="0.7"/>
<define name="SPEED_GAINZ" value="0.6"/>
<!-- Other -->
<define name="FILTER_CUTOFF" value="2.0"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="17.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="ZERO_AIRSPEED" value="TRUE"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="THRUST_X_EFF" value="0.00055"/>
</section>
<section name="FORWARD">
<define name="TURN_AIRSPEED_TH" value="8.0"/> <!-- This is the pitch angle that the drone will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="0.0" unit="deg"/>
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="36.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="37.2" unit="V"/>
<define name="LOW_BAT_LEVEL" value="48.4" unit="V"/>
<define name="MAX_BAT_LEVEL" value="50.4" unit="V"/>
<define name="BAT_NB_CELLS" value="12"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, pusher" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwing25" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="7"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
</airframe>
conf/flight_plans/tudelft/rotating_wing25kg_EHVB.xml
+164
View File
@@ -0,0 +1,164 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="60" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="1100" name="Rotating wing Valkenburg" security_height="2">
<header/>
<waypoints>
<waypoint name="HOME" x="12.6" y="-48.7"/>
<waypoint name="CLIMB" x="62.6" y="-80.4"/>
<waypoint name="trans" x="100." y="100."/>
<waypoint name="decel" x="100." y="100."/>
<waypoint name="end_trans" x="100." y="100."/>
<waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/>
<waypoint name="p1" lat="52.1684116" lon="4.4149282"/>
<waypoint name="p2" lat="52.1675165" lon="4.4147158"/>
<waypoint name="p3" lat="52.1688983" lon="4.4139655"/>
<waypoint name="p4" lat="52.1687661" lon="4.4133220"/>
<waypoint name="circ" lat="52.1684116" lon="4.4149282"/>
<waypoint name="TD" lat="52.1681602" lon="4.4127708"/>
<waypoint name="APP" lat="52.1679567" lon="4.4136344"/>
<waypoint name="FOLLOW" lat="52.16850964562752" lon="4.413635008734417"/>
<waypoint lat="52.169189" lon="4.410820" name="C1"/>
<waypoint lat="52.168049" lon="4.406923" name="C2"/>
<waypoint lat="52.166515" lon="4.408235" name="C3"/>
<waypoint lat="52.163255" lon="4.407668" name="C4"/>
<waypoint lat="52.161908" lon="4.410082" name="C5"/>
<waypoint lat="52.162641" lon="4.416992" name="C6"/>
<waypoint lat="52.164861" lon="4.427268" name="C7"/>
<waypoint lat="52.170422" lon="4.427511" name="C8"/>
<waypoint lat="52.172276" lon="4.424011" name="C9"/>
</waypoints>
<sectors>
<sector color="red" name="Flyzone">
<corner name="C1"/>
<corner name="C2"/>
<corner name="C3"/>
<corner name="C4"/>
<corner name="C5"/>
<corner name="C6"/>
<corner name="C7"/>
<corner name="C8"/>
<corner name="C9"/>
</sector>
</sectors>
<modules>
<!--<module name="follow_me"/>-->
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<set value="false" var="force_forward"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="false" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="false" var="force_forward"/>
<exception cond="GetPosAlt() @LT 4.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>2" vmode="throttle"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="false" var="force_forward"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<set value="false" var="force_forward"/>
<stay wp="p1"/>
</block>
<block name="go_p2">
<set value="false" var="force_forward"/>
<call_once fun="nav_set_heading_deg(90)"/>
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="line_p1_p2">
<set value="false" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<stay wp="p2"/>
</block>
<block name="line_trans_fwd">
<set value="false" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_p1)"/>
<stay wp="end_trans"/>
</block>
<block name="line_trans_quad">
<set value="false" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_p1)"/>
<stay wp="end_trans"/>
</block>
<block name="route">
<set value="false" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/>
</block>
<block name="Oval">
<set value="false" var="force_forward"/>
<oval p1="p1" p2="p2" radius="100"/>
</block>
<block name="Circle_CW">
<set value="false" var="force_forward"/>
<circle radius="100" wp="circ"/>
</block>
<block name="Circle_CCW">
<set value="false" var="force_forward"/>
<circle radius="-100" wp="circ"/>
</block>
<block name="Circle_CW_fwd">
<set value="true" var="force_forward"/>
<circle radius="100" wp="circ"/>
</block>
<block name="Circle_CCW_fwd">
<set value="true" var="force_forward"/>
<circle radius="-100" wp="circ"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="false" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="false" var="force_forward"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="false" var="force_forward"/>
<exception cond="!(GetPosAlt() @LT 12.0)" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="false" var="force_forward"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<exception cond="!(GetPosAlt() @LT 2.0)" deroute="flare_low"/>
</block>
<block name="flare_low">
<set value="false" var="force_forward"/>
<!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<!-- <call_once fun="NavStartDetectGround()"/> -->
<!-- <stay climb="-0.5" vmode="climb" wp="TD"/> -->
</block>
<block name="landed">
<set value="false" var="force_forward"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/modules/ctrl_eff_sched_rot_wing.xml
+4 -1
View File
@@ -1,7 +1,9 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ctrl_eff_sched_rot_wing" dir="ctrl">
<doc>
<description>The control effectiveness scheduler for the rotating wing drone type</description>
<description>The control effectiveness scheduler for the rotating wing quadplane drone type
- it requires a servo called ROTATION_MECH
</description>
<section name="ROT_WING" prefix="ROT_WING_EFF_SCHED_">
<define name="IXX_BODY" value="0" description=""/>
<define name="IYY_BODY" value="0" description=""/>
@@ -30,6 +32,7 @@
<makefile>
<file name="ctrl_eff_sched_rot_wing.c"/>
<test>
<define name="SERVO_ROTATION_MECH" value="0"/>
<define name="INDI_NUM_ACT" value="4"/>
<define name="INDI_OUTPUTS" value="3"/>
<define name="ROT_WING_EFF_SCHED_IXX_BODY" value="1"/>
conf/simulator/jsbsim/aircraft/rotwing25.xml
+1 -1
View File
@@ -41,7 +41,7 @@
<ixy unit="SLUG*FT2"> 0. </ixy>
<ixz unit="SLUG*FT2"> 0. </ixz>
<iyz unit="SLUG*FT2"> 0. </iyz>
<emptywt unit="LBS"> 28.67 </emptywt>
<emptywt unit="LBS"> 56 </emptywt>
<location name="CG" unit="M">
<x> 0 </x>
<y> 0 </y>
conf/userconf/tudelft/conf.xml
+11
View File
@@ -571,4 +571,15 @@
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="red"
/>
<aircraft
name="rot_wing_25kg"
ac_id="29"
airframe="airframes/tudelft/rot_wing_25kg.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing25kg_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/ctrl_eff_sched_rot_wing.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml"
gui_color="red"
/>
</conf>
sw/airborne/filters/notch_filter_float.h
+1 -1
View File
@@ -101,7 +101,7 @@ static inline void notch_filter_update(struct SecondOrderNotchFilter *filter, fl
*/
static inline float notch_filter_get_output(struct notch_filter_float *filter)
static inline float notch_filter_get_output(struct SecondOrderNotchFilter *filter)
{
return filter->yn1;
}
sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c
+5 -1
View File
@@ -28,6 +28,10 @@
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "modules/core/abi.h"
#ifndef SERVO_ROTATION_MECH
#error ctrl_eff_sched_rot_wing requires a servo named ROTATION_MECH
#endif
#ifndef ROT_WING_EFF_SCHED_IXX_BODY
#error "NO ROT_WING_EFF_SCHED_IXX_BODY defined"
@@ -117,7 +121,7 @@ static abi_event wing_position_ev;
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
{
for (int i=0; i<num_act; i++){
if (pos_msg[i].set.position && (pos_msg[i].idx = 7))
if (pos_msg[i].set.position && (pos_msg[i].idx = SERVO_ROTATION_MECH))
{
// Get wing rotation angle from sensor
eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;
sw/airborne/modules/radio_control/rc_datalink.h
+4
View File
@@ -40,6 +40,10 @@
#define RADIO_YAW 2
#define RADIO_THROTTLE 3
#define RADIO_MODE 4
#define RADIO_AUX1 5
#define RADIO_AUX2 6
#define RADIO_AUX4 7
#define RADIO_AUX6 8
extern int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
extern volatile bool rc_dl_frame_available;