Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 17:06:31 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Rotwing control update made compatible with master (#3374)

Browse Source
This commit is contained in:
Christophe De Wagter
2024-10-03 13:55:33 +02:00
committed by GitHub
parent 0abafed245
commit 4e98f10f67
32 changed files with 2072 additions and 3458 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rotwing5.xml
+49 -280
View File
@@ -1,13 +1,13 @@
<!-- This is a Rotating Wing 25kg <!-- This is a Rotating Wing 25kg
* Airframe: TUD00455 * Airframe: ???
* Autopilot: Pixhawk 6X * Autopilot: Pixhawk 6X
* Datalink: Herelink * Datalink: Herelink
* GPS: UBlox F9P (2x) * GPS: UBlox F9P (2x)
* RC: SBUS Crossfire (diversity) * RC: SBUS Crossfire (diversity)
--> -->
<airframe name="RotatingWing25Kg"> <airframe name="RotatingWing5">
<description>RotatingWing25Kg</description> <description>RotatingWing 5</description>
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<target name="ap" board="pixhawk_6x"> <target name="ap" board="pixhawk_6x">
@@ -37,14 +37,14 @@
<!-- Sensors connected to supercan --> <!-- Sensors connected to supercan -->
<module name="range_sensor_uavcan"/> <module name="range_sensor_uavcan"/>
<module name="power_uavcan"> <module name="power_uavcan">
<define name="POWER_UAVCAN_BATTERY_CIRCUITS" value="{{14,0},{6,1}}"/> <define name="POWER_UAVCAN_BATTERY_CIRCUITS" value="{{14,0},{6,0}}"/>
</module> </module>
<!-- MAVLink Genius communication --> <!-- MAVLink Genius communication -->
<module name="mavlink"> <module name="mavlink">
<configure name="MAVLINK_BAUD" value="B57600"/> <configure name="MAVLINK_BAUD" value="B57600"/>
<configure name="MAVLINK_PORT" value="UART3"/> <configure name="MAVLINK_PORT" value="UART3"/>
<define name="MAV_AUTOPILOT_ID" value="MAV_AUTOPILOT_ARDUPILOTMEGA"/> <define name="MAV_AUTOPILOT_ID" value="MAV_AUTOPILOT_ARDUPILOTMEGA"/>
</module> </module>
<!-- Log in high speed (Remove for outdoor flights) --> <!-- Log in high speed (Remove for outdoor flights) -->
@@ -64,6 +64,18 @@
<define name="RADIO_PARACHUTE" value="0"/> <define name="RADIO_PARACHUTE" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/> <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
<define name="USE_SONAR" value="1"/> <!-- Because uavcan can't handle simulation yet -->
<!-- MAVLink simulation -->
<module name="mavlink">
<define name="MAV_AUTOPILOT_ID" value="MAV_AUTOPILOT_ARDUPILOTMEGA"/>
<define name="UDP1_PORT_OUT" value="24550"/>
<define name="UDP1_PORT_IN" value="14551"/>
<define name="UDP1_HOST" value="127.0.0.1"/>
<define name="MAVLINK_DEBUG" value="printf"/>
<configure name="MAVLINK_PORT" value="UDP1"/>
</module>
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -79,7 +91,7 @@
</module> </module>
<module name="airspeed" type="ms45xx_i2c"> <module name="airspeed" type="ms45xx_i2c">
<configure name="MS45XX_I2C_DEV" value="I2C2"/> <configure name="MS45XX_I2C_DEV" value="I2C2"/>
<define name="MS45XX_AIRSPEED_SCALE" value="2.0833"/> <define name="MS45XX_PRESSURE_SCALE" value="1.3895"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/> <define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/> <define name="MS45XX_LOWPASS_TAU" value="0.15"/>
</module> </module>
@@ -102,7 +114,7 @@
<module name="imu" type="pixhawk6x"/> <module name="imu" type="pixhawk6x"/>
<module name="ins" type="ekf2"> <module name="ins" type="ekf2">
<!-- The Cube is mounted 37cm forwards of the CG --> <!-- The Cube is mounted 37cm forwards of the CG -->
<define name="INS_EKF2_IMU_POS_X" value="0.37"/> <define name="INS_EKF2_IMU_POS_X" value="0.324"/>
<define name="INS_EKF2_IMU_POS_Y" value="0.0"/> <define name="INS_EKF2_IMU_POS_Y" value="0.0"/>
<define name="INS_EKF2_IMU_POS_Z" value="0.0"/> <define name="INS_EKF2_IMU_POS_Z" value="0.0"/>
@@ -163,7 +175,7 @@
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) --> <servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) --> <servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC is neutral --> <servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC is neutral -->
<servo no="4" name="BMOTOR_PUSH" min="-3276" neutral="-3276" max="3605"/> <!-- Supercan (1100-1940us) --> <servo no="4" name="BMOTOR_PUSH" min="-4095" neutral="-3522" max="4095"/> <!-- Supercan (1070 idle 1000-2000) -->
<servo no="5" name="BPARACHUTE" min="-8191" neutral="0" max="8191"/> <servo no="5" name="BPARACHUTE" min="-8191" neutral="0" max="8191"/>
</servos> </servos>
@@ -183,7 +195,7 @@
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC --> <servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC -->
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC --> <servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC -->
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) --> <servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
<servo no="4" name="MOTOR_PUSH" min="-3276" neutral="-3276" max="3605"/> <!-- Supercan (1100-1940us) --> <servo no="4" name="MOTOR_PUSH" min="-4095" neutral="-3522" max="4095"/> <!-- Supercan (1070 idle 1000-2000) -->
<servo no="5" name="PARACHUTE" min="-8191" neutral="0" max="8191"/> <servo no="5" name="PARACHUTE" min="-8191" neutral="0" max="8191"/>
</servos> </servos>
@@ -198,7 +210,7 @@
</servos> </servos>
<servos driver="Faulhaber"> <servos driver="Faulhaber">
<servo no="0" name="ROTATION_MECH" min="29" neutral="1707" max="3385"/> <servo no="0" name="ROTATION_MECH" min="140" neutral="1805" max="3470"/>
</servos> </servos>
<commands> <commands>
@@ -214,7 +226,7 @@
<let var="rc_motors_off" value="Or(LessThan(RadioControlValues(RADIO_KILL_SWITCH), -4800), MoreThan(RadioControlValues(RADIO_PARACHUTE), 4800))"/> <let var="rc_motors_off" value="Or(LessThan(RadioControlValues(RADIO_KILL_SWITCH), -4800), MoreThan(RadioControlValues(RADIO_PARACHUTE), 4800))"/>
<let var="th_hold" value="Or($rc_motors_off, !autopilot_get_motors_on())"/> <let var="th_hold" value="Or($rc_motors_off, !autopilot_get_motors_on())"/>
<let var="servo_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let var="servo_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let var="hover_off" value="Or($th_hold, bool_disable_hover_motors)"/> <let var="hover_off" value="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/> <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<!-- Main bus --> <!-- Main bus -->
@@ -226,9 +238,9 @@
<set servo="SERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/> <set servo="SERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/>
<set servo="SERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/> <set servo="SERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/>
<set servo="AIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/> <set servo="AIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="FLAP_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/> <set servo="FLAP_RIGHT" value="($servo_hold? (RadioControlValues(RADIO_ROLL) + rw_flap_offset) : actuators_pprz[7] + rw_flap_offset)"/>
<set servo="AIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/> <set servo="AIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="FLAP_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/> <set servo="FLAP_LEFT" value="($servo_hold? (RadioControlValues(RADIO_ROLL) - rw_flap_offset) : actuators_pprz[7] - rw_flap_offset)"/>
<set servo="PARACHUTE" value="RadioControlValues(RADIO_PARACHUTE)"/> <set servo="PARACHUTE" value="RadioControlValues(RADIO_PARACHUTE)"/>
<!-- Second bus --> <!-- Second bus -->
@@ -240,13 +252,13 @@
<set servo="BSERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/> <set servo="BSERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/>
<set servo="BSERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/> <set servo="BSERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/>
<set servo="BAIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/> <set servo="BAIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="BFLAP_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/> <set servo="BFLAP_RIGHT" value="($servo_hold? (RadioControlValues(RADIO_ROLL) + rw_flap_offset) : actuators_pprz[7] + rw_flap_offset)"/>
<set servo="BAIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/> <set servo="BAIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="BFLAP_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/> <set servo="BFLAP_LEFT" value="($servo_hold? (RadioControlValues(RADIO_ROLL) - rw_flap_offset) : actuators_pprz[7] - rw_flap_offset)"/>
<set servo="BPARACHUTE" value="RadioControlValues(RADIO_PARACHUTE)"/> <set servo="BPARACHUTE" value="RadioControlValues(RADIO_PARACHUTE)"/>
<!-- Rotation mechanism --> <!-- Rotation mechanism -->
<set servo="ROTATION_MECH" value="rotwing_state_skewing.servo_pprz_cmd"/> <set servo="ROTATION_MECH" value="rotwing_state.skew_cmd"/>
</command_laws> </command_laws>
@@ -266,71 +278,8 @@
<define name="CHIRP_RADIO_CHANNEL" value="9"/> <define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section> </section>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_">
<define name="IXX_BODY" value="0.3953"/>
<define name="IYY_BODY" value="8.472"/>
<define name="IZZ" value="10.18"/>
<define name="IXX_WING" value="0.5385"/>
<define name="IYY_WING" value="1.671"/>
<define name="M" value="23.66"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.01326745"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{77.768,0.0}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.01994434, -0.00885004}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{0.0, -0.0}"/>
<define name="K_ELEVATOR" value="{ 1.27655, -13.3525, -96.0}"/>
<define name="K_RUDDER" value="{-72.5037, -0.9329, -3.23651}"/>
<define name="K_AILERON" value="5.0"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.027775,-2.41146}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</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="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
<define name="ADC_CHANNEL_CURRENT2" value="ADC_4"/>
<!-- Others -->
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<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"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_ANTENNA_DISTANCE" value="2.40"/> <!-- In meters -->
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<!-- <define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/> -->
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
<!-- Magnetometers calibration (08-04-2024 @ Valkenburg) --> <!-- Magnetometers calibration (NOT DONE!) -->
<define name="MAG_CALIB" type="array"> <define name="MAG_CALIB" type="array">
<field type="struct"> <field type="struct">
<field name="abi_id" value="5"/> <field name="abi_id" value="5"/>
@@ -339,13 +288,13 @@
<field name="scale" value="true"/> <field name="scale" value="true"/>
<field name="rotation" value="true"/> <field name="rotation" value="true"/>
</field> </field>
<field name="neutral" value="-66,372,-134" type="int[]"/> <field name="neutral" value="132,415,-357" type="int[]"/>
<field name="scale" value="{{34618,27035,27547},{57579,48737,47275}}"/> <field name="scale" value="{{13019,21924,2280},{22037,40349,3953}}"/>
<field name="body_to_sensor" value="{{0,-16384,0, -16384,0,0, 0,0,-16384}}"/> <field name="body_to_sensor" value="{{0,-16384,0, -16384,0,0, 0,0,-16384}}"/>
</field> </field>
</define> </define>
<!-- Accelerometers calibration --> <!-- Accelerometers calibration (NOT DONE!) -->
<define name="ACCEL_CALIB" type="array"> <define name="ACCEL_CALIB" type="array">
<field type="struct"> <field type="struct">
<field name="abi_id" value="26"/> <field name="abi_id" value="26"/>
@@ -360,7 +309,7 @@
</field> </field>
</define> </define>
<!-- Gyrometer calibration --> <!-- Gyrometer calibration (NOT DONE!) -->
<define name="GYRO_CALIB" type="array"> <define name="GYRO_CALIB" type="array">
<field type="struct"> <field type="struct">
<field name="abi_id" value="26"/> <field name="abi_id" value="26"/>
@@ -379,201 +328,9 @@
<!--define name="LOG_HIGHSPEED" value="TRUE"/--> <!--define name="LOG_HIGHSPEED" value="TRUE"/-->
<!-- Define axis in hover frame --> <!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/> <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_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="180." 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="5" 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 25 kg-->
<define name="G1_ROLL" value="{ 0.0, -9.0, 0.0, 9.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.14, 0.0, -1.14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{ -0.29, 0.29, -0.29, 0.29, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{ -0.54, -0.54, -0.54, -0.54, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{13.68, 13.68, 13.68, 13.68, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}" />
<define VALUE="{1000, 1000, 1, 100, 100}" NAME="WLS_PRIORITIES"/>
<define VALUE="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}" NAME="WLS_WU"/>
<!-- Rate INDI -->
<define name="MAX_RATE" value="1.5"/>
<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
<define name="REF_ERR_Q" value="25.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.5"/>
<define name="REF_RATE_Q" value="4.5"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="FALSE" />
<define name="FILT_CUTOFF_P" value="3.0"/>
<define name="FILT_CUTOFF_Q" value="3.0"/>
<define name="FILT_CUTOFF_R" value="3.0"/>
<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, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.85"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="40" 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_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- 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="25.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="ZERO_AIRSPEED" value="FALSE"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="MAX_PUSHER_INCREMENT" value="2000"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="LINE_GAIN" value="0.2"/>
<define name="CLIMB_SPEED_FWD" value="2.0"/>
<define name="DESCEND_SPEED_FWD" value="-3.0"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="4.0"/>
</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="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_RATE_DIRECT"/>
<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="52.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="48.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, aileron, flaperon, 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="9"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section> </section>
<checklist> <checklist>
@@ -590,8 +347,20 @@
<item name="takeoff location">Put UAV on take-off location</item> <item name="takeoff location">Put UAV on take-off location</item>
<item name="flight plan">Check flight plan</item> <item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item> <item name="flight block">Switch to correct flight block</item>
<item name="drone tag">Switch on drone tag</item>
<item name="camera">Switch on camera</item> <item name="camera">Switch on camera</item>
<item name="parachute">Arm parachute</item> <item name="parachute">Arm parachute</item>
<item name="announce">Announce flight to other airspace users</item> <item name="announce">Announce flight to other airspace users</item>
</checklist> </checklist>
<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="52.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="48.4" unit="V"/>
<define name="BAT_NB_CELLS" value="12"/>
</section>
<include href="conf/airframes/tudelft/rotwing_25kg_common.xml" />
</airframe> </airframe>
conf/airframes/tudelft/rotwing6.xml
+52 -276
View File
@@ -1,13 +1,13 @@
<!-- This is a Rotating Wing 25kg <!-- This is a Rotating Wing 25kg
* Airframe: TUD00455 * Airframe: ???
* Autopilot: Pixhawk 6X * Autopilot: Pixhawk 6X
* Datalink: Herelink * Datalink: Herelink
* GPS: UBlox F9P (2x) * GPS: UBlox F9P (2x)
* RC: SBUS Crossfire (diversity) * RC: SBUS Crossfire (diversity)
--> -->
<airframe name="RotatingWing25Kg"> <airframe name="RotatingWing6">
<description>RotatingWing25Kg</description> <description>RotatingWing 6</description>
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<target name="ap" board="pixhawk_6x"> <target name="ap" board="pixhawk_6x">
@@ -40,6 +40,13 @@
<define name="POWER_UAVCAN_BATTERY_CIRCUITS" value="{{14,0},{6,1}}"/> <define name="POWER_UAVCAN_BATTERY_CIRCUITS" value="{{14,0},{6,1}}"/>
</module> </module>
<!-- MAVLink Genius communication -->
<module name="mavlink">
<configure name="MAVLINK_BAUD" value="B57600"/>
<configure name="MAVLINK_PORT" value="UART3"/>
<define name="MAV_AUTOPILOT_ID" value="MAV_AUTOPILOT_ARDUPILOTMEGA"/>
</module>
<!-- Log in high speed (Remove for outdoor flights) --> <!-- Log in high speed (Remove for outdoor flights) -->
<!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> --> <!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> -->
</target> </target>
@@ -57,6 +64,18 @@
<define name="RADIO_PARACHUTE" value="0"/> <define name="RADIO_PARACHUTE" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/> <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
<define name="USE_SONAR" value="1"/> <!-- Because uavcan can't handle simulation yet -->
<!-- MAVLink simulation -->
<module name="mavlink">
<define name="MAV_AUTOPILOT_ID" value="MAV_AUTOPILOT_ARDUPILOTMEGA"/>
<define name="UDP1_PORT_OUT" value="24550"/>
<define name="UDP1_PORT_IN" value="14551"/>
<define name="UDP1_HOST" value="127.0.0.1"/>
<define name="MAVLINK_DEBUG" value="printf"/>
<configure name="MAVLINK_PORT" value="UDP1"/>
</module>
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -72,7 +91,7 @@
</module> </module>
<module name="airspeed" type="ms45xx_i2c"> <module name="airspeed" type="ms45xx_i2c">
<configure name="MS45XX_I2C_DEV" value="I2C2"/> <configure name="MS45XX_I2C_DEV" value="I2C2"/>
<define name="MS45XX_AIRSPEED_SCALE" value="2.0833"/> <define name="MS45XX_PRESSURE_SCALE" value="1.3231"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/> <define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/> <define name="MS45XX_LOWPASS_TAU" value="0.15"/>
</module> </module>
@@ -95,7 +114,7 @@
<module name="imu" type="pixhawk6x"/> <module name="imu" type="pixhawk6x"/>
<module name="ins" type="ekf2"> <module name="ins" type="ekf2">
<!-- The Cube is mounted 37cm forwards of the CG --> <!-- The Cube is mounted 37cm forwards of the CG -->
<define name="INS_EKF2_IMU_POS_X" value="0.37"/> <define name="INS_EKF2_IMU_POS_X" value="0.324"/>
<define name="INS_EKF2_IMU_POS_Y" value="0.0"/> <define name="INS_EKF2_IMU_POS_Y" value="0.0"/>
<define name="INS_EKF2_IMU_POS_Z" value="0.0"/> <define name="INS_EKF2_IMU_POS_Z" value="0.0"/>
@@ -155,7 +174,7 @@
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) --> <servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) --> <servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC is neutral --> <servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC is neutral -->
<servo no="4" name="BMOTOR_PUSH" min="-3276" neutral="-3276" max="3605"/> <!-- Supercan (1100-1940us) --> <servo no="4" name="BMOTOR_PUSH" min="-3276" neutral="-2500" max="3605"/> <!-- Supercan (1100-1940us) -->
<servo no="5" name="BPARACHUTE" min="-8191" neutral="0" max="8191"/> <servo no="5" name="BPARACHUTE" min="-8191" neutral="0" max="8191"/>
</servos> </servos>
@@ -175,7 +194,7 @@
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC --> <servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC -->
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC --> <servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/> <!-- T-Motor ESC -->
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) --> <servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
<servo no="4" name="MOTOR_PUSH" min="-3276" neutral="-3276" max="3605"/> <!-- Supercan (1100-1940us) --> <servo no="4" name="MOTOR_PUSH" min="-3276" neutral="-2500" max="3605"/> <!-- Supercan (1100-1940us) -->
<servo no="5" name="PARACHUTE" min="-8191" neutral="0" max="8191"/> <servo no="5" name="PARACHUTE" min="-8191" neutral="0" max="8191"/>
</servos> </servos>
@@ -190,7 +209,7 @@
</servos> </servos>
<servos driver="Faulhaber"> <servos driver="Faulhaber">
<servo no="0" name="ROTATION_MECH" min="29" neutral="1707" max="3385"/> <servo no="0" name="ROTATION_MECH" min="140" neutral="1805" max="3470"/>
</servos> </servos>
<commands> <commands>
@@ -206,7 +225,7 @@
<let var="rc_motors_off" value="Or(LessThan(RadioControlValues(RADIO_KILL_SWITCH), -4800), MoreThan(RadioControlValues(RADIO_PARACHUTE), 4800))"/> <let var="rc_motors_off" value="Or(LessThan(RadioControlValues(RADIO_KILL_SWITCH), -4800), MoreThan(RadioControlValues(RADIO_PARACHUTE), 4800))"/>
<let var="th_hold" value="Or($rc_motors_off, !autopilot_get_motors_on())"/> <let var="th_hold" value="Or($rc_motors_off, !autopilot_get_motors_on())"/>
<let var="servo_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let var="servo_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let var="hover_off" value="Or($th_hold, bool_disable_hover_motors)"/> <let var="hover_off" value="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/> <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<!-- Main bus --> <!-- Main bus -->
@@ -218,9 +237,9 @@
<set servo="SERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/> <set servo="SERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/>
<set servo="SERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/> <set servo="SERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/>
<set servo="AIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/> <set servo="AIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="FLAP_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/> <set servo="FLAP_RIGHT" value="($servo_hold? (RadioControlValues(RADIO_ROLL) + rw_flap_offset) : actuators_pprz[7] + rw_flap_offset)"/>
<set servo="AIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/> <set servo="AIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="FLAP_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/> <set servo="FLAP_LEFT" value="($servo_hold? (RadioControlValues(RADIO_ROLL) - rw_flap_offset) : actuators_pprz[7] - rw_flap_offset)"/>
<set servo="PARACHUTE" value="RadioControlValues(RADIO_PARACHUTE)"/> <set servo="PARACHUTE" value="RadioControlValues(RADIO_PARACHUTE)"/>
<!-- Second bus --> <!-- Second bus -->
@@ -232,13 +251,13 @@
<set servo="BSERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/> <set servo="BSERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/>
<set servo="BSERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/> <set servo="BSERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/>
<set servo="BAIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/> <set servo="BAIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="BFLAP_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/> <set servo="BFLAP_RIGHT" value="($servo_hold? (RadioControlValues(RADIO_ROLL) + rw_flap_offset) : actuators_pprz[7] + rw_flap_offset)"/>
<set servo="BAIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/> <set servo="BAIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="BFLAP_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/> <set servo="BFLAP_LEFT" value="($servo_hold? (RadioControlValues(RADIO_ROLL) - rw_flap_offset) : actuators_pprz[7] - rw_flap_offset)"/>
<set servo="BPARACHUTE" value="RadioControlValues(RADIO_PARACHUTE)"/> <set servo="BPARACHUTE" value="RadioControlValues(RADIO_PARACHUTE)"/>
<!-- Rotation mechanism --> <!-- Rotation mechanism -->
<set servo="ROTATION_MECH" value="rotwing_state_skewing.servo_pprz_cmd"/> <set servo="ROTATION_MECH" value="rotwing_state.skew_cmd"/>
</command_laws> </command_laws>
@@ -258,71 +277,8 @@
<define name="CHIRP_RADIO_CHANNEL" value="9"/> <define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section> </section>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_">
<define name="IXX_BODY" value="0.3953"/>
<define name="IYY_BODY" value="8.472"/>
<define name="IZZ" value="10.18"/>
<define name="IXX_WING" value="0.5385"/>
<define name="IYY_WING" value="1.671"/>
<define name="M" value="23.66"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.01326745"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{77.768,0.0}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.01994434, -0.00885004}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{0.0, -0.0}"/>
<define name="K_ELEVATOR" value="{ 1.27655, -13.3525, -96.0}"/>
<define name="K_RUDDER" value="{-72.5037, -0.9329, -3.23651}"/>
<define name="K_AILERON" value="5.0"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.027775,-2.41146}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</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="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
<define name="ADC_CHANNEL_CURRENT2" value="ADC_4"/>
<!-- Others -->
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<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"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_ANTENNA_DISTANCE" value="2.40"/> <!-- In meters -->
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<!-- <define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/> -->
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
<!-- Magnetometers calibration (08-04-2024 @ Valkenburg) --> <!-- Magnetometers calibration (NOT DONE!) -->
<define name="MAG_CALIB" type="array"> <define name="MAG_CALIB" type="array">
<field type="struct"> <field type="struct">
<field name="abi_id" value="5"/> <field name="abi_id" value="5"/>
@@ -331,13 +287,13 @@
<field name="scale" value="true"/> <field name="scale" value="true"/>
<field name="rotation" value="true"/> <field name="rotation" value="true"/>
</field> </field>
<field name="neutral" value="-66,372,-134" type="int[]"/> <field name="neutral" value="132,415,-357" type="int[]"/>
<field name="scale" value="{{34618,27035,27547},{57579,48737,47275}}"/> <field name="scale" value="{{13019,21924,2280},{22037,40349,3953}}"/>
<field name="body_to_sensor" value="{{0,-16384,0, -16384,0,0, 0,0,-16384}}"/> <field name="body_to_sensor" value="{{0,-16384,0, -16384,0,0, 0,0,-16384}}"/>
</field> </field>
</define> </define>
<!-- Accelerometers calibration --> <!-- Accelerometers calibration (NOT DONE!) -->
<define name="ACCEL_CALIB" type="array"> <define name="ACCEL_CALIB" type="array">
<field type="struct"> <field type="struct">
<field name="abi_id" value="26"/> <field name="abi_id" value="26"/>
@@ -352,7 +308,7 @@
</field> </field>
</define> </define>
<!-- Gyrometer calibration --> <!-- Gyrometer calibration (NOT DONE!) -->
<define name="GYRO_CALIB" type="array"> <define name="GYRO_CALIB" type="array">
<field type="struct"> <field type="struct">
<field name="abi_id" value="26"/> <field name="abi_id" value="26"/>
@@ -371,201 +327,9 @@
<!--define name="LOG_HIGHSPEED" value="TRUE"/--> <!--define name="LOG_HIGHSPEED" value="TRUE"/-->
<!-- Define axis in hover frame --> <!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/> <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_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="180." 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="5" 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 25 kg-->
<define name="G1_ROLL" value="{ 0.0, -9.0, 0.0, 9.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.14, 0.0, -1.14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{ -0.29, 0.29, -0.29, 0.29, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{ -0.54, -0.54, -0.54, -0.54, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{13.68, 13.68, 13.68, 13.68, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}" />
<define VALUE="{1000, 1000, 1, 100, 100}" NAME="WLS_PRIORITIES"/>
<define VALUE="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}" NAME="WLS_WU"/>
<!-- Rate INDI -->
<define name="MAX_RATE" value="1.5"/>
<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
<define name="REF_ERR_Q" value="25.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.5"/>
<define name="REF_RATE_Q" value="4.5"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="FALSE" />
<define name="FILT_CUTOFF_P" value="3.0"/>
<define name="FILT_CUTOFF_Q" value="3.0"/>
<define name="FILT_CUTOFF_R" value="3.0"/>
<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, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.85"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="40" 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_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- 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="25.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="ZERO_AIRSPEED" value="FALSE"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="MAX_PUSHER_INCREMENT" value="2000"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="LINE_GAIN" value="0.2"/>
<define name="CLIMB_SPEED_FWD" value="2.0"/>
<define name="DESCEND_SPEED_FWD" value="-3.0"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="4.0"/>
</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="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_RATE_DIRECT"/>
<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="52.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="48.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, aileron, flaperon, 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="9"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section> </section>
<checklist> <checklist>
@@ -582,8 +346,20 @@
<item name="takeoff location">Put UAV on take-off location</item> <item name="takeoff location">Put UAV on take-off location</item>
<item name="flight plan">Check flight plan</item> <item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item> <item name="flight block">Switch to correct flight block</item>
<item name="drone tag">Switch on drone tag</item>
<item name="camera">Switch on camera</item> <item name="camera">Switch on camera</item>
<item name="parachute">Arm parachute</item> <item name="parachute">Arm parachute</item>
<item name="announce">Announce flight to other airspace users</item> <item name="announce">Announce flight to other airspace users</item>
</checklist> </checklist>
<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="52.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="48.4" unit="V"/>
<define name="BAT_NB_CELLS" value="12"/>
</section>
<include href="conf/airframes/tudelft/rotwing_25kg_common.xml" />
</airframe> </airframe>
conf/airframes/tudelft/rotwing_25kg_common.xml
+258
View File
@@ -0,0 +1,258 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_">
<define name="IXX_BODY" value="0.3953"/>
<define name="IYY_BODY" value="8.472"/>
<define name="IZZ" value="10.18"/>
<define name="IXX_WING" value="0.5385"/>
<define name="IYY_WING" value="1.671"/>
<define name="M" value="23.66"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="{0.0, 0.0111}"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{0.0, 0.005}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.01994434, -0.00885004}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{0.0, -0.0}"/>
<define name="K_ELEVATOR" value="{ 1.27655, -6.0, -48.0}"/>
<define name="K_RUDDER" value="{-72.5037, -0.9329, -16.75}"/>
<define name="K_AILERON" value="5.0"/>
<define name="K_FLAPERON" value="2.35"/>
<define name="K_PUSHER" value="{0.027775,-2.41146}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{1264.51356869248,1.11607176184111,-0.0000229206473957927}"/>
<define name="K_LIFT_WING" value="{-0.7811 -1.4319}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="MISC">
<define name="DEFAULT_CIRCLE_RADIUS" value="300.0"/>
<!-- Others -->
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="300"/>
<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"/>
<define name="NAV_HYBRID_MAX_AIRSPEED" value="30"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.8"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<!-- Ground detect -->
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<!-- AGL distance -->
<define name="AGL_DIST_FILTER" value="0.07"/>
<!-- Don't oversaturate the UAVCAN bus -->
<define name="ACTUATORS_UAVCAN_RAW_DIV" value="2"/> <!-- ESC's at half the PERIODIC_FREQ -->
<define name="ACTUATORS_UAVCAN_CMD_DIV" value="4"/> <!-- Servo's at 1/4 the PERIODIC_FREQ -->
</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="5" 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 25 kg-->
<define name="G1_ROLL" value="{ 0.0, -9.0, 0.0, 9.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.14, 0.0, -1.14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{ -0.29, 0.29, -0.29, 0.29, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{ -0.54, -0.54, -0.54, -0.54, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{13.68, 13.68, 13.68, 13.68, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}" />
<!-- Rate INDI -->
<define name="MAX_RATE" value="1.5"/>
<!-- Reference -->
<define name="REF_ERR_P" value="25.0"/>
<define name="REF_ERR_Q" value="25.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="4.5"/>
<define name="REF_RATE_Q" value="4.5"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="FALSE" />
<define name="FILT_CUTOFF_P" value="3.0"/>
<define name="FILT_CUTOFF_Q" value="3.0"/>
<define name="FILT_CUTOFF_R" value="3.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 0.2, 1.0, 0.2, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.55"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="40" 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="ROTWING" prefix="ROTWING_">
<define name="FW_MIN_AIRSPEED" value="21"/> <!-- Forward stall airspeed + margin (motors off) -->
<define name="FW_QUAD_MIN_AIRSPEED" value="19"/> <!-- Forward stall airspeed + margin with quad motors on -->
<define name="FW_CRUISE_AIRSPEED" value="25"/> <!-- Default cruise airspeed -->
<define name="FW_MAX_AIRSPEED" value="30"/> <!-- Maximum forward airspeed -->
<define name="FW_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in fixed wing mode -->
<define name="QUAD_NOPUSH_AIRSPEED" value="8"/> <!-- Maximum quadrotor without pusher motor airspeed -->
<define name="QUAD_MAX_AIRSPEED" value="15"/> <!-- Maximum quadrotor airspeed (with pusher motor)-->
<define name="QUAD_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in quad mode -->
<define name="SKEW_UP_AIRSPEED" value="13"/> <!-- Airspeed where the skewing starts when going up in airspeed -->
<define name="SKEW_DOWN_AIRSPEED" value="11"/> <!-- Airspeed where the skewing starts when going down in airspeed -->
<define name="SKEW_REF_MODEL" value="FALSE"/> <!-- Enable second order reference model for the skewing command -->
<define name="SKEW_REF_MODEL_P_GAIN" value="0.004"/> <!-- Skewing reference model proportional gain -->
<define name="SKEW_REF_MODEL_D_GAIN" value="0.007"/> <!-- Skewing reference model differential gain -->
<define name="SKEW_REF_MODEL_MAX_SPEED" value="9.6"/> <!-- Maximum rotational skewing speed bound for the reference model -->
</section>
<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 3000., 4., 30.}"/>
<!-- Gains -->
<define name="POS_GAIN" value="1.0"/>
<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="25.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="MAX_PUSHER_INCREMENT" value="2000"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="4.0"/>
<!-- Climb and descend speeds -->
<define name="FWD_CLIMB_SPEED" value="2.0"/>
<define name="FWD_DESCEND_SPEED" value="-3.0"/>
<define name="QUAD_CLIMB_SPEED" value="2.0"/>
<define name="QUAD_DESCEND_SPEED" value="-1.0"/>
</section>
<section name="FORWARD">
<define name="TURN_AIRSPEED_TH" value="8.0"/>
<define name="TRANSITION_MAX_OFFSET" value="0.0" unit="deg"/>
</section>
<section name="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</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>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, aileron, flaperon, pusher, skew" 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="9"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
</airframe>
conf/airframes/tudelft/rotwing_7kg_common.xml
+316
View File
@@ -0,0 +1,316 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_">
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="{15.37287553553,0.0}"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.000968063}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{1588.40,0.9212749,-1.833829e-05}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="MISC">
<!-- Others -->
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<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"/>
<define name="NAV_HYBRID_MAX_AIRSPEED" value="22"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.8"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="100.0"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
<!-- Ground detect -->
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<!-- AGL distance -->
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
<!-- Flight plan defines -->
<define name="FLARE_HEIGHT" value="12"/>
<define name="HYBRID_HEIGHT" value="20"/>
<define name="TRANSITION_HEIGHT" value="40"/>
<define name="LIFTOFF_PITCH_LIMIT_DEG" value="5"/>
<define name="LIFTOFF_ROLL_LIMIT_DEG" value="5"/>
<define name="FWD_SIDESLIP_GAIN" value="0.25"/>
</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="5" 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 7 kg-->
<define name="G1_ROLL" value="{ 0.0, -15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.5, 0.0, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{- 0.3, 0.3, -0.3, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.55}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<!-- Rate INDI -->
<define name="MAX_RATE" value="1.5"/>
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="7.0"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="TRUE"/>
<define name="FILT_CUTOFF_P" value="5.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.68"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="50" 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="ROTWING" prefix="ROTWING_">
<define name="FW_MIN_AIRSPEED" value="17.0"/> <!-- Forward stall airspeed + margin (motors off) -->
<define name="FW_QUAD_MIN_AIRSPEED" value="15.0"/> <!-- Forward stall airspeed + margin with quad motors on -->
<define name="FW_CRUISE_AIRSPEED" value="19.0"/> <!-- Default cruise airspeed -->
<define name="FW_MAX_AIRSPEED" value="22.0"/> <!-- Maximum forward airspeed -->
<define name="FW_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in fixed wing mode -->
<define name="QUAD_NOPUSH_AIRSPEED" value="5.0"/> <!-- Maximum quadrotor without pusher motor airspeed -->
<define name="QUAD_MAX_AIRSPEED" value="12.0"/> <!-- Maximum quadrotor airspeed (with pusher motor)-->
<define name="QUAD_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in quad mode -->
<define name="SKEW_UP_AIRSPEED" value="10.0"/> <!-- Airspeed where the skewing starts when going up in airspeed -->
<define name="SKEW_DOWN_AIRSPEED" value="8.0"/> <!-- Airspeed where the skewing starts when going down in airspeed -->
<define name="SKEW_REF_MODEL" value="TRUE"/> <!-- Enable second order reference model for the skewing command -->
<define name="SKEW_REF_MODEL_P_GAIN" value="0.001"/> <!-- Skewing reference model proportional gain -->
<define name="SKEW_REF_MODEL_D_GAIN" value="0.003"/> <!-- Skewing reference model differential gain -->
<define name="SKEW_REF_MODEL_MAX_SPEED" value="20"/> <!-- Maximum rotational skewing speed bound for the reference model -->
</section>
<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- Gains -->
<define name="POS_GAIN" value="1.0"/>
<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.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
<!-- Climb and descend speeds -->
<define name="FWD_CLIMB_SPEED" value="2.0"/>
<define name="FWD_DESCEND_SPEED" value="-3.0"/>
<define name="QUAD_CLIMB_SPEED" value="2.0"/>
<define name="QUAD_DESCEND_SPEED" value="-1.0"/>
<!-- Coordinated turns -->
<define name="COORDINATED_TURN_AIRSPEED_MARGIN" value="8.0"/>
</section>
<section name="FORWARD">
<define name="TURN_AIRSPEED_TH" value="10.0"/>
<define name="TRANSITION_MAX_OFFSET" value="0.0" unit="deg"/>
</section>
<section name="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</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>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, aileron, flap, pusher, skew" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwing3" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="10"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
<!--Airspeed estimation using EKF-->
<section name="EKF_AW">
<define name="EKF_AW_P0_V_BODY" value="1.E-2f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1.E-5f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1.E-10f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1.0E-4f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1.0E-9f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1.0E-6f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1.0E-8f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1.E-5f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1.E-5f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1.E-5f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1.E-5f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1.E-7f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_AZ_SCHED_GAIN" value="2" />
<define name="EKF_AW_AZ_SCHED_START_DEG" value="60" />
<define name="EKF_AW_AZ_SCHED_END_DEG" value="70" />
<define name="EKF_AW_AX_SCHED_GAIN" value="0" />
<define name="EKF_AW_AX_SCHED_START_DEG" value="45" />
<define name="EKF_AW_AX_SCHED_END_DEG" value="60" />
<define name="EKF_AW_QUICK_CONVERGENCE" value="true" />
<define name="EKF_AW_QUICK_CONVERGENCE_TIME" value="15" />
<define name="EKF_AW_WING_INSTALLED" value="true" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED_X" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Y" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Z" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_VEHICLE_MASS" value="6.5" description="Mass of the vehicle"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="-4E-2f" description="K*u*u"/>
<define name="EKF_AW_K3_FX_HOVER" value="-3E-1f" description="K*u"/>
<define name="EKF_AW_K1_FX_WING" value="-3.21432e-02f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FX_WING" value="1.67195e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FX_WING" value="5.9441e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FX_WING" value="3.9839e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K5_FX_WING" value="3.5321e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K_FY_V" value="-3.2E-1f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FY_WING" value="0.0f" description="Fy = K1*cos(skew)*sin(skew)*u^2"/>
<define name="EKF_AW_K1_FZ_WING" value="-1.0008e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FZ_WING" value="-8.6965e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FZ_WING" value="1.45783e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FZ_WING" value="2.185395e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
</section>
</airframe>
conf/airframes/tudelft/rotwing_v3b.xml
+36 -310
View File
@@ -30,16 +30,12 @@
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/> <define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<!--Only send gyro and accel that is being used--> <!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/> <define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/> <define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<module name="lidar_tfmini">
<configure name="TFMINI_PORT" value="UART8"/>
<configure name="USE_TFMINI_AGL" value="TRUE"/>
</module>
<module name="wing_rotation_adc_sensor"> <module name="wing_rotation_adc_sensor">
<define name="ADC_WING_ROT_SCALE" value="0.00247111"/> <define name="ADC_WING_ROT_SCALE" value="0.00247111"/>
<define name="ADC_WING_ROT_OFFSET" value="-25.635294"/> <define name="ADC_WING_ROT_OFFSET" value="-25.635294"/>
@@ -69,6 +65,7 @@
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/> <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
<define name="USE_SONAR" value="1"/> <!-- Because uavcan can't handle simulation yet -->
</target> </target>
<!-- Kongsberg datalink --> <!-- Kongsberg datalink -->
@@ -99,12 +96,17 @@
<configure name="UBX_GPS_BAUD" value="B460800"/> <configure name="UBX_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/> <define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module> </module>
<module name="lidar_tfmini">
<configure name="TFMINI_PORT" value="UART8"/>
<configure name="USE_TFMINI_AGL" value="TRUE"/>
</module>
<!-- IMU / INS --> <!-- IMU / INS -->
<module name="imu" type="cube"/> <module name="imu" type="cube"/>
<module name="ins" type="ekf2"/> <module name="ins" type="ekf2"/>
<module name="parachute"/> <module name="parachute"/>
<module name="ekf_aw"/>
<!-- Actuators on dual CAN bus --> <!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan"> <module name="actuators" type="uavcan">
@@ -141,64 +143,6 @@
<module name="pfc_actuators"/> <module name="pfc_actuators"/>
<module name="agl_dist"/> <module name="agl_dist"/>
<module name="approach_moving_target"/> <module name="approach_moving_target"/>
<!--Airspeed estimation using EKF-->
<module name="ekf_aw">
<define name="EKF_AW_WRAPPER_ROTWING_TYPE_A" value="0" description="1 for A, 0 for b"/>
<define name="EKF_AW_P0_V_BODY" value="1.E-2f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1.E-5f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1.E-10f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1.0E-4f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1.0E-9f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1.0E-6f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1.0E-8f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1.E-5f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1.E-5f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1.E-5f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1.E-5f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1.E-7f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_AZ_SCHED_GAIN" value="2" />
<define name="EKF_AW_AZ_SCHED_START_DEG" value="60" />
<define name="EKF_AW_AZ_SCHED_END_DEG" value="70" />
<define name="EKF_AW_AX_SCHED_GAIN" value="0" />
<define name="EKF_AW_AX_SCHED_START_DEG" value="45" />
<define name="EKF_AW_AX_SCHED_END_DEG" value="60" />
<define name="EKF_AW_QUICK_CONVERGENCE" value="true" />
<define name="EKF_AW_QUICK_CONVERGENCE_TIME" value="15" />
<define name="EKF_AW_WING_INSTALLED" value="true" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED_X" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Y" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Z" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_VEHICLE_MASS" value="6.5" description="Mass of the vehicle"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="-4E-2f" description="K*u*u"/>
<define name="EKF_AW_K3_FX_HOVER" value="-3E-1f" description="K*u"/>
<define name="EKF_AW_K1_FX_WING" value="-3.21432e-02f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FX_WING" value="1.67195e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FX_WING" value="5.9441e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FX_WING" value="3.9839e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K5_FX_WING" value="3.5321e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K_FY_V" value="-3.2E-1f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FY_WING" value="0.0f" description="Fy = K1*cos(skew)*sin(skew)*u^2"/>
<define name="EKF_AW_K1_FZ_WING" value="-1.0008e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FZ_WING" value="-8.6965e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FZ_WING" value="1.45783e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FZ_WING" value="2.185395e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_TELEMETRY_DEBUG" value="false" description="debug mode to send timing info through telemetry"/>
</module>
</firmware> </firmware>
<!-- PWM actuators --> <!-- PWM actuators -->
@@ -257,9 +201,9 @@
<command_laws> <command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/> <let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/> <let VAR="hover_off" VALUE="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<let var="ail_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 20)"/> <let var="ail_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 20)"/>
<let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/> <let var="flap_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 50)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/> <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<call fun="pfc_actuators_run()"/> <call fun="pfc_actuators_run()"/>
@@ -270,7 +214,7 @@
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/> <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/> <set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="ROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="ROTATION_MECH"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/> <set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
@@ -295,36 +239,7 @@
<define name="CHIRP_RADIO_CHANNEL" value="9"/> <define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section> </section>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_"> <section name="CALIBRATION">
<define name="IXX_BODY" value="-0.13362537"/>
<define name="IYY_BODY" value="0.77187463"/>
<define name="IZZ" value="1.18"/>
<define name="IXX_WING" value="0.25352115"/>
<define name="IYY_WING" value="0.38472811"/>
<define name="M" value="6.94"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-1.16702}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="MISC">
<!-- Voltage and current measurements --> <!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/> <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/> <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
@@ -333,7 +248,7 @@
<define name="PFC_ACTUATORS" type="array"> <define name="PFC_ACTUATORS" type="array">
<!-- Aerodynamic --> <!-- Aerodynamic -->
<field type="struct"> <field type="struct">
<field name="feedback_id" value="255"/> <field name="feedback_id" value="SERVO_SERVO_ELEVATOR_IDX"/>
<field name="feedback_id2" value="255"/> <field name="feedback_id2" value="255"/>
<field name="low" value="-4500"/> <field name="low" value="-4500"/>
<field name="high" value="4500"/> <field name="high" value="4500"/>
@@ -342,7 +257,7 @@
<field name="timeout" value="1"/> <field name="timeout" value="1"/>
</field> </field>
<field type="struct"> <field type="struct">
<field name="feedback_id" value="255"/> <field name="feedback_id" value="SERVO_SERVO_RUDDER_IDX"/>
<field name="feedback_id2" value="255"/> <field name="feedback_id2" value="255"/>
<field name="low" value="-4500"/> <field name="low" value="-4500"/>
<field name="high" value="4500"/> <field name="high" value="4500"/>
@@ -441,33 +356,10 @@
<field name="low" value="-9600"/> <field name="low" value="-9600"/>
<field name="high" value="2000"/> <field name="high" value="2000"/>
<field name="low_feedback" value="0"/> <field name="low_feedback" value="0"/>
<field name="high_feedback" value="1800"/> <field name="high_feedback" value="2800"/>
<field name="timeout" value="3"/> <field name="timeout" value="3"/>
</field> </field>
</define> </define>
<!-- Others -->
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<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"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
@@ -481,168 +373,26 @@
<define name="BODY_TO_IMU_PSI" value="-90." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="-90." unit="deg"/>
</section> </section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_"> <checklist>
<!-- Limits --> <item name="pic" type="text">Enter the PIC</item>
<define name="SP_MAX_PHI" value="45." unit="deg" /> <item name="pac" type="text">Enter the PAC</item>
<define name="SP_MAX_THETA" value="45." unit="deg"/> <item name="gcs" type="text">Enter the GCS op</item>
<define name="SP_MAX_R" value="90." unit="deg/s"/> <item name="goal" type="text">Goal of the flight</item>
<define name="DEADBAND_R" value="200"/> <item name="basic law">Location, airspace and weather</item>
<define name="SP_PSI_DELTA_LIMIT" value="5" unit="deg"/> <item name="RC Battery">Check the RC battery</item>
<item name="tail connection">Check tail connection</item>
<!-- Reference model --> <item name="wing tape">Check wings taped and secured</item>
<define name="REF_OMEGA_P" value="800" unit="deg/s"/> <item name="inspection">Inspect airframe condition</item>
<define name="REF_ZETA_P" value="0.85"/> <item name="attitude">Check attitude and heading</item>
<define name="REF_MAX_P" value="300." unit="deg/s"/> <item name="airspeed">Airspeed sensor calibration</item>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/> <item name="takeoff location">Put UAV on take-off location</item>
<item name="actuators">Automated actuator check</item>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/> <item name="flight plan">Check flight plan</item>
<define name="REF_ZETA_Q" value="0.85"/> <item name="flight block">Switch to correct flight block</item>
<define name="REF_MAX_Q" value="300." unit="deg/s"/> <item name="drone tag">Switch on drone tag</item>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/> <item name="camera">Switch on camera</item>
<item name="announce">Announce flight to other airspace users</item>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/> </checklist>
<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 7 kg-->
<define name="G1_ROLL" value="{ 0.0, -15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.5, 0.0, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{- 0.3, 0.3, -0.3, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.55}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="7.0"/>
<define name="REF_RATE_Q" value="9.0"/>
<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="20.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.85"/>
<define name="ROTWING_V3B" value="1" />
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="50" 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_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- 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.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="LINE_GAIN" value="0.2"/>
<define name="CLIMB_SPEED_FWD" value="2.0"/>
<define name="DESCEND_SPEED_FWD" value="-3.0"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
</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="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</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>
<section name="BAT"> <section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/> <define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
@@ -653,29 +403,5 @@
<define name="BAT_NB_CELLS" value="6"/> <define name="BAT_NB_CELLS" value="6"/>
</section> </section>
<section name="SIMULATOR" prefix="NPS_"> <include href="conf/airframes/tudelft/rotwing_7kg_common.xml" />
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, aileron, flap, pusher, skew" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwing3" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="10"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
<checklist>
<item name="pic" type="text">Enter the PIC</item>
<item name="pac" type="text">Enter the PAC</item>
<item name="gcs" type="text">Enter the GCS op</item>
<item name="goal" type="text">Goal of the flight</item>
<item name="basic law">Location, airspace and weather</item>
<item name="RC Battery">Check the RC battery</item>
<item name="tail connection">Check tail connection</item>
<item name="wing tape">Check wings taped and secured</item>
<item name="attitude">Check attitude and heading</item>
<item name="takeoff location">Put UAV on take-off location</item>
<item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item>
<item name="camera">Switch on camera</item>
<item name="announce">Announce flight to other airspace users</item>
</checklist>
</airframe> </airframe>
conf/airframes/tudelft/rotwing_v3d.xml
+36 -318
View File
@@ -30,6 +30,7 @@
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/> <define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<!--Only send gyro and accel that is being used--> <!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/> <define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
@@ -56,6 +57,7 @@
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/> <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
<define name="USE_SONAR" value="1"/> <!-- Because uavcan can't handle simulation yet -->
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -70,7 +72,7 @@
</module> </module>
<module name="airspeed" type="ms45xx_i2c"> <module name="airspeed" type="ms45xx_i2c">
<configure name="MS45XX_I2C_DEV" value="i2c2"/> <configure name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.618426"/> <define name="MS45XX_PRESSURE_SCALE" value="1.90"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/> <define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/> <define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/> <define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
@@ -82,8 +84,12 @@
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it --> <define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module> </module>
<module name="air_data"/> <module name="air_data"/>
<configure name="PRIMARY_GPS" value="ublox"/>
<configure name="SECONDARY_GPS" value="ublox2"/>
<module name="gps" type="ublox"> <module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/> <configure name="UBX_GPS_BAUD" value="B460800"/>
<configure name="UBX2_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/> <define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module> </module>
@@ -92,6 +98,7 @@
<module name="ins" type="ekf2"/> <module name="ins" type="ekf2"/>
<module name="parachute"/> <module name="parachute"/>
<module name="ekf_aw"/>
<!-- Actuators on dual CAN bus --> <!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan"> <module name="actuators" type="uavcan">
@@ -128,73 +135,15 @@
<module name="pfc_actuators"/> <module name="pfc_actuators"/>
<module name="agl_dist"/> <module name="agl_dist"/>
<module name="approach_moving_target"/> <module name="approach_moving_target"/>
<!--Airspeed estimation using EKF-->
<module name="ekf_aw">
<define name="EKF_AW_WRAPPER_ROTWING_TYPE_A" value="0" description="1 for A, 0 for b"/>
<define name="EKF_AW_P0_V_BODY" value="1.E-2f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1.E-5f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1.E-10f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1.0E-4f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1.0E-9f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1.0E-6f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1.0E-8f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1.E-5f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1.E-5f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1.E-5f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1.E-5f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1.E-7f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_AZ_SCHED_GAIN" value="2" />
<define name="EKF_AW_AZ_SCHED_START_DEG" value="60" />
<define name="EKF_AW_AZ_SCHED_END_DEG" value="70" />
<define name="EKF_AW_AX_SCHED_GAIN" value="0" />
<define name="EKF_AW_AX_SCHED_START_DEG" value="45" />
<define name="EKF_AW_AX_SCHED_END_DEG" value="60" />
<define name="EKF_AW_QUICK_CONVERGENCE" value="true" />
<define name="EKF_AW_QUICK_CONVERGENCE_TIME" value="15" />
<define name="EKF_AW_WING_INSTALLED" value="true" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED_X" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Y" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Z" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_VEHICLE_MASS" value="6.5" description="Mass of the vehicle"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="-4E-2f" description="K*u*u"/>
<define name="EKF_AW_K3_FX_HOVER" value="-3E-1f" description="K*u"/>
<define name="EKF_AW_K1_FX_WING" value="-3.21432e-02f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FX_WING" value="1.67195e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FX_WING" value="5.9441e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FX_WING" value="3.9839e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K5_FX_WING" value="3.5321e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K_FY_V" value="-3.2E-1f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FY_WING" value="0.0f" description="Fy = K1*cos(skew)*sin(skew)*u^2"/>
<define name="EKF_AW_K1_FZ_WING" value="-1.0008e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FZ_WING" value="-8.6965e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FZ_WING" value="1.45783e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FZ_WING" value="2.185395e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_TELEMETRY_DEBUG" value="false" description="debug mode to send timing info through telemetry"/>
</module>
</firmware> </firmware>
<!-- Can bus 1 actuators --> <!-- Can bus 1 actuators -->
<servos driver="Uavcan1"> <servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="8191"/> <servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/> <servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/> <servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="8191"/> <servo no="4" name="MOTOR_PUSH" min="0" neutral="1000" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-2375" neutral="-172" max="2031"/> <servo no="5" name="ROTATION_MECH" min="-2375" neutral="-172" max="2031"/>
</servos> </servos>
@@ -206,10 +155,10 @@
<!-- Can bus 2 actuators --> <!-- Can bus 2 actuators -->
<servos driver="Uavcan2"> <servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="8191"/> <servo no="0" name="BMOTOR_FRONT" min="0" neutral="1000" max="7372"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/> <servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/> <servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/> <servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="5" name="BROTATION_MECH" min="-2375" neutral="-172" max="2031"/> <servo no="5" name="BROTATION_MECH" min="-2375" neutral="-172" max="2031"/>
</servos> </servos>
@@ -244,9 +193,9 @@
<command_laws> <command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/> <let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/> <let VAR="hover_off" VALUE="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<let var="ail_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 20)"/> <let var="ail_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 20)"/>
<let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/> <let var="flap_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 50)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/> <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<call fun="pfc_actuators_run()"/> <call fun="pfc_actuators_run()"/>
@@ -257,7 +206,7 @@
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/> <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/> <set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="ROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="ROTATION_MECH"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/> <set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
@@ -268,7 +217,7 @@
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="BROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="BROTATION_MECH"/>
</command_laws> </command_laws>
<section PREFIX="SYS_ID_" NAME="SYS_ID"> <section PREFIX="SYS_ID_" NAME="SYS_ID">
@@ -283,36 +232,7 @@
<define name="CHIRP_RADIO_CHANNEL" value="9"/> <define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section> </section>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_"> <section name="CALIBRATION">
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="MISC">
<!-- Voltage and current measurements --> <!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/> <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/> <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
@@ -429,37 +349,10 @@
<field name="low" value="-9600"/> <field name="low" value="-9600"/>
<field name="high" value="2000"/> <field name="high" value="2000"/>
<field name="low_feedback" value="0"/> <field name="low_feedback" value="0"/>
<field name="high_feedback" value="2200"/> <field name="high_feedback" value="3200"/>
<field name="timeout" value="3"/> <field name="timeout" value="3"/>
</field> </field>
</define> </define>
<!-- 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="200"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<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"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
@@ -473,193 +366,6 @@
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section> </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="5" 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 7 kg-->
<define name="G1_ROLL" value="{ 0.0, -15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.5, 0.0, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{- 0.3, 0.3, -0.3, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.55}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="TRUE"/>
<define name="FILT_CUTOFF_P" value="5.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.85"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="50" 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_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- 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.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="MAX_PUSHER_INCREMENT" value="4000"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="LINE_GAIN" value="0.2"/>
<define name="CLIMB_SPEED_FWD" value="2.0"/>
<define name="DESCEND_SPEED_FWD" value="-3.0"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
<define name="ACCEL_FWD_BX_LIM" value="3.0"/>
</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="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.1" />
<define name="SLOPE" value="50.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</section>
<section name="TARGET_POS">
<define name="TARGET_OFFSET_DISTANCE" value="20.0"/>
</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>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, aileron, flap, pusher, skew" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwing3" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="10"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
<checklist> <checklist>
<item name="pic" type="text">Enter the PIC</item> <item name="pic" type="text">Enter the PIC</item>
<item name="pac" type="text">Enter the PAC</item> <item name="pac" type="text">Enter the PAC</item>
@@ -676,7 +382,19 @@
<item name="actuators">Automated actuator check</item> <item name="actuators">Automated actuator check</item>
<item name="flight plan">Check flight plan</item> <item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item> <item name="flight block">Switch to correct flight block</item>
<item name="drone tag">Switch on drone tag</item>
<item name="camera">Switch on camera</item> <item name="camera">Switch on camera</item>
<item name="announce">Announce flight to other airspace users</item> <item name="announce">Announce flight to other airspace users</item>
</checklist> </checklist>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<include href="conf/airframes/tudelft/rotwing_7kg_common.xml" />
</airframe> </airframe>
conf/airframes/tudelft/rotwing_v3e.xml
+27 -309
View File
@@ -30,6 +30,7 @@
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/> <define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<!--Only send gyro and accel that is being used--> <!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/> <define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
@@ -56,6 +57,7 @@
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/> <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
<define name="USE_SONAR" value="1"/> <!-- Because uavcan can't handle simulation yet -->
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -82,8 +84,12 @@
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it --> <define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module> </module>
<module name="air_data"/> <module name="air_data"/>
<configure name="PRIMARY_GPS" value="ublox"/>
<configure name="SECONDARY_GPS" value="ublox2"/>
<module name="gps" type="ublox"> <module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/> <configure name="UBX_GPS_BAUD" value="B460800"/>
<configure name="UBX2_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/> <define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module> </module>
@@ -92,6 +98,7 @@
<module name="ins" type="ekf2"/> <module name="ins" type="ekf2"/>
<module name="parachute"/> <module name="parachute"/>
<module name="ekf_aw"/>
<!-- Actuators on dual CAN bus --> <!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan"> <module name="actuators" type="uavcan">
@@ -128,64 +135,6 @@
<module name="pfc_actuators"/> <module name="pfc_actuators"/>
<module name="agl_dist"/> <module name="agl_dist"/>
<module name="approach_moving_target"/> <module name="approach_moving_target"/>
<!--Airspeed estimation using EKF-->
<module name="ekf_aw">
<define name="EKF_AW_WRAPPER_ROTWING_TYPE_A" value="0" description="1 for A, 0 for b"/>
<define name="EKF_AW_P0_V_BODY" value="1.E-2f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1.E-5f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1.E-10f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1.0E-4f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1.0E-9f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1.0E-6f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1.0E-8f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1.E-5f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1.E-5f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1.E-5f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1.E-5f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1.E-7f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_AZ_SCHED_GAIN" value="2" />
<define name="EKF_AW_AZ_SCHED_START_DEG" value="60" />
<define name="EKF_AW_AZ_SCHED_END_DEG" value="70" />
<define name="EKF_AW_AX_SCHED_GAIN" value="0" />
<define name="EKF_AW_AX_SCHED_START_DEG" value="45" />
<define name="EKF_AW_AX_SCHED_END_DEG" value="60" />
<define name="EKF_AW_QUICK_CONVERGENCE" value="true" />
<define name="EKF_AW_QUICK_CONVERGENCE_TIME" value="15" />
<define name="EKF_AW_WING_INSTALLED" value="true" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED_X" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Y" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Z" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_VEHICLE_MASS" value="6.5" description="Mass of the vehicle"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="-4E-2f" description="K*u*u"/>
<define name="EKF_AW_K3_FX_HOVER" value="-3E-1f" description="K*u"/>
<define name="EKF_AW_K1_FX_WING" value="-3.21432e-02f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FX_WING" value="1.67195e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FX_WING" value="5.9441e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FX_WING" value="3.9839e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K5_FX_WING" value="3.5321e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K_FY_V" value="-3.2E-1f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FY_WING" value="0.0f" description="Fy = K1*cos(skew)*sin(skew)*u^2"/>
<define name="EKF_AW_K1_FZ_WING" value="-1.0008e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FZ_WING" value="-8.6965e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FZ_WING" value="1.45783e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FZ_WING" value="2.185395e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_TELEMETRY_DEBUG" value="false" description="debug mode to send timing info through telemetry"/>
</module>
</firmware> </firmware>
<!-- Can bus 1 actuators --> <!-- Can bus 1 actuators -->
@@ -194,7 +143,7 @@
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/> <servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/> <servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/> <servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/> <servo no="4" name="MOTOR_PUSH" min="0" neutral="1000" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-3629" neutral="66" max="3760"/> <servo no="5" name="ROTATION_MECH" min="-3629" neutral="66" max="3760"/>
</servos> </servos>
@@ -244,9 +193,9 @@
<command_laws> <command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/> <let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/> <let VAR="hover_off" VALUE="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<let var="ail_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 20)"/> <let var="ail_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 20)"/>
<let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/> <let var="flap_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 50)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/> <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<call fun="pfc_actuators_run()"/> <call fun="pfc_actuators_run()"/>
@@ -257,7 +206,7 @@
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/> <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/> <set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="ROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="ROTATION_MECH"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/> <set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
@@ -268,7 +217,7 @@
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="BROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="BROTATION_MECH"/>
</command_laws> </command_laws>
<section PREFIX="SYS_ID_" NAME="SYS_ID"> <section PREFIX="SYS_ID_" NAME="SYS_ID">
@@ -283,36 +232,7 @@
<define name="CHIRP_RADIO_CHANNEL" value="9"/> <define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section> </section>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_"> <section name="CALIBRATION">
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="MISC">
<!-- Voltage and current measurements --> <!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/> <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/> <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
@@ -429,37 +349,10 @@
<field name="low" value="-9600"/> <field name="low" value="-9600"/>
<field name="high" value="2000"/> <field name="high" value="2000"/>
<field name="low_feedback" value="0"/> <field name="low_feedback" value="0"/>
<field name="high_feedback" value="2200"/> <field name="high_feedback" value="3200"/>
<field name="timeout" value="3"/> <field name="timeout" value="3"/>
</field> </field>
</define> </define>
<!-- 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="200"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<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"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
@@ -473,193 +366,6 @@
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section> </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="5" 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 7 kg-->
<define name="G1_ROLL" value="{ 0.0, -15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.5, 0.0, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{- 0.3, 0.3, -0.3, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.55}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="TRUE"/>
<define name="FILT_CUTOFF_P" value="5.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.85"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="50" 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_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- 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.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="MAX_PUSHER_INCREMENT" value="4000"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="LINE_GAIN" value="0.2"/>
<define name="CLIMB_SPEED_FWD" value="2.0"/>
<define name="DESCEND_SPEED_FWD" value="-3.0"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
<define name="ACCEL_FWD_BX_LIM" value="3.0"/>
</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="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</section>
<section name="TARGET_POS">
<define name="TARGET_OFFSET_DISTANCE" value="20.0"/>
</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>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, aileron, flap, pusher, skew" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwing3" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="10"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
<checklist> <checklist>
<item name="pic" type="text">Enter the PIC</item> <item name="pic" type="text">Enter the PIC</item>
<item name="pac" type="text">Enter the PAC</item> <item name="pac" type="text">Enter the PAC</item>
@@ -676,7 +382,19 @@
<item name="actuators">Automated actuator check</item> <item name="actuators">Automated actuator check</item>
<item name="flight plan">Check flight plan</item> <item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item> <item name="flight block">Switch to correct flight block</item>
<item name="drone tag">Switch on drone tag</item>
<item name="camera">Switch on camera</item> <item name="camera">Switch on camera</item>
<item name="announce">Announce flight to other airspace users</item> <item name="announce">Announce flight to other airspace users</item>
</checklist> </checklist>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<include href="conf/airframes/tudelft/rotwing_7kg_common.xml" />
</airframe> </airframe>
conf/airframes/tudelft/rotwing_v3f.xml
+32 -314
View File
@@ -30,6 +30,7 @@
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/> <define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="MULTI_GPS_MODE" value="GPS_MODE_PRIMARY"/>
<!--Only send gyro and accel that is being used--> <!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/> <define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
@@ -56,6 +57,7 @@
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/> <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
<define name="USE_SONAR" value="1"/> <!-- Because uavcan can't handle simulation yet -->
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -70,7 +72,7 @@
</module> </module>
<module name="airspeed" type="ms45xx_i2c"> <module name="airspeed" type="ms45xx_i2c">
<configure name="MS45XX_I2C_DEV" value="i2c2"/> <configure name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.652116663"/> <define name="MS45XX_PRESSURE_SCALE" value="1.90"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/> <define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/> <define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/> <define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
@@ -82,8 +84,12 @@
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it --> <define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module> </module>
<module name="air_data"/> <module name="air_data"/>
<configure name="PRIMARY_GPS" value="ublox"/>
<configure name="SECONDARY_GPS" value="ublox2"/>
<module name="gps" type="ublox"> <module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/> <configure name="UBX_GPS_BAUD" value="B460800"/>
<configure name="UBX2_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/> <define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module> </module>
@@ -92,6 +98,7 @@
<module name="ins" type="ekf2"/> <module name="ins" type="ekf2"/>
<module name="parachute"/> <module name="parachute"/>
<module name="ekf_aw"/>
<!-- Actuators on dual CAN bus --> <!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan"> <module name="actuators" type="uavcan">
@@ -128,64 +135,6 @@
<module name="pfc_actuators"/> <module name="pfc_actuators"/>
<module name="agl_dist"/> <module name="agl_dist"/>
<module name="approach_moving_target"/> <module name="approach_moving_target"/>
<!--Airspeed estimation using EKF-->
<module name="ekf_aw">
<define name="EKF_AW_WRAPPER_ROTWING_TYPE_A" value="0" description="1 for A, 0 for b"/>
<define name="EKF_AW_P0_V_BODY" value="1.E-2f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1.E-5f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1.E-10f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1.0E-4f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1.0E-9f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1.0E-6f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1.0E-8f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1.E-5f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1.E-5f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1.E-5f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1.E-5f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1.E-7f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_AZ_SCHED_GAIN" value="2" />
<define name="EKF_AW_AZ_SCHED_START_DEG" value="60" />
<define name="EKF_AW_AZ_SCHED_END_DEG" value="70" />
<define name="EKF_AW_AX_SCHED_GAIN" value="0" />
<define name="EKF_AW_AX_SCHED_START_DEG" value="45" />
<define name="EKF_AW_AX_SCHED_END_DEG" value="60" />
<define name="EKF_AW_QUICK_CONVERGENCE" value="true" />
<define name="EKF_AW_QUICK_CONVERGENCE_TIME" value="15" />
<define name="EKF_AW_WING_INSTALLED" value="true" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED_X" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Y" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Z" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_VEHICLE_MASS" value="6.5" description="Mass of the vehicle"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="-4E-2f" description="K*u*u"/>
<define name="EKF_AW_K3_FX_HOVER" value="-3E-1f" description="K*u"/>
<define name="EKF_AW_K1_FX_WING" value="-3.21432e-02f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FX_WING" value="1.67195e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FX_WING" value="5.9441e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FX_WING" value="3.9839e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K5_FX_WING" value="3.5321e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K_FY_V" value="-3.2E-1f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FY_WING" value="0.0f" description="Fy = K1*cos(skew)*sin(skew)*u^2"/>
<define name="EKF_AW_K1_FZ_WING" value="-1.0008e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FZ_WING" value="-8.6965e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FZ_WING" value="1.45783e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FZ_WING" value="2.185395e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_TELEMETRY_DEBUG" value="false" description="debug mode to send timing info through telemetry"/>
</module>
</firmware> </firmware>
<!-- Can bus 1 actuators --> <!-- Can bus 1 actuators -->
@@ -194,14 +143,14 @@
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/> <servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/> <servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/> <servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/> <servo no="4" name="MOTOR_PUSH" min="0" neutral="1000" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-1769" neutral="66" max="1900"/> <servo no="5" name="ROTATION_MECH" min="-1769" neutral="66" max="1900"/>
</servos> </servos>
<!-- CAN BUS 1 command outputs--> <!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd"> <servos driver="Uavcan1Cmd">
<servo no="6" name="SERVO_ELEVATOR" min="3211" neutral="3211" max="-6102"/> <servo no="6" name="SERVO_ELEVATOR" min="3211" neutral="3211" max="-6102"/>
<servo no="7" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/> <servo no="7" name="SERVO_RUDDER" min="-4750" neutral="0" max="4750"/>
</servos> </servos>
<!-- Can bus 2 actuators --> <!-- Can bus 2 actuators -->
@@ -244,9 +193,9 @@
<command_laws> <command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/> <let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/> <let VAR="hover_off" VALUE="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<let var="ail_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 20)"/> <let var="ail_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 20)"/>
<let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/> <let var="flap_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 55)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/> <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<call fun="pfc_actuators_run()"/> <call fun="pfc_actuators_run()"/>
@@ -257,18 +206,18 @@
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/> <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/> <set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="ROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="ROTATION_MECH"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/> <set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? (RadioControlValues(RADIO_ROLL) - rw_flap_offset) : (actuators_pprz[7] - rw_flap_offset))" SERVO="FLAP_LEFT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(4, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_RIGHT"/> <set VALUE="$flap_limit_hit? pfc_actuators_value(4, 0) : ($servo_hold? (RadioControlValues(RADIO_ROLL) + rw_flap_offset) : (actuators_pprz[7] + rw_flap_offset))" SERVO="FLAP_RIGHT"/>
<!-- Backup commands --> <!-- Backup commands -->
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(7, -9600)) : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(7, -9600)) : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="BROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="BROTATION_MECH"/>
</command_laws> </command_laws>
<section PREFIX="SYS_ID_" NAME="SYS_ID"> <section PREFIX="SYS_ID_" NAME="SYS_ID">
@@ -283,36 +232,7 @@
<define name="CHIRP_RADIO_CHANNEL" value="9"/> <define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section> </section>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_"> <section name="CALIBRATION">
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="MISC">
<!-- Voltage and current measurements --> <!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/> <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/> <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
@@ -382,7 +302,7 @@
<field name="low" value="-9600"/> <field name="low" value="-9600"/>
<field name="high" value="9600"/> <field name="high" value="9600"/>
<field name="low_feedback" value="1.57"/> <field name="low_feedback" value="1.57"/>
<field name="high_feedback" value="0"/> <field name="high_feedback" value="0."/>
<field name="timeout" value="5"/> <field name="timeout" value="5"/>
</field> </field>
@@ -429,37 +349,10 @@
<field name="low" value="-9600"/> <field name="low" value="-9600"/>
<field name="high" value="2000"/> <field name="high" value="2000"/>
<field name="low_feedback" value="0"/> <field name="low_feedback" value="0"/>
<field name="high_feedback" value="2000"/> <field name="high_feedback" value="3000"/>
<field name="timeout" value="3"/> <field name="timeout" value="3"/>
</field> </field>
</define> </define>
<!-- 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="200"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<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"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
@@ -473,193 +366,6 @@
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section> </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="5" 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 7 kg-->
<define name="G1_ROLL" value="{ 0.0, -15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.5, 0.0, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{- 0.3, 0.3, -0.3, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.55}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="TRUE"/>
<define name="FILT_CUTOFF_P" value="5.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.85"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="50" 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_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- 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.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="MAX_PUSHER_INCREMENT" value="4000"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="LINE_GAIN" value="0.2"/>
<define name="CLIMB_SPEED_FWD" value="2.0"/>
<define name="DESCEND_SPEED_FWD" value="-3.0"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
<define name="ACCEL_FWD_BX_LIM" value="3.0"/>
</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="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</section>
<section name="TARGET_POS">
<define name="TARGET_OFFSET_DISTANCE" value="20.0"/>
</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>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, aileron, flap, pusher, skew" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwing3" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="10"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
<checklist> <checklist>
<item name="pic" type="text">Enter the PIC</item> <item name="pic" type="text">Enter the PIC</item>
<item name="pac" type="text">Enter the PAC</item> <item name="pac" type="text">Enter the PAC</item>
@@ -676,7 +382,19 @@
<item name="actuators">Automated actuator check</item> <item name="actuators">Automated actuator check</item>
<item name="flight plan">Check flight plan</item> <item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item> <item name="flight block">Switch to correct flight block</item>
<item name="drone tag">Switch on drone tag</item>
<item name="camera">Switch on camera</item> <item name="camera">Switch on camera</item>
<item name="announce">Announce flight to other airspace users</item> <item name="announce">Announce flight to other airspace users</item>
</checklist> </checklist>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<include href="conf/airframes/tudelft/rotwing_7kg_common.xml" />
</airframe> </airframe>
conf/airframes/tudelft/rotwing_v3g.xml
+40 -313
View File
@@ -57,6 +57,7 @@
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/> <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
<define name="USE_SONAR" value="1"/> <!-- Because uavcan can't handle simulation yet -->
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -71,7 +72,7 @@
</module> </module>
<module name="airspeed" type="ms45xx_i2c"> <module name="airspeed" type="ms45xx_i2c">
<configure name="MS45XX_I2C_DEV" value="i2c2"/> <configure name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.65"/> <define name="MS45XX_PRESSURE_SCALE" value="1.90"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/> <define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/> <define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/> <define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
@@ -97,6 +98,7 @@
<module name="ins" type="ekf2"/> <module name="ins" type="ekf2"/>
<module name="parachute"/> <module name="parachute"/>
<module name="ekf_aw"/>
<!-- Actuators on dual CAN bus --> <!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan"> <module name="actuators" type="uavcan">
@@ -133,64 +135,6 @@
<module name="pfc_actuators"/> <module name="pfc_actuators"/>
<module name="agl_dist"/> <module name="agl_dist"/>
<module name="approach_moving_target"/> <module name="approach_moving_target"/>
<!--Airspeed estimation using EKF-->
<module name="ekf_aw">
<define name="EKF_AW_WRAPPER_ROTWING_TYPE_A" value="0" description="1 for A, 0 for b"/>
<define name="EKF_AW_P0_V_BODY" value="1.E-2f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1.E-5f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1.E-10f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1.0E-4f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1.0E-9f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1.0E-6f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1.0E-8f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1.E-5f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1.E-5f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1.E-5f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1.E-5f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1.E-7f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_AZ_SCHED_GAIN" value="2" />
<define name="EKF_AW_AZ_SCHED_START_DEG" value="60" />
<define name="EKF_AW_AZ_SCHED_END_DEG" value="70" />
<define name="EKF_AW_AX_SCHED_GAIN" value="0" />
<define name="EKF_AW_AX_SCHED_START_DEG" value="45" />
<define name="EKF_AW_AX_SCHED_END_DEG" value="60" />
<define name="EKF_AW_QUICK_CONVERGENCE" value="true" />
<define name="EKF_AW_QUICK_CONVERGENCE_TIME" value="15" />
<define name="EKF_AW_WING_INSTALLED" value="true" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED_X" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Y" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Z" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_VEHICLE_MASS" value="6.5" description="Mass of the vehicle"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="-4E-2f" description="K*u*u"/>
<define name="EKF_AW_K3_FX_HOVER" value="-3E-1f" description="K*u"/>
<define name="EKF_AW_K1_FX_WING" value="-3.21432e-02f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FX_WING" value="1.67195e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FX_WING" value="5.9441e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FX_WING" value="3.9839e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K5_FX_WING" value="3.5321e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K_FY_V" value="-3.2E-1f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FY_WING" value="0.0f" description="Fy = K1*cos(skew)*sin(skew)*u^2"/>
<define name="EKF_AW_K1_FZ_WING" value="-1.0008e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FZ_WING" value="-8.6965e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FZ_WING" value="1.45783e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FZ_WING" value="2.185395e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_TELEMETRY_DEBUG" value="false" description="debug mode to send timing info through telemetry"/>
</module>
</firmware> </firmware>
<!-- Can bus 1 actuators --> <!-- Can bus 1 actuators -->
@@ -199,14 +143,14 @@
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/> <servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/> <servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/> <servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/> <servo no="4" name="MOTOR_PUSH" min="0" neutral="1000" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-1671" neutral="188" max="2048"/> <servo no="5" name="ROTATION_MECH" min="-1671" neutral="188" max="2048"/>
</servos> </servos>
<!-- CAN BUS 1 command outputs--> <!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd"> <servos driver="Uavcan1Cmd">
<servo no="6" name="SERVO_ELEVATOR" min="5400" neutral="5400" max="-4349"/> <servo no="6" name="SERVO_ELEVATOR" min="5400" neutral="5400" max="-4349"/>
<servo no="7" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/> <servo no="7" name="SERVO_RUDDER" min="-4750" neutral="0" max="4750"/>
</servos> </servos>
<!-- Can bus 2 actuators --> <!-- Can bus 2 actuators -->
@@ -249,9 +193,9 @@
<command_laws> <command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/> <let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/> <let VAR="hover_off" VALUE="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<let var="ail_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 20)"/> <let var="ail_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 20)"/>
<let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/> <let var="flap_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 50)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/> <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<call fun="pfc_actuators_run()"/> <call fun="pfc_actuators_run()"/>
@@ -262,7 +206,7 @@
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/> <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/> <set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="ROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="ROTATION_MECH"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/> <set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
@@ -273,7 +217,7 @@
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="BROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="BROTATION_MECH"/>
</command_laws> </command_laws>
<section PREFIX="SYS_ID_" NAME="SYS_ID"> <section PREFIX="SYS_ID_" NAME="SYS_ID">
@@ -288,36 +232,7 @@
<define name="CHIRP_RADIO_CHANNEL" value="9"/> <define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section> </section>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_"> <section name="CALIBRATION">
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="MISC">
<!-- Voltage and current measurements --> <!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/> <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/> <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
@@ -387,7 +302,7 @@
<field name="low" value="-9600"/> <field name="low" value="-9600"/>
<field name="high" value="9600"/> <field name="high" value="9600"/>
<field name="low_feedback" value="1.57"/> <field name="low_feedback" value="1.57"/>
<field name="high_feedback" value="0"/> <field name="high_feedback" value="0.0"/>
<field name="timeout" value="5"/> <field name="timeout" value="5"/>
</field> </field>
@@ -434,41 +349,28 @@
<field name="low" value="-9600"/> <field name="low" value="-9600"/>
<field name="high" value="2000"/> <field name="high" value="2000"/>
<field name="low_feedback" value="0"/> <field name="low_feedback" value="0"/>
<field name="high_feedback" value="2200"/> <field name="high_feedback" value="3200"/>
<field name="timeout" value="3"/> <field name="timeout" value="3"/>
</field> </field>
</define> </define>
<!-- Others -->
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<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"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
<define name="MAG_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="5"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
<field name="rotation" value="true"/>
</field>
<field name="neutral" value="1757,3096,1119" type="int[]"/>
<field name="scale" value="{{10462,24574,6961},{18175,43831,12532}}"/>
<field name="body_to_sensor" value="{{0,16384,0,16384,0,0,0,0,-16384}}"/>
</field>
</define>
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-19,0,28}, .scale={{1537,43219,6232},{157,4410,641}}, .filter_sample_freq=1127, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-1,2,33}, .scale={{21914,8531,5489},{4477,1738,1120}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-47,0,3}, .scale={{17288,29444,25808},{3531,6031,5293}}}}"/> <define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-19,0,28}, .scale={{1537,43219,6232},{157,4410,641}}, .filter_sample_freq=1127, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-1,2,33}, .scale={{21914,8531,5489},{4477,1738,1120}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-47,0,3}, .scale={{17288,29444,25808},{3531,6031,5293}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true, .rotation=true},.neutral={3163,2783,1623}, .scale={{25833,28413,12237},{45868,51206,22472}}, .body_to_sensor={{0,16384,0,16384,0,0,0,0,-16384}}}}"/>
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1127, .filter_freq=30}}"/> <define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1127, .filter_freq=30}}"/>
<!-- Define axis in hover frame --> <!-- Define axis in hover frame -->
@@ -477,193 +379,6 @@
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section> </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="5" 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 7 kg-->
<define name="G1_ROLL" value="{ 0.0, -15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.5, 0.0, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{- 0.3, 0.3, -0.3, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.55}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="TRUE"/>
<define name="FILT_CUTOFF_P" value="5.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.85"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="50" 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_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- 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.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="MAX_PUSHER_INCREMENT" value="4000"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="LINE_GAIN" value="0.2"/>
<define name="CLIMB_SPEED_FWD" value="2.0"/>
<define name="DESCEND_SPEED_FWD" value="-3.0"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
<define name="ACCEL_FWD_BX_LIM" value="3.0"/>
</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="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.1" />
<define name="SLOPE" value="50.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</section>
<section name="TARGET_POS">
<define name="TARGET_OFFSET_DISTANCE" value="20.0"/>
</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>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, aileron, flap, pusher, skew" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwing3" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="10"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
<checklist> <checklist>
<item name="pic" type="text">Enter the PIC</item> <item name="pic" type="text">Enter the PIC</item>
<item name="pac" type="text">Enter the PAC</item> <item name="pac" type="text">Enter the PAC</item>
@@ -680,7 +395,19 @@
<item name="actuators">Automated actuator check</item> <item name="actuators">Automated actuator check</item>
<item name="flight plan">Check flight plan</item> <item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item> <item name="flight block">Switch to correct flight block</item>
<item name="drone tag">Switch on drone tag</item>
<item name="camera">Switch on camera</item> <item name="camera">Switch on camera</item>
<item name="announce">Announce flight to other airspace users</item> <item name="announce">Announce flight to other airspace users</item>
</checklist> </checklist>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<include href="conf/airframes/tudelft/rotwing_7kg_common.xml" />
</airframe> </airframe>
conf/airframes/tudelft/rotwing_v3h.xml
+25 -309
View File
@@ -57,6 +57,7 @@
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
<define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/> <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
<define name="USE_SONAR" value="1"/> <!-- Because uavcan can't handle simulation yet -->
</target> </target>
<!-- Herelink datalink --> <!-- Herelink datalink -->
@@ -71,7 +72,7 @@
</module> </module>
<module name="airspeed" type="ms45xx_i2c"> <module name="airspeed" type="ms45xx_i2c">
<configure name="MS45XX_I2C_DEV" value="i2c2"/> <configure name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.65"/> <define name="MS45XX_PRESSURE_SCALE" value="1.90"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/> <define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/> <define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/> <define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
@@ -97,6 +98,7 @@
<module name="ins" type="ekf2"/> <module name="ins" type="ekf2"/>
<module name="parachute"/> <module name="parachute"/>
<module name="ekf_aw"/>
<!-- Actuators on dual CAN bus --> <!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan"> <module name="actuators" type="uavcan">
@@ -133,64 +135,6 @@
<module name="pfc_actuators"/> <module name="pfc_actuators"/>
<module name="agl_dist"/> <module name="agl_dist"/>
<module name="approach_moving_target"/> <module name="approach_moving_target"/>
<!--Airspeed estimation using EKF-->
<module name="ekf_aw">
<define name="EKF_AW_WRAPPER_ROTWING_TYPE_A" value="0" description="1 for A, 0 for b"/>
<define name="EKF_AW_P0_V_BODY" value="1.E-2f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1.E-5f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1.E-10f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1.0E-4f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1.0E-9f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1.0E-6f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1.0E-8f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1.E-5f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1.E-5f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1.E-5f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1.E-5f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1.E-7f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_AZ_SCHED_GAIN" value="2" />
<define name="EKF_AW_AZ_SCHED_START_DEG" value="60" />
<define name="EKF_AW_AZ_SCHED_END_DEG" value="70" />
<define name="EKF_AW_AX_SCHED_GAIN" value="0" />
<define name="EKF_AW_AX_SCHED_START_DEG" value="45" />
<define name="EKF_AW_AX_SCHED_END_DEG" value="60" />
<define name="EKF_AW_QUICK_CONVERGENCE" value="true" />
<define name="EKF_AW_QUICK_CONVERGENCE_TIME" value="15" />
<define name="EKF_AW_WING_INSTALLED" value="true" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED_X" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Y" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Z" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_VEHICLE_MASS" value="6.5" description="Mass of the vehicle"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="-4E-2f" description="K*u*u"/>
<define name="EKF_AW_K3_FX_HOVER" value="-3E-1f" description="K*u"/>
<define name="EKF_AW_K1_FX_WING" value="-3.21432e-02f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FX_WING" value="1.67195e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FX_WING" value="5.9441e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FX_WING" value="3.9839e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K5_FX_WING" value="3.5321e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K_FY_V" value="-3.2E-1f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FY_WING" value="0.0f" description="Fy = K1*cos(skew)*sin(skew)*u^2"/>
<define name="EKF_AW_K1_FZ_WING" value="-1.0008e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FZ_WING" value="-8.6965e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FZ_WING" value="1.45783e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FZ_WING" value="2.185395e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_TELEMETRY_DEBUG" value="false" description="debug mode to send timing info through telemetry"/>
</module>
</firmware> </firmware>
<!-- Can bus 1 actuators --> <!-- Can bus 1 actuators -->
@@ -199,14 +143,14 @@
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/> <servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/> <servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/> <servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="7372"/> <servo no="4" name="MOTOR_PUSH" min="0" neutral="1000" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-1392" neutral="451" max="2294"/> <servo no="5" name="ROTATION_MECH" min="-1392" neutral="451" max="2294"/>
</servos> </servos>
<!-- CAN BUS 1 command outputs--> <!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd"> <servos driver="Uavcan1Cmd">
<servo no="6" name="SERVO_ELEVATOR" min="1200" neutral="1200" max="-8191"/> <servo no="6" name="SERVO_ELEVATOR" min="1200" neutral="1200" max="-8191"/>
<servo no="7" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/> <servo no="7" name="SERVO_RUDDER" min="-4750" neutral="0" max="4750"/>
</servos> </servos>
<!-- Can bus 2 actuators --> <!-- Can bus 2 actuators -->
@@ -249,9 +193,9 @@
<command_laws> <command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/> <let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/> <let VAR="hover_off" VALUE="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<let var="ail_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 20)"/> <let var="ail_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 20)"/>
<let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/> <let var="flap_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 50)"/>
<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/> <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<call fun="pfc_actuators_run()"/> <call fun="pfc_actuators_run()"/>
@@ -262,7 +206,7 @@
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : pfc_actuators_value(1, actuators_pprz[4]))" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/> <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? pfc_actuators_value(0, 0) : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/> <set VALUE="($th_hold? ($servo_hold? -9600 : pfc_actuators_value(11, -9600)) : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="ROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="ROTATION_MECH"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(2, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/> <set VALUE="$ail_limit_hit? pfc_actuators_value(5, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/> <set VALUE="$flap_limit_hit? pfc_actuators_value(3, 0) : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
@@ -273,7 +217,7 @@
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(8, -9600)) : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(9, -9600)) : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
<set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/> <set VALUE="($hover_off? ($servo_hold? -9600 : pfc_actuators_value(10, -9600)) : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
<set VALUE="pfc_actuators_value(6, rotwing_state_skewing.servo_pprz_cmd)" SERVO="BROTATION_MECH"/> <set VALUE="pfc_actuators_value(6, rotwing_state.skew_cmd)" SERVO="BROTATION_MECH"/>
</command_laws> </command_laws>
<section PREFIX="SYS_ID_" NAME="SYS_ID"> <section PREFIX="SYS_ID_" NAME="SYS_ID">
@@ -288,36 +232,7 @@
<define name="CHIRP_RADIO_CHANNEL" value="9"/> <define name="CHIRP_RADIO_CHANNEL" value="9"/>
</section> </section>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_"> <section name="CALIBRATION">
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="MISC">
<!-- Voltage and current measurements --> <!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/> <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/> <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
@@ -434,36 +349,10 @@
<field name="low" value="-9600"/> <field name="low" value="-9600"/>
<field name="high" value="2000"/> <field name="high" value="2000"/>
<field name="low_feedback" value="0"/> <field name="low_feedback" value="0"/>
<field name="high_feedback" value="2000"/> <field name="high_feedback" value="3000"/>
<field name="timeout" value="3"/> <field name="timeout" value="3"/>
</field> </field>
</define> </define>
<!-- Others -->
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<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"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="INS_EKF2_GPS_ANTENNA_DISTANCE" value="1.02"/>
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
@@ -477,191 +366,6 @@
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section> </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="5" 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 7 kg-->
<define name="G1_ROLL" value="{ 0.0, -15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 1.5, 0.0, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_YAW" value="{- 0.3, 0.3, -0.3, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST" value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_THRUST_X" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.55}"/>
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>
<!-- Actuator dynamics -->
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<!-- Reference -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="70.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="9.0"/>
<define name="REF_RATE_R" value="3.9"/>
<define name="MAX_R" value="30.0" unit="deg/s"/>
<!-- Filters -->
<define name="FILTER_RATES_SECOND_ORDER" value="TRUE"/>
<define name="FILT_CUTOFF_P" value="5.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
<define name="YAW_DISTURBANCE_LIMIT" value="0.85"/>
</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="-4.0"/> <!-- climb -->
<define name="REF_MAX_ZD" value="4.0"/> <!-- descend -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="50" 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_">
<!--WLS settings-->
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{100., 300., 4., 30.}"/>
<!-- 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.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="19.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="NAV_CIRCLE_DIST" value="60."/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="LINE_GAIN" value="0.2"/>
<define name="CLIMB_SPEED_FWD" value="2.0"/>
<define name="DESCEND_SPEED_FWD" value="-3.0"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="PUSHER_INDEX" value="8"/>
<define name="MAX_PITCH" value="12"/>
<define name="MIN_PITCH" value="-20"/>
<define name="MAX_LAT_ACCEL" value="5.0"/>
<define name="ACCEL_FWD_BX_LIM" value="3.0"/>
</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="AMT" prefix="APPROACH_MOVING_TARGET_">
<define name="CUTOFF_FREQ_FILTERS_HZ" value="0.25"/>
<define name="ERR_SLOWDOWN_GAIN" value="0.25" />
<define name="SLOPE" value="60.0"/>
<define name="DISTANCE" value="70.0"/>
<define name="SPEED" value="0.0"/>
</section>
<section name="TARGET_POS">
<define name="TARGET_OFFSET_DISTANCE" value="20.0"/>
</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>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, rudder, elevator, aileron, flap, pusher, skew" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwing3" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="10"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
<checklist> <checklist>
<item name="pic" type="text">Enter the PIC</item> <item name="pic" type="text">Enter the PIC</item>
<item name="pac" type="text">Enter the PAC</item> <item name="pac" type="text">Enter the PAC</item>
@@ -678,7 +382,19 @@
<item name="actuators">Automated actuator check</item> <item name="actuators">Automated actuator check</item>
<item name="flight plan">Check flight plan</item> <item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item> <item name="flight block">Switch to correct flight block</item>
<item name="drone tag">Switch on drone tag</item>
<item name="camera">Switch on camera</item> <item name="camera">Switch on camera</item>
<item name="announce">Announce flight to other airspace users</item> <item name="announce">Announce flight to other airspace users</item>
</checklist> </checklist>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<include href="conf/airframes/tudelft/rotwing_7kg_common.xml" />
</airframe> </airframe>
conf/flight_plans/tudelft/rotating_wing_EHR8.xml
+24 -23
View File
@@ -53,34 +53,35 @@
</exceptions> </exceptions>
<blocks> <blocks>
<block name="Wait GPS"> <block name="Wait GPS">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/> <while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block> </block>
<block name="Geo init"> <block name="Geo init">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/> <while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetGroundReferenceHere()"/> <call_once fun="NavSetGroundReferenceHere()"/>
</block> </block>
<block name="Holding point"> <block name="Holding point">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/> <set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png"> <block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<exception cond="stage_time > 10" deroute="Holding point"/> <exception cond="stage_time > 10" deroute="Holding point"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<call_once fun="NavResurrect()"/> <call_once fun="NavResurrect()"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/> <attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
</block> </block>
<block name="Wait takeoff"> <block name="Wait takeoff">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="stage_time > 10" deroute="Holding point"/> <exception cond="stage_time > 10" deroute="Holding point"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 5.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 5.0) @AND (stage_time > 2)" vmode="throttle"/> <attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 5.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 5.0) @AND (stage_time > 2)" vmode="throttle"/>
</block> </block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png"> <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/> <exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/> <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set var="stage_timer_msec" value="get_sys_time_msec()"/> <set var="stage_timer_msec" value="get_sys_time_msec()"/>
@@ -92,31 +93,31 @@
</block> </block>
<block name="Climb"> <block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/> <exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/> <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block> </block>
<block name="Standby" strip_button="Standby" strip_icon="home.png"> <block name="Standby" strip_button="Standby" strip_icon="home.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="STDBY"/> <stay wp="STDBY"/>
</block> </block>
<block name="Approach APP"> <block name="Approach APP">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="Approach Flare"/> <exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="Approach Flare"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/> <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> </block>
<block name="Approach Flare"> <block name="Approach Flare">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/> <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/> <exception cond="ground_detect()" deroute="Holding point"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/> <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> </block>
<!-- <block name="follow_module"> <!-- <block name="follow_module">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/> <stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
</block> --> </block> -->
<block name="route"> <block name="route">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go from="p1" hmode="route" wp="p2"/> <go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/> <go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/> <go from="p3" hmode="route" wp="p4"/>
@@ -124,7 +125,7 @@
<deroute block="route"/> <deroute block="route"/>
</block> </block>
<block name="small_route" strip_button="Route" strip_icon="path.png"> <block name="small_route" strip_button="Route" strip_icon="path.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p2"/> <go wp="p2"/>
<go wp="p3"/> <go wp="p3"/>
<go wp="p4"/> <go wp="p4"/>
@@ -132,47 +133,47 @@
<deroute block="small_route"/> <deroute block="small_route"/>
</block> </block>
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png"> <block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="100" wp="circ"/> <circle radius="100" wp="circ"/>
</block> </block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png"> <block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-100" wp="circ"/> <circle radius="-100" wp="circ"/>
</block> </block>
<block name="big_Circle_CW_fwd"> <block name="big_Circle_CW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="150" wp="circ"/> <circle radius="150" wp="circ"/>
</block> </block>
<block name="big_Circle_CCW_fwd"> <block name="big_Circle_CCW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-150" wp="circ"/> <circle radius="-150" wp="circ"/>
</block> </block>
<block name="Transition_quad" strip_button="Transition Quad" strip_icon="wp_quad.png"> <block name="Transition_quad" strip_button="Transition Quad" strip_icon="wp_quad.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<go wp="STDBY"/> <go wp="STDBY"/>
<deroute block="Standby"/> <deroute block="Standby"/>
</block> </block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png"> <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
</block> </block>
<block name="land"> <block name="land">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="TD"/> <go wp="TD"/>
</block> </block>
<block name="descend" strip_button="Descend" strip_icon="descend.png"> <block name="descend" strip_button="Descend" strip_icon="descend.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/> <exception cond="GetPosHeight() @LT 12.0" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/> <stay climb="-1.0" vmode="climb" wp="TD"/>
</block> </block>
<block name="flare"> <block name="flare">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/> <stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>--> <!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/> <exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
</block> </block>
<block name="flare_low"> <block name="flare_low">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="NavDetectGround()" deroute="Holding point"/> <exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/> <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/> <exception cond="ground_detect()" deroute="Holding point"/>
@@ -180,7 +181,7 @@
<stay climb="-0.5" vmode="climb" wp="TD"/> <stay climb="-0.5" vmode="climb" wp="TD"/>
</block> </block>
<block name="landed"> <block name="landed">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
</blocks> </blocks>
conf/flight_plans/tudelft/rotating_wing_EHVB.xml
+33 -50
View File
@@ -1,5 +1,5 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd"> <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="110" ground_alt="0" lat0="52.1681551" lon0="4.4126468" max_dist_from_home="1070" name="Rotating Wing Valkenburg" security_height="2"> <flight_plan alt="100" ground_alt="0" lat0="52.1681551" lon0="4.4126468" max_dist_from_home="1070" name="Rotating Wing Valkenburg" security_height="2">
<header/> <header/>
<waypoints> <waypoints>
<waypoint name="HOME" x="0" y="0"/> <waypoint name="HOME" x="0" y="0"/>
@@ -53,34 +53,35 @@
</exceptions> </exceptions>
<blocks> <blocks>
<block name="Wait GPS"> <block name="Wait GPS">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/> <while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block> </block>
<block name="Geo init"> <block name="Geo init">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/> <while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/> <call_once fun="NavSetAltitudeReferenceHere()"/>
</block> </block>
<block name="Holding point"> <block name="Holding point">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/> <set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png"> <block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<exception cond="stage_time > 10" deroute="Holding point"/> <exception cond="stage_time > 10" deroute="Holding point"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<call_once fun="NavResurrect()"/> <call_once fun="NavResurrect()"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/> <attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
</block> </block>
<block name="Wait takeoff"> <block name="Wait takeoff">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="stage_time > 10" deroute="Holding point"/> <exception cond="stage_time > 10" deroute="Holding point"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 3.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 3.0) @AND (stage_time > 2)" vmode="throttle"/> <attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 5.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 5.0) @AND (stage_time > 2)" vmode="throttle"/>
</block> </block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png"> <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/> <exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/> <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set var="stage_timer_msec" value="get_sys_time_msec()"/> <set var="stage_timer_msec" value="get_sys_time_msec()"/>
@@ -94,52 +95,44 @@
</block> </block>
<block name="Climb"> <block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/> <exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/> <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block> </block>
<block name="Standby" strip_button="Standby" strip_icon="home.png"> <block name="Standby" strip_button="Standby" strip_icon="home.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="STDBY"/>
</block>
<block name="Standby_hybrid">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<stay wp="STDBY"/> <stay wp="STDBY"/>
</block> </block>
<block name="Standby_free"> <block name="Standby_free">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_FREE)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/> <stay wp="STDBY"/>
</block> </block>
<block name="stay_p1" strip_button="Stay P1" strip_icon="wp_quad.png"> <block name="stay_p1" strip_button="Stay P1" strip_icon="wp_quad.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p1"/> <stay wp="p1"/>
</block> </block>
<block name="stay_p2"> <block name="stay_p2">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p2"/> <stay wp="p2"/>
</block> </block>
<block name="p1_p2"> <block name="p1_p2">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="p1"/> <go wp="p1"/>
<stay until="stage_time>5" wp="p1"/> <stay until="stage_time>5" wp="p1"/>
<go wp="p2"/> <go wp="p2"/>
<stay until="stage_time>5" wp="p2"/> <stay until="stage_time>5" wp="p2"/>
<deroute block="p1_p2"/> <deroute block="p1_p2"/>
</block> </block>
<!-- <block name="Standby_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<stay wp="STDBY"/>
</block> -->
<!-- <block name="Approach APP"> <!-- <block name="Approach APP">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/> <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> --> </block> -->
<!-- <block name="follow_module"> <!-- <block name="follow_module">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/> <stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
</block> --> </block> -->
<!-- <block name="route"> <!-- <block name="route">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go from="p1" hmode="route" wp="p2"/> <go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/> <go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/> <go from="p3" hmode="route" wp="p4"/>
@@ -147,7 +140,7 @@
<deroute block="route"/> <deroute block="route"/>
</block> --> </block> -->
<!-- <block name="small_route" strip_button="Route" strip_icon="path.png"> <!-- <block name="small_route" strip_button="Route" strip_icon="path.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p2"/> <go wp="p2"/>
<go wp="p3"/> <go wp="p3"/>
<go wp="p4"/> <go wp="p4"/>
@@ -155,59 +148,49 @@
<deroute block="small_route"/> <deroute block="small_route"/>
</block> --> </block> -->
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png"> <block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="nav.radius" wp="circ"/> <circle radius="nav.radius" wp="circ"/>
</block> </block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png"> <block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-nav.radius" wp="circ"/> <circle radius="-nav.radius" wp="circ"/>
</block> </block>
<block name="Circle_CW_hybrid" strip_icon="circle-right.png"> <block name="Circle_Auto_fwd" strip_button="CircleAuto" strip_icon="circle-left.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/> <!-- <call_once var="circle_direction" fun="rotwing_state_choose_circle_direction(WP_circ)"/> -->
<circle radius="nav.radius" wp="circ"/> <!-- <circle radius="-nav.radius" wp="circ"/> -->
<exception cond="!rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CW_fwd"/>
<exception cond="rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CCW_fwd"/>
</block> </block>
<block name="Circle_CCW_hybrid" strip_icon="circle-left.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<circle radius="-nav.radius" wp="circ"/>
</block>
<!-- <block name="big_Circle_CW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="150" wp="circ"/>
</block>
<block name="big_Circle_CCW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="-150" wp="circ"/>
</block> -->
<block name="Approach APP"> <block name="Approach APP">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/> <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> </block>
<block name="Transition_quad" strip_button="Transition Quad" strip_icon="wp_quad.png"> <block name="Transition_quad" strip_button="Transition Quad" strip_icon="wp_quad.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<go wp="STDBY"/> <go wp="STDBY"/>
<deroute block="Standby"/> <deroute block="Standby"/>
</block> </block>
<block name="land here"> <block name="land here">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
</block> </block>
<block name="land" strip_button="Land" strip_icon="land-right.png"> <block name="land" strip_button="Land" strip_icon="land-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="TD"/> <go wp="TD"/>
</block> </block>
<block name="descend" strip_button="Descend" strip_icon="descend.png"> <block name="descend" strip_button="Descend" strip_icon="descend.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/> <exception cond="GetPosHeight() @LT 12.0" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/> <stay climb="-1.0" vmode="climb" wp="TD"/>
</block> </block>
<block name="flare"> <block name="flare">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/> <stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>--> <!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/> <exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
</block> </block>
<block name="flare_low"> <block name="flare_low">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<!-- <exception cond="NavDetectGround()" deroute="Holding point"/> --> <!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
<exception cond="!nav_is_in_flight()" deroute="Holding point"/> <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/> <exception cond="ground_detect()" deroute="Holding point"/>
@@ -215,7 +198,7 @@
<stay climb="-0.5" vmode="climb" wp="TD"/> <stay climb="-0.5" vmode="climb" wp="TD"/>
</block> </block>
<block name="landed"> <block name="landed">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
</blocks> </blocks>
conf/flight_plans/tudelft/rotating_wing_terschelling.xml
-166
View File
@@ -1,166 +0,0 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="70" ground_alt="0" lat0="53.3988083" lon0="5.2487480" max_dist_from_home="2000" name="Rotating Wing Terschelling" security_height="2">
<header/>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="CLIMB" x="300." y="70"/>
<waypoint name="STDBY" x="-60" y="5"/>
<waypoint name="p1" x="-65" y="80"/>
<waypoint name="p2" x="-120" y="480"/>
<waypoint name="p3" x="-400" y="400"/>
<waypoint name="p4" x="-150" y="70"/>
<waypoint name="circ" x="160" y="25"/>
<waypoint name="TD" x="10" y="-1"/>
<waypoint name="APP" x="-200" y="150"/>
<waypoint name="FOLLOW" x="300" y="80"/>
<!-- Terschelling -->
<waypoint lat="53.403590" lon="5.214102" name="C1"/>
<waypoint lat="53.399129" lon="5.219031" name="C2"/>
<waypoint lat="53.391752" lon="5.234877" name="C3"/>
<waypoint lat="53.399786" lon="5.273960" name="C4"/>
<waypoint lat="53.416200" lon="5.265441" name="C5"/>
<waypoint lat="53.424575" lon="5.252654" name="C6"/>
<waypoint lat="53.422246" lon="5.233265" name="C7"/>
<waypoint lat="53.418859" lon="5.216361" name="C8"/>
<waypoint lat="53.411762" lon="5.208801" 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">
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_p2,WP_p3,WP_p4,WP_STDBY,WP_circ,WP_APP"/>
</module> -->
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosHeight() @GT 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">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<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">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
</block>
<block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="STDBY"/>
</block>
<block name="Approach APP">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<!-- <block name="follow_module">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
</block> -->
<block name="route">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<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="small_route" strip_button="Route" strip_icon="path.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<go wp="p2"/>
<go wp="p3"/>
<go wp="p4"/>
<go wp="p1"/>
<deroute block="small_route"/>
</block>
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="100" wp="circ"/>
</block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="-100" wp="circ"/>
</block>
<block name="big_Circle_CW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="150" wp="circ"/>
</block>
<block name="big_Circle_CCW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="-150" wp="circ"/>
</block>
<block name="Transition_quad">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<go wp="TD"/>
</block>
<block name="descend">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
</block>
<block name="flare_low">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/tudelft/rotating_wing_tharde.xml
+22 -22
View File
@@ -40,28 +40,28 @@
</exceptions> </exceptions>
<blocks> <blocks>
<block name="Wait GPS"> <block name="Wait GPS">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/> <while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block> </block>
<block name="Geo init"> <block name="Geo init">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/> <while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/> <call_once fun="NavSetAltitudeReferenceHere()"/>
</block> </block>
<block name="Holding point"> <block name="Holding point">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/> <set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png"> <block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavResurrect()"/> <call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block> </block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png"> <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/> <exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough --> <call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
@@ -70,24 +70,24 @@
</block> </block>
<block name="Climb"> <block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/> <exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/> <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block> </block>
<block name="Standby" strip_button="Standby" strip_icon="home.png"> <block name="Standby" strip_button="Standby" strip_icon="home.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="STDBY"/> <stay wp="STDBY"/>
</block> </block>
<block name="Approach APP"> <block name="Approach APP">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/> <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> </block>
<!-- <block name="follow_module"> <!-- <block name="follow_module">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/> <stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
</block> --> </block> -->
<block name="route"> <block name="route">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go from="p1" hmode="route" wp="p2"/> <go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/> <go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/> <go from="p3" hmode="route" wp="p4"/>
@@ -95,7 +95,7 @@
<deroute block="route"/> <deroute block="route"/>
</block> </block>
<block name="small_route" strip_button="Route" strip_icon="path.png"> <block name="small_route" strip_button="Route" strip_icon="path.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p2"/> <go wp="p2"/>
<go wp="p3"/> <go wp="p3"/>
<go wp="p4"/> <go wp="p4"/>
@@ -103,47 +103,47 @@
<deroute block="small_route"/> <deroute block="small_route"/>
</block> </block>
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png"> <block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="100" wp="circ"/> <circle radius="100" wp="circ"/>
</block> </block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png"> <block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-100" wp="circ"/> <circle radius="-100" wp="circ"/>
</block> </block>
<block name="big_Circle_CW_fwd"> <block name="big_Circle_CW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="150" wp="circ"/> <circle radius="150" wp="circ"/>
</block> </block>
<block name="big_Circle_CCW_fwd"> <block name="big_Circle_CCW_fwd">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-150" wp="circ"/> <circle radius="-150" wp="circ"/>
</block> </block>
<block name="Transition_quad"> <block name="Transition_quad">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<go wp="STDBY"/> <go wp="STDBY"/>
<deroute block="Standby"/> <deroute block="Standby"/>
</block> </block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png"> <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
</block> </block>
<block name="land"> <block name="land">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="TD"/> <go wp="TD"/>
</block> </block>
<block name="descend"> <block name="descend">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/> <exception cond="GetPosHeight() @LT 12.0" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/> <stay climb="-1.0" vmode="climb" wp="TD"/>
</block> </block>
<block name="flare"> <block name="flare">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/> <stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>--> <!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/> <exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
</block> </block>
<block name="flare_low"> <block name="flare_low">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="NavDetectGround()" deroute="Holding point"/> <exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/> <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/> <exception cond="ground_detect()" deroute="Holding point"/>
@@ -151,7 +151,7 @@
<stay climb="-0.5" vmode="climb" wp="TD"/> <stay climb="-0.5" vmode="climb" wp="TD"/>
</block> </block>
<block name="landed"> <block name="landed">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
</blocks> </blocks>
conf/flight_plans/tudelft/rotating_wing_troia.xml
+309
View File
@@ -0,0 +1,309 @@
<?xml version="1.0"?>
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="115" ground_alt="15" lat0="38.474472222222225" lon0="-8.871222222222222" max_dist_from_home="80000" name="Rotating wing Troia" security_height="30">
<waypoints>
<waypoint lat="38.4874248" lon="-8.8691705" name="1-1"/>
<waypoint lat="38.4734375" lon="-8.8541523" name="1-2"/>
<waypoint lat="38.4690399" lon="-8.8614329" name="1-3"/>
<waypoint lat="38.4630730" lon="-8.8715400" name="1-4"/>
<waypoint lat="38.4701413" lon="-8.8785666" name="1-5"/>
<waypoint lat="38.4779321" lon="-8.8854282" name="1-6"/>
<waypoint lat="38.4796216" lon="-8.8819603" name="1-7"/>
<waypoint lat="38.4821792" lon="-8.8775063" name="1-8"/>
<waypoint lat="38.4831320" lon="-8.8691903" name="1-1_soft"/>
<waypoint lat="38.4742608" lon="-8.8598735" name="1-2_soft"/>
<waypoint lat="38.4710995" lon="-8.8649587" name="1-3_soft"/>
<waypoint lat="38.4676940" lon="-8.8708800" name="1-4_soft"/>
<waypoint lat="38.4767258" lon="-8.8799233" name="1-5_soft"/>
<waypoint lat="38.486388" lon="-8.868333" name="sado_e_1"/>
<waypoint lat="38.511111" lon="-8.84444" name="sado_e_2"/>
<waypoint lat="38.485" lon="-8.834166" name="sado_e_3"/>
<waypoint lat="38.47305555" lon="-8.83027777" name="sado_e_4"/>
<waypoint lat="38.47305555" lon="-8.85555555" name="sado_e_5"/>
<waypoint lat="38.4863888" lon="-8.8683333" name="sado_e_6"/>
<!-- <waypoint lat="38.468493" lon="-8.886115" name="2-1_soft"
38.4863888888889 -8.86833333333333
38.5111111111111 -8.84444444444445
38.485 -8.83416666666667
38.4730555555556 -8.83027777777778
38.4730555555556 -8.85555555555556
38.4863888888889 -8.86833333333333
/>
<waypoint lat="38.467498" lon="-8.884860" name="2-2_soft"/>
<waypoint lat="38.475621" lon="-8.874463" name="2-4_soft"/>
<waypoint lat="38.468610" lon="-8.877984" name="2-3_soft"/> -->
<!-- <waypoint lat="38.499227" lon="-8.858617" name="3-1"/>
<waypoint lat="38.476154" lon="-8.833249" name="3-2"/>
<waypoint lat="38.469040" lon="-8.861433" name="3-3"/>
<waypoint lat="38.467811" lon="-8.879136" name="3-4"/>
<waypoint lat="38.464900" lon="-8.884918" name="3-5"/>
<waypoint lat="38.471784" lon="-8.899161" name="3-6"/>
<waypoint lat="38.478446" lon="-8.885780" name="3-7"/>
<waypoint lat="38.480130" lon="-8.886261" name="3-8"/>
<waypoint lat="38.493038" lon="-8.858636" name="3-1_soft"/>
<waypoint lat="38.478620" lon="-8.845008" name="3-2_soft"/>
<waypoint lat="38.470562" lon="-8.867834" name="3-3_soft"/>
<waypoint lat="38.476390" lon="-8.887219" name="3-5_soft"/>
<waypoint lat="38.468803" lon="-8.880064" name="3-4_soft"/> -->
<waypoint lat="38.4" lon="-9.608888888888888" name="6-1"/>
<waypoint lat="38.409166666666664" lon="-9.196944444444444" name="6-2"/>
<waypoint lat="38.434444444444445" lon="-9.116666666666667" name="6-3"/>
<waypoint lat="38.43361111111111" lon="-9.054722222222223" name="6-4"/>
<waypoint lat="38.48777777777778" lon="-8.93361111111111" name="6-5"/>
<waypoint lat="38.46722222222222" lon="-8.885555555555555" name="6-6"/>
<waypoint lat="38.44444444444444" lon="-8.854722222222222" name="6-7"/>
<waypoint lat="38.30027777777778" lon="-8.854722222222222" name="6-8"/>
<waypoint lat="38.30027777777778" lon="-8.850833333333334" name="6-9"/>
<waypoint lat="38.215833333333336" lon="-8.850277777777778" name="6-10"/>
<waypoint lat="38.16833333333334" lon="-9.001666666666667" name="6-11"/>
<waypoint lat="38.16694444444445" lon="-9.198888888888888" name="6-12"/>
<waypoint lat="38.000277777777775" lon="-9.200555555555555" name="6-13"/>
<waypoint lat="38.000277777777775" lon="-9.450277777777778" name="6-14"/>
<waypoint name="HOME" x="111.225" y="-35.761"/>
<waypoint name="CLIMB" x="137.087" y="156.759"/>
<waypoint name="STDBY" lat="38.4746" lon="-8.86917"/>
<waypoint name="circ" lat="38.4751" lon="-8.86715"/>
<waypoint name="TD" x="111.497" y="-30.473" height="30"/>
<!-- <waypoint name="APP" x="70" y="-25"/> -->
<!-- <waypoint name="FOLLOW" x="300" y="80"/> -->
<waypoint name="p1" x="171.84" y="83.954"/>
<waypoint name="p2" x="419.479" y="282.12"/>
<waypoint name="p3" x="641.263" y="14.065"/>
<waypoint name="p4" x="385.379" y="-204.223"/>
<waypoint name="Downwind" x="170.2" y="209.1" height="50"/>
<waypoint name="Base" x="534.285" y="-58.323" height="40"/>
<waypoint name="Final" x="257.543" y="-180.466" height="30"/>
<waypoint name="ML_global_target" x="397.8" y="156.359"/>
</waypoints>
<sectors>
<sector color="red" name="A1">
<corner name="1-1"/>
<corner name="1-2"/>
<corner name="1-3"/>
<corner name="1-4"/>
<corner name="1-5"/>
<corner name="1-6"/>
<corner name="1-7"/>
<corner name="1-8"/>
</sector>
<sector color="orange" name="A1_soft">
<corner name="1-1_soft"/>
<corner name="1-2_soft"/>
<corner name="1-3_soft"/>
<corner name="1-4_soft"/>
<corner name="1-5_soft"/>
</sector>
<sector color="red" name="A3">
<corner name="6-1"/>
<corner name="6-2"/>
<corner name="6-3"/>
<corner name="6-4"/>
<corner name="6-5"/>
<corner name="6-6"/>
<corner name="6-7"/>
<corner name="6-8"/>
<corner name="6-9"/>
<corner name="6-10"/>
<corner name="6-11"/>
<corner name="6-12"/>
<corner name="6-13"/>
<corner name="6-14"/>
</sector>
<sector color="blue" name="SADO_EAST">
<corner name="sado_e_1"/>
<corner name="sado_e_2"/>
<corner name="sado_e_3"/>
<corner name="sado_e_4"/>
<corner name="sado_e_5"/>
<corner name="sado_e_6"/>
</sector><!--
<sector color="orange" name="A2_soft">
<corner name="3-1_soft"/>
<corner name="3-2_soft"/>
<corner name="3-3_soft"/>
<corner name="3-4_soft"/>
<corner name="3-5_soft"/>
</sector> -->
</sectors>
<variables>
<variable var="liftoff_pitch"/>
<variable var="liftoff_roll"/>
<variable var="stage_timer_msec"/>
</variables>
<exceptions>
<!-- Hard geofence (red -> Holding point) -->
<exception cond="And( Or(!InsideA1(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 300.0), Or(!InsideA3(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 300.0)) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<!-- Soft geofence (orange -> GO Standby) -->
<exception cond="And( Or(!InsideA1_soft(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 250.0), Or(!InsideA3(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 250.0)) @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_free"/>
<!-- Datalink loss (25 seconds -> GO STANDBY) -->
<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_free"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<exception cond="stage_time > 10" deroute="Holding point"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
</block>
<block name="Wait takeoff">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="stage_time > 10" deroute="Holding point"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 5.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 5.0) @AND (stage_time > 2)" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set var="stage_timer_msec" value="get_sys_time_msec()"/>
<set var="liftoff_roll" value="DegOfRad(stateGetNedToBodyEulers_f()->phi)"/>
<set var="liftoff_pitch" value="DegOfRad(stateGetNedToBodyEulers_f()->theta)"/>
<attitude pitch="liftoff_pitch" roll="liftoff_roll" throttle="0.75" until="(get_sys_time_msec()-stage_timer_msec)>250" vmode="throttle"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
</block>
<block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="STDBY"/>
</block>
<block name="Standby_free">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/>
</block>
<block name="mavlink_guided">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<stay wp="ML_global_target"/>
</block>
<!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> -->
<!-- <block name="follow_module">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
</block> -->
<!-- <block name="route">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<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="small_route_p1" strip_button="Route_P1" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p1"/>
</block>
<block name="small_route_p2" strip_button="Route_P2" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p2"/>
</block>
<block name="small_route_p3" strip_button="Route P3" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p3"/>
</block>
<block name="small_route_p4" strip_button="Route P4" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p4"/>
<deroute block="small_route_p1"/>
</block>
<block name="stay_p1" strip_button="Stay P1" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p1"/>
</block>
<block name="stay_p2">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p2"/>
</block>
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="nav.radius" wp="circ"/>
</block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-nav.radius" wp="circ"/>
</block>
<block name="Circle_Auto_fwd" strip_button="CircleAuto" strip_icon="circle-left.png">
<!-- <call_once var="circle_direction" fun="rotwing_state_choose_circle_direction(WP_circ)"/> -->
<!-- <circle radius="-nav.radius" wp="circ"/> -->
<exception cond="!rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CW_fwd"/>
<exception cond="rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CCW_fwd"/>
</block>
<!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> -->
<block name="Transition_quad" strip_button="Transition Quad" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="Downwind" strip_button="Downwind" strip_icon="land-right.png">
<go wp="Downwind"/>
</block>
<block name="Base">
<go wp="Base"/>
</block>
<block name="Final">
<go wp="Final"/>
<deroute block="land"/>
</block>
<block name="land here">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land" strip_button="Land" strip_icon="land-right.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="TD"/>
</block>
<block name="descend" strip_button="Descend" strip_icon="descend.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
</block>
<block name="flare_low">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/>
<!-- <call_once fun="NavStartDetectGround()"/> -->
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/modules/eff_scheduling_rotwing.xml
+7 -2
View File
@@ -22,7 +22,12 @@
<dl_settings> <dl_settings>
<dl_settings NAME="eff_sched"> <dl_settings NAME="eff_sched">
<dl_setting var="eff_sched_pusher_time" min="0.002" step="0.002" max="3." shortname="push_time"/> <dl_setting var="eff_sched_pusher_time" min="0.002" step="0.002" max="3." shortname="push_time"/>
<dl_setting shortname="roll_scaling" var="roll_eff_scaling" min="0.3" step="0.01" max="1.0"/> <dl_setting var="rw_flap_offset" min="-9600" step="10" max="9600" shortname="flap_offset"/>
<dl_setting var="eff_sched_p.Iyy_body" min="0" step="0.1" max="30" shortname="Iyy_body"/>
<dl_setting var="eff_sched_p.Ixx_body" min="0" step="0.1" max="30" shortname="Ixx_body"/>
<dl_setting var="eff_sched_p.Iyy_wing" min="0" step="0.1" max="30" shortname="Iyy_wing"/>
<dl_setting var="eff_sched_p.Ixx_wing" min="0" step="0.1" max="30" shortname="Ixx_wing"/>
<dl_setting var="eff_sched_p.m" min="0" step="0.1" max="50" shortname="M"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
@@ -67,4 +72,4 @@
<define name="ROTWING_EFF_SCHED_K_LIFT_TAIL" value="1"/> <define name="ROTWING_EFF_SCHED_K_LIFT_TAIL" value="1"/>
</test> </test>
</makefile> </makefile>
</module> </module>
conf/modules/guidance_indi_hybrid.xml
+12 -2
View File
@@ -7,10 +7,14 @@
- guidance_indi_hybid_tailsitter - guidance_indi_hybid_tailsitter
- guidance_indi_hybid_quadplane - guidance_indi_hybid_quadplane
</description> </description>
<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_HYBRID_"> <section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<define name="USE_WLS" value="FALSE|TRUE" description="use WLS allocation instead of matrix inversion (default: FALSE)"/> <define name="USE_WLS" value="FALSE|TRUE" description="use WLS allocation instead of matrix inversion (default: FALSE)"/>
<define name="WLS_PRIORITIES" value="{100., 100., 1.}" description="WLS priorities on control objectives"/> <define name="WLS_PRIORITIES" value="{100., 100., 1.}" description="WLS priorities on control objectives"/>
<define name="WLS_WU" value="{1., 1., 1.}" description="WLS weighting on outputs"/> <define name="WLS_WU" value="{1., 1., 1.}" description="WLS weighting on outputs"/>
<define name="MAX_LAT_ACCEL" value="9.81" description="Maximal Lateral Acceleration (in the Body-y Direction)"/>
<define name="COORDINATED_TURN_MIN_AIRSPEED" value="10.0" description="Yaw-rate computations based on airspeed are dividing by the airspeed, so a lower bound is important"/>
<define name="COORDINATED_TURN_MAX_AIRSPEED" value="30.0" description="Maximum airspeed in yaw-rate computations"/>
<define name="COORDINATED_TURN_AIRSPEED_MARGIN" value="0.0" description="Margin above maximum speed where we are sure the sensor reading is false"/>
</section> </section>
</doc> </doc>
<settings> <settings>
@@ -24,11 +28,17 @@
<dl_setting var="gih_params.speed_gain" min="0" step="0.1" max="10.0" shortname="kd" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/> <dl_setting var="gih_params.speed_gain" min="0" step="0.1" max="10.0" shortname="kd" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/>
<dl_setting var="gih_params.speed_gainz" min="0" step="0.1" max="10.0" shortname="kd_z" param="GUIDANCE_INDI_SPEED_GAINZ" persistent="true"/> <dl_setting var="gih_params.speed_gainz" min="0" step="0.1" max="10.0" shortname="kd_z" param="GUIDANCE_INDI_SPEED_GAINZ" persistent="true"/>
<dl_setting var="gih_params.heading_bank_gain" min="0" step="0.1" max="30.0" shortname="bank gain" param="GUIDANCE_INDI_HEADING_BANK_GAIN" persistent="true"/> <dl_setting var="gih_params.heading_bank_gain" min="0" step="0.1" max="30.0" shortname="bank gain" param="GUIDANCE_INDI_HEADING_BANK_GAIN" persistent="true"/>
<dl_setting var="guidance_indi_max_airspeed" min="12.0" step="1.0" max="30.0" shortname="cruise_airspeed" param="GUIDANCE_INDI_MAX_AIRSPEED"/> <dl_setting var="gih_params.max_airspeed" min="12.0" step="1.0" max="30.0" shortname="max_airspeed" param="GUIDANCE_INDI_MAX_AIRSPEED"/>
<dl_setting var="guidance_indi_max_bank" min="10.0" step="1.0" max="50.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK" unit="rad" alt_unit="deg"/> <dl_setting var="guidance_indi_max_bank" min="10.0" step="1.0" max="50.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK" unit="rad" alt_unit="deg"/>
<dl_setting var="guidance_indi_min_pitch" min="-130.0" step="1.0" max="-30.0" shortname="min_pitch" param="GUIDANCE_INDI_MIN_PITCH"/> <dl_setting var="guidance_indi_min_pitch" min="-130.0" step="1.0" max="-30.0" shortname="min_pitch" param="GUIDANCE_INDI_MIN_PITCH"/>
<dl_setting var="take_heading_control" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/> <dl_setting var="take_heading_control" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/>
<dl_setting var="guidance_indi_airspeed_filtering" min="0" step="1" max="1" values="OFF|ON" shortname="as_filt"/> <dl_setting var="guidance_indi_airspeed_filtering" min="0" step="1" max="1" values="OFF|ON" shortname="as_filt"/>
<dl_setting var="gih_params.climb_vspeed_fwd" min="0" step="0.1" max="20.0" shortname="fwd_vspeed_climb"/>
<dl_setting var="gih_params.descend_vspeed_fwd" min="-20.0" step="0.1" max="0" shortname="fwd_vspeed_descend"/>
<dl_setting var="gih_params.climb_vspeed_quad" min="0" step="0.1" max="20.0" shortname="quad_vspeed_climb"/>
<dl_setting var="gih_params.descend_vspeed_quad" min="-20.0" step="0.1" max="0" shortname="quad_vspeed_descend"/>
<dl_setting var="gih_params.stall_protect_gain" min="0" step="0.1" max="20.0" shortname="stall_gain"/>
<dl_setting var="coordinated_turn_use_accel" min="0" step="1" max="1" values="FALSE|TRUE" shortname="coordinated_turn_use_accel"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
conf/modules/rotwing_state.xml
+12 -10
View File
@@ -3,26 +3,28 @@
<doc> <doc>
<description>This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state.</description> <description>This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state.</description>
<section name="ROTWING_STATE" prefix="ROTWING_STATE_"> <section name="ROTWING_STATE" prefix="ROTWING_STATE_">
<define name="USE_ROTATION_REF_MODEL" value="FALSE" description="Slow down the wing rotation with a reference model"/>
</section> </section>
</doc> </doc>
<settings> <settings>
<dl_settings> <dl_settings>
<dl_settings NAME="RotWingState"> <dl_settings NAME="RotWing">
<dl_setting var="rotwing_state_skewing.wing_angle_deg_sp" min="0" step="1" max="90" shortname="skew angle"/> <dl_setting var="rotwing_state.nav_state" min="0" max="4" step="1" values="FORCE_HOVER|REQ_HOVER|FORCE_FW|REQ_FW|FREE" shortname="nav_state"/>
<dl_setting var="rotwing_state_skewing.force_rotation_angle" min="0" step="1" max="1" values="FALSE|TRUE" shortname="force_skew"/> <dl_setting var="rotwing_state.fw_min_airspeed" min="0" max="50" step="0.1" shortname="fw_min_airspeed"/>
<dl_setting var="rotwing_state_max_hover_speed" min="5" step="0.5" max="25" shortname="hover_speed"/> <dl_setting var="rotwing_state.cruise_airspeed" min="0" max="50" step="0.1" shortname="cruise_airspeed"/>
<dl_setting var="hover_motors_active" min="0" step="1" max="1" values="FALSE|TRUE" shortname="h_motors_active"/> <dl_setting var="rotwing_state.force_skew" min="0" max="1" step="1" values="FALSE|TRUE" shortname="force_skew"/>
<dl_setting var="bool_disable_hover_motors" min="0" step="1" max="1" values="FALSE|TRUE" shortname="h_motors_disable"/> <dl_setting var="rotwing_state.sp_skew_angle_deg" min="0" max="90" step="1" shortname="sp_skew_angle"/>
<dl_setting var="rotwing_state.fail_skew_angle" min="0" max="1" step="1" values="OFF|ON" shortname="fail_skew"/>
<dl_setting var="rotwing_state.fail_hover_motor" min="0" max="1" step="1" values="OFF|ON" shortname="fail_hover"/>
<dl_setting var="rotwing_state.fail_pusher_motor" min="0" max="1" step="1" values="OFF|ON" shortname="fail_pusher"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
<header> <header>
<file name="rotwing_state.h"/> <file name="rotwing_state.h"/>
</header> </header>
<init fun="init_rotwing_state()"/> <init fun="rotwing_state_init()"/>
<periodic fun="periodic_rotwing_state()" freq="50"/> <periodic fun="rotwing_state_periodic()"/>
<periodic fun="rotwing_state_skew_actuator_periodic()"/>
<makefile> <makefile>
<file name="rotwing_state.c"/> <file name="rotwing_state.c"/>
</makefile> </makefile>
conf/telemetry/highspeed_rotorcraft.xml
+61 -3
View File
@@ -24,7 +24,7 @@
<message name="ENERGY" period="0.5"/> <message name="ENERGY" period="0.5"/>
<message name="DATALINK_REPORT" period="5.1"/> <message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/> <message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="0.1"/> <message name="AIR_DATA" period="0.05"/>
<message name="SURVEY" period="2.5"/> <message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.05"/> <message name="OPTIC_FLOW_EST" period="0.05"/>
<message name="VECTORNAV_INFO" period="0.5"/> <message name="VECTORNAV_INFO" period="0.5"/>
@@ -42,7 +42,7 @@
<message name="AHRS_BIAS" period="7.5"/> <message name="AHRS_BIAS" period="7.5"/>
<message name="HOVER_LOOP" period="0.3"/> <message name="HOVER_LOOP" period="0.3"/>
<message name="GUIDANCE_H_REF_INT" period="0.31"/> <message name="GUIDANCE_H_REF_INT" period="0.31"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.4"/> <message name="GUIDANCE_INDI_HYBRID" period="0.05"/>
<message name="HYBRID_GUIDANCE" period="0.4"/> <message name="HYBRID_GUIDANCE" period="0.4"/>
<message name="ESC" period="0.5"/> <message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.1"/> <message name="AHRS_REF_QUAT" period="0.1"/>
@@ -64,6 +64,8 @@
<message name="WLS_V" period="0.2"/> <message name="WLS_V" period="0.2"/>
<message name="WLS_U" period="0.2"/> <message name="WLS_U" period="0.2"/>
<message name="FUELCELL" period="1.5"/> <message name="FUELCELL" period="1.5"/>
<message name="POWER_DEVICE" period="0.12"/>
<message name="IMU_HEATER" period="15.1"/>
</mode> </mode>
<mode name="sim" key_press="d"> <mode name="sim" key_press="d">
@@ -276,10 +278,65 @@
<message name="GPS_RTK" period="1"/> <message name="GPS_RTK" period="1"/>
</mode> </mode>
<mode name="Reduced">
<message name="DL_VALUE" period="0.7"/>
<message name="ROTORCRAFT_STATUS" period="0.4"/>
<message name="ROTORCRAFT_FP" period="0.20"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.20"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="2.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="0.6"/>
<message name="INS" period="0.6"/>
<message name="ENERGY" period="1.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="AIR_DATA" period="0.2"/>
<message name="INS_EKF2" period="0.55"/>
<message name="INS_EKF2_EXT" period="4.25"/>
<message name="GUIDANCE_H_REF_INT" period="0.9"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<message name="ESC" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.07"/>
<message name="GPS_RTK" period="0.9"/>
<message name="APPROACH_MOVING_TARGET" period="2.1"/>
<message name="TARGET_POS_INFO" period="2.1"/>
<message name="ROTATING_WING_STATE" period="0.07"/>
<message name="ACTUATORS" period="0.07"/>
<message name="GUIDANCE" period="0.07"/>
<message name="STAB_ATTITUDE" period="0.07"/>
<message name="IMU_HEATER" period="15.1"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="DEBUG_VECT" period="0.9"/>
</mode>
<mode name="Minimal">
<message name="ACTUATORS" period="0.3"/>
<message name="AHRS_REF_QUAT" period="0.3"/>
<message name="AIR_DATA" period="0.4"/>
<message name="ALIVE" period="2.1"/>
<message name="DATALINK_REPORT" period="11.1"/>
<message name="ENERGY" period="3.3"/>
<message name="ESC" period="0.4"/>
<message name="GPS_INT" period="1.7"/>
<message name="IMU_HEATER" period="15.1"/>
<message name="INS" period="1.7"/>
<message name="ROTATING_WING_STATE" period="0.3"/>
<message name="ROTORCRAFT_FP" period="0.9"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.9"/>
<message name="ROTORCRAFT_STATUS" period="0.9"/>
<message name="STAB_ATTITUDE" period="0.3"/>
<message name="TARGET_POS_INFO" period="2.1"/>
<message name="WP_MOVED" period="2.1"/>
</mode>
</process> </process>
<process name="FlightRecorder"> <process name="FlightRecorder">
<mode name="default"> <mode name="default">
<message name="ALIVE" period="15.1"/>
<message name="AUTOPILOT_VERSION" period="11.1"/> <message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="GPS" period="11.1"/> <message name="GPS" period="11.1"/>
<message name="DL_VALUE" period="2.5"/> <message name="DL_VALUE" period="2.5"/>
@@ -327,7 +384,7 @@
<message name="DEBUG" period="0.02"/> <!-- For the parachute module --> <message name="DEBUG" period="0.02"/> <!-- For the parachute module -->
<message name="WLS_V" period="0.002"/> <message name="WLS_V" period="0.002"/>
<message name="WLS_U" period="0.002"/> <message name="WLS_U" period="0.002"/>
<message name="POWER_DEVICE" period="0.12"/> <message name="POWER_DEVICE" period="0.02"/>
<message name="FUELCELL" period="1.5"/> <message name="FUELCELL" period="1.5"/>
</mode> </mode>
</process> </process>
@@ -347,6 +404,7 @@
<message name="GPS_GLOBAL_ORIGIN" period="5.1"/> <message name="GPS_GLOBAL_ORIGIN" period="5.1"/>
<message name="VFR_HUD" period="0.5"/> <message name="VFR_HUD" period="0.5"/>
<message name="RC_CHANNELS" period="0.2"/> <message name="RC_CHANNELS" period="0.2"/>
<message name="EXTENDED_SYS_STATE" period="1.0"/>
</mode> </mode>
</process> </process>
conf/userconf/tudelft/RW_control_panel.xml
+10
View File
@@ -1506,5 +1506,15 @@
<arg flag="-g" constant="1050x300+0+1000"/> <arg flag="-g" constant="1050x300+0+1000"/>
</program> </program>
</session> </session>
<session name="RW25">
<program name="Data Link">
<arg flag="-udp"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="PprzGCS"/>
<program name="RotWingStatus"/>
<program name="Wind"/>
</session>
</section> </section>
</control_panel> </control_panel>
conf/userconf/tudelft/conf.xml
+3 -3
View File
@@ -610,7 +610,7 @@
airframe="airframes/tudelft/rotwing_v3f.xml" airframe="airframes/tudelft/rotwing_v3f.xml"
radio="radios/crossfire_sbus.xml" radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml" telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml" flight_plan="flight_plans/tudelft/rotating_wing_troia.xml"
settings="settings/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.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/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.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/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red" gui_color="red"
@@ -665,9 +665,9 @@
airframe="airframes/tudelft/rotwing5.xml" airframe="airframes/tudelft/rotwing5.xml"
radio="radios/crossfire_sbus.xml" radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml" telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml" flight_plan="flight_plans/tudelft/rotating_wing_troia.xml"
settings="settings/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml"
settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/electrical.xml modules/follow_me.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/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.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/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red" gui_color="red"
/> />
<aircraft <aircraft
conf/userconf/tudelft/control_panel.xml
+48 -4
View File
@@ -197,8 +197,8 @@
<program name="Messages"/> <program name="Messages"/>
<program name="PprzGCS"/> <program name="PprzGCS"/>
<program name="Herelink PprzLink Router"> <program name="Herelink PprzLink Router">
<arg flag="-e udp://192.168.0.11:5242:192.168.0.10:5243"/> <arg flag="-e udp://192.168.0.11:5242:192.168.0.10:5243"/>
<arg flag="-e udp://0.0.0.0:4243:192.168.43.255:4242"/> <arg flag="-e udp://0.0.0.0:4243:192.168.43.255:4242"/>
</program> </program>
</session> </session>
@@ -229,8 +229,8 @@
<arg flag="-udp"/> <arg flag="-udp"/>
</program> </program>
<program name="Herelink PprzLink Router"> <program name="Herelink PprzLink Router">
<arg flag="-e udp://192.168.0.11:5242:192.168.0.10:5243"/> <arg flag="-e udp://192.168.0.11:5242:192.168.0.10:5243"/>
<arg flag="-e udp://0.0.0.0:4243:192.168.43.255:4242"/> <arg flag="-e udp://0.0.0.0:4243:192.168.43.255:4242"/>
</program> </program>
</session> </session>
@@ -294,6 +294,50 @@
<program name="IridiumDialer"/> <program name="IridiumDialer"/>
</session> </session>
<session name="RW25">
<program name="Data Link">
<arg flag="-udp"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="PprzGCS"/>
<program name="RotWingStatus"/>
<program name="Wind"/>
</session>
<session name="ROTATING_WING_STATE">
<program name="Messages"/>
<program name="Simulator">
<arg flag="-a" constant="RotatingWingV3B"/>
<arg flag="-t" constant="nps"/>
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-udp_broadcast"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="PprzGCS"/>
<program name="Real-time Plotter">
<arg flag="-g" constant="1000x250-0+0"/>
<arg flag="-t" constant="SKEW"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:ROTATING_WING_STATE:meas_skew_angle"/>
<arg flag="-c" constant="*:telemetry:ROTATING_WING_STATE:sp_skew_angle"/>
<arg flag="-n"/>
<arg flag="-g" constant="1000x250-0+250"/>
<arg flag="-t" constant="SKEW"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:ROTATING_WING_STATE:nav_airspeed"/>
<arg flag="-c" constant="*:telemetry:ROTATING_WING_STATE:min_airspeed"/>
<arg flag="-c" constant="*:telemetry:ROTATING_WING_STATE:max_airspeed"/>
<arg flag="-c" constant="*:telemetry:AIR_DATA:airspeed"/>
</program>
</session>
<session name="Raw Sensors"> <session name="Raw Sensors">
<program name="Real-time Plotter"> <program name="Real-time Plotter">
<arg flag="-g" constant="1000x250-0+0"/> <arg flag="-g" constant="1000x250-0+0"/>
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+103 -39
View File
@@ -71,6 +71,42 @@
#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2) #define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)
#endif #endif
#ifndef GUIDANCE_INDI_MAX_AIRSPEED
#error "You must have an airspeed sensor to use this guidance"
#endif
#ifndef GUIDANCE_INDI_MIN_AIRSPEED
#define GUIDANCE_INDI_MIN_AIRSPEED -10.f
#endif
/**
* Climb speed when navigation is making turns instead of direct lines
*/
#ifndef GUIDANCE_INDI_FWD_CLIMB_SPEED
#define GUIDANCE_INDI_FWD_CLIMB_SPEED 4.0
#endif
/**
* Descend speed when navigation is making turns instead of direct lines
*/
#ifndef GUIDANCE_INDI_FWD_DESCEND_SPEED
#define GUIDANCE_INDI_FWD_DESCEND_SPEED -4.0
#endif
/**
* Climb speed when navigation is doing direct lines
*/
#ifndef GUIDANCE_INDI_QUAD_CLIMB_SPEED
#define GUIDANCE_INDI_QUAD_CLIMB_SPEED 2.0
#endif
/**
* Descend speed when navigation is doing direct lines
*/
#ifndef GUIDANCE_INDI_QUAD_DESCEND_SPEED
#define GUIDANCE_INDI_QUAD_DESCEND_SPEED -2.0
#endif
struct guidance_indi_hybrid_params gih_params = { struct guidance_indi_hybrid_params gih_params = {
.pos_gain = GUIDANCE_INDI_POS_GAIN, .pos_gain = GUIDANCE_INDI_POS_GAIN,
.pos_gainz = GUIDANCE_INDI_POS_GAINZ, .pos_gainz = GUIDANCE_INDI_POS_GAINZ,
@@ -82,13 +118,15 @@ struct guidance_indi_hybrid_params gih_params = {
.liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, // coefficient of airspeed squared .liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, // coefficient of airspeed squared
.liftd_p80 = GUIDANCE_INDI_LIFTD_P80, .liftd_p80 = GUIDANCE_INDI_LIFTD_P80,
.liftd_p50 = GUIDANCE_INDI_LIFTD_P50, .liftd_p50 = GUIDANCE_INDI_LIFTD_P50,
.min_airspeed = GUIDANCE_INDI_MIN_AIRSPEED,
.max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED,
.stall_protect_gain = 1.5, // m/s^2 downward acceleration per m/s airspeed loss
.climb_vspeed_fwd = GUIDANCE_INDI_FWD_CLIMB_SPEED,
.descend_vspeed_fwd = GUIDANCE_INDI_FWD_DESCEND_SPEED,
.climb_vspeed_quad = GUIDANCE_INDI_QUAD_CLIMB_SPEED,
.descend_vspeed_quad = GUIDANCE_INDI_QUAD_DESCEND_SPEED,
}; };
#ifndef GUIDANCE_INDI_MAX_AIRSPEED
#error "You must have an airspeed sensor to use this guidance"
#endif
float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
// Quadplanes can hover at various pref pitch // Quadplanes can hover at various pref pitch
float guidance_indi_pitch_pref_deg = 0; float guidance_indi_pitch_pref_deg = 0;
@@ -113,7 +151,7 @@ float guidance_indi_pitch_pref_deg = 0;
/*Airspeed threshold where making a turn is "worth it"*/ /*Airspeed threshold where making a turn is "worth it"*/
#ifndef TURN_AIRSPEED_TH #ifndef TURN_AIRSPEED_TH
#define TURN_AIRSPEED_TH 10.0 #define TURN_AIRSPEED_TH 13 .0
#endif #endif
/*Boolean to force the heading to a static value (only use for specific experiments)*/ /*Boolean to force the heading to a static value (only use for specific experiments)*/
@@ -158,20 +196,21 @@ static void guidance_indi_filter_thrust(void);
#define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF 0.5 #define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF 0.5
#endif #endif
#ifndef GUIDANCE_INDI_CLIMB_SPEED_FWD
#define GUIDANCE_INDI_CLIMB_SPEED_FWD 4.0
#endif
#ifndef GUIDANCE_INDI_DESCEND_SPEED_FWD
#define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0
#endif
#ifndef GUIDANCE_INDI_MAX_LAT_ACCEL #ifndef GUIDANCE_INDI_MAX_LAT_ACCEL
#define GUIDANCE_INDI_MAX_LAT_ACCEL 9.81 #define GUIDANCE_INDI_MAX_LAT_ACCEL 9.81
#endif #endif
float climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD; #ifndef GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED
float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_SPEED_FWD; #define GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED 10.0
#endif
#ifndef GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED
#define GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED 30.0
#endif
#ifndef GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN
#define GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN 0.0
#endif
float inv_eff[4]; float inv_eff[4];
@@ -179,6 +218,16 @@ float inv_eff[4];
float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK; float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK;
float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH; float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH;
#if defined(ROTWING_STATE_FW_MAX_AIRSPEED) && defined(ROTWING_STATE_QUAD_MAX_AIRSPEED)
float gih_coordinated_turn_min_airspeed = ROTWING_STATE_QUAD_MAX_AIRSPEED;
float gih_coordinated_turn_max_airspeed = ROTWING_STATE_FW_MAX_AIRSPEED + GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN;
#else
float gih_coordinated_turn_min_airspeed = GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED;
float gih_coordinated_turn_max_airspeed = GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED + GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN;
#endif
bool coordinated_turn_use_accel = false;
/** state eulers in zxy order */ /** state eulers in zxy order */
struct FloatEulers eulers_zxy; struct FloatEulers eulers_zxy;
@@ -192,6 +241,7 @@ Butterworth2LowPass accely_filt;
Butterworth2LowPass guidance_indi_airspeed_filt; Butterworth2LowPass guidance_indi_airspeed_filt;
struct FloatVect2 desired_airspeed; struct FloatVect2 desired_airspeed;
float gi_unbounded_airspeed_sp = 0.f;
float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U]; float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U];
struct FloatVect3 euler_cmd; struct FloatVect3 euler_cmd;
@@ -247,6 +297,7 @@ struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0};
float time_of_vel_sp = 0.0; float time_of_vel_sp = 0.0;
void guidance_indi_propagate_filters(void); void guidance_indi_propagate_filters(void);
void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed);
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h" #include "modules/datalink/telemetry.h"
@@ -344,7 +395,7 @@ void guidance_indi_enter(void)
thrust_in = stabilization.cmd[COMMAND_THRUST]; thrust_in = stabilization.cmd[COMMAND_THRUST];
thrust_act = thrust_in; thrust_act = thrust_in;
guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi; guidance_indi_hybrid_heading_sp = eulers_zxy.psi;
float tau = 1.0 / (2.0 * M_PI * filter_cutoff); float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
float sample_time = 1.0 / PERIODIC_FREQUENCY; float sample_time = 1.0 / PERIODIC_FREQUENCY;
@@ -352,9 +403,6 @@ void guidance_indi_enter(void)
init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0); init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
} }
/*Obtain eulers with zxy rotation order*/
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
init_butterworth_2_low_pass(&roll_filt, tau, sample_time, eulers_zxy.phi); init_butterworth_2_low_pass(&roll_filt, tau, sample_time, eulers_zxy.phi);
init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta); init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta);
init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in); init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
@@ -364,6 +412,11 @@ void guidance_indi_enter(void)
init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0); init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0);
} }
void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed) {
gih_params.min_airspeed = min_airspeed;
gih_params.max_airspeed = max_airspeed;
}
/** /**
* @param accel_sp accel setpoint in NED frame [m/s^2] * @param accel_sp accel setpoint in NED frame [m/s^2]
* @param heading_sp the desired heading [rad] * @param heading_sp the desired heading [rad]
@@ -459,7 +512,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
float airspeed_turn = stateGetAirspeed_f(); float airspeed_turn = stateGetAirspeed_f();
#endif #endif
// We are dividing by the airspeed, so a lower bound is important // We are dividing by the airspeed, so a lower bound is important
Bound(airspeed_turn, 10.0f, 30.0f); Bound(airspeed_turn, gih_coordinated_turn_min_airspeed, gih_coordinated_turn_max_airspeed);
guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x; guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y; guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
@@ -586,6 +639,7 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
float airspeed = 0.f; float airspeed = 0.f;
#else #else
float airspeed = stateGetAirspeed_f(); float airspeed = stateGetAirspeed_f();
Bound(airspeed, 0.0f, 100.0f);
if (guidance_indi_airspeed_filtering) { if (guidance_indi_airspeed_filtering) {
airspeed = guidance_indi_airspeed_filt.o[0]; airspeed = guidance_indi_airspeed_filt.o[0];
} }
@@ -598,18 +652,27 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
VECT2_DIFF(desired_airspeed, gi_speed_sp, windspeed); // Use 2d part of gi_speed_sp VECT2_DIFF(desired_airspeed, gi_speed_sp, windspeed); // Use 2d part of gi_speed_sp
float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed); float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
// Make turn instead of straight line gi_unbounded_airspeed_sp = norm_des_as;
// Check if some minimum airspeed is desired (e.g. to prevent stall)
if (norm_des_as < gih_params.min_airspeed) {
norm_des_as = gih_params.min_airspeed;
}
float gi_airspeed_sp = norm_des_as;
// Make turn instead of straight line, control airspeed
if ((airspeed > TURN_AIRSPEED_TH) && (norm_des_as > (TURN_AIRSPEED_TH+2.0f))) { if ((airspeed > TURN_AIRSPEED_TH) && (norm_des_as > (TURN_AIRSPEED_TH+2.0f))) {
// Give the wind cancellation priority. // Give the wind cancellation priority.
if (norm_des_as > guidance_indi_max_airspeed) { if (norm_des_as > gih_params.max_airspeed) {
float groundspeed_factor = 0.0f; float groundspeed_factor = 0.0f;
// if the wind is faster than we can fly, just fly in the wind direction // if the wind is faster than we can fly, just fly in the wind direction
if (FLOAT_VECT2_NORM(windspeed) < guidance_indi_max_airspeed) { if (FLOAT_VECT2_NORM(windspeed) < gih_params.max_airspeed) {
float av = gi_speed_sp.x * gi_speed_sp.x + gi_speed_sp.y * gi_speed_sp.y; float av = gi_speed_sp.x * gi_speed_sp.x + gi_speed_sp.y * gi_speed_sp.y;
float bv = -2.f * (windspeed.x * gi_speed_sp.x + windspeed.y * gi_speed_sp.y); float bv = -2.f * (windspeed.x * gi_speed_sp.x + windspeed.y * gi_speed_sp.y);
float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed; float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - gih_params.max_airspeed * gih_params.max_airspeed;
float dv = bv * bv - 4.0f * av * cv; float dv = bv * bv - 4.0f * av * cv;
@@ -625,14 +688,11 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
desired_airspeed.x = groundspeed_factor * gi_speed_sp.x - windspeed.x; desired_airspeed.x = groundspeed_factor * gi_speed_sp.x - windspeed.x;
desired_airspeed.y = groundspeed_factor * gi_speed_sp.y - windspeed.y; desired_airspeed.y = groundspeed_factor * gi_speed_sp.y - windspeed.y;
speed_sp_b_x = guidance_indi_max_airspeed; gi_airspeed_sp = gih_params.max_airspeed;
} }
// desired airspeed can not be larger than max airspeed
speed_sp_b_x = Min(norm_des_as, guidance_indi_max_airspeed);
if (force_forward) { if (force_forward) {
speed_sp_b_x = guidance_indi_max_airspeed; gi_airspeed_sp = gih_params.max_airspeed;
} }
// Calculate accel sp in body axes, because we need to regulate airspeed // Calculate accel sp in body axes, because we need to regulate airspeed
@@ -645,7 +705,7 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
BoundAbs(sp_accel_b.y, GUIDANCE_INDI_MAX_LAT_ACCEL); BoundAbs(sp_accel_b.y, GUIDANCE_INDI_MAX_LAT_ACCEL);
// Control the airspeed // Control the airspeed
sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain; sp_accel_b.x = (gi_airspeed_sp - airspeed) * gih_params.speed_gain;
accel_sp.x = cpsi * sp_accel_b.x - spsi * sp_accel_b.y; accel_sp.x = cpsi * sp_accel_b.x - spsi * sp_accel_b.y;
accel_sp.y = spsi * sp_accel_b.x + cpsi * sp_accel_b.y; accel_sp.y = spsi * sp_accel_b.x + cpsi * sp_accel_b.y;
@@ -659,8 +719,8 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
float speed_increment = speed_sp_b_x - groundspeed_x; float speed_increment = speed_sp_b_x - groundspeed_x;
// limit groundspeed setpoint to max_airspeed + (diff gs and airspeed) // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
if ((speed_increment + airspeed) > guidance_indi_max_airspeed) { if ((speed_increment + airspeed) > gih_params.max_airspeed) {
speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed; speed_sp_b_x = gih_params.max_airspeed + groundspeed_x - airspeed;
} }
} }
@@ -673,22 +733,25 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
} }
// Bound the acceleration setpoint // Bound the acceleration setpoint
float accelbound = 3.0f + airspeed / guidance_indi_max_airspeed * 5.0f; // FIXME remove hard coded values float accelbound = 3.0f + airspeed / gih_params.max_airspeed * 5.0f; // FIXME remove hard coded values
float_vect3_bound_in_2d(&accel_sp, accelbound); float_vect3_bound_in_2d(&accel_sp, accelbound);
/*BoundAbs(sp_accel.x, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
/*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
BoundAbs(accel_sp.z, 3.0); BoundAbs(accel_sp.z, 3.0);
if (!rotwing_state_pusher_motor_running() && !rotwing_state_hover_motors_running()) {
accel_sp.z = gih_params.stall_protect_gain * (gi_airspeed_sp - airspeed);
BoundAbs(accel_sp.z, 5.0);
}
return accel_sp; return accel_sp;
} }
static float bound_vz_sp(float vz_sp) static float bound_vz_sp(float vz_sp)
{ {
// Bound vertical speed setpoint // Bound vertical speed setpoint
if (stateGetAirspeed_f() > 13.f) { if (stateGetAirspeed_f() > TURN_AIRSPEED_TH) {
Bound(vz_sp, -climb_vspeed_fwd, -descend_vspeed_fwd); Bound(vz_sp, -gih_params.climb_vspeed_fwd, -gih_params.descend_vspeed_fwd);
} else { } else {
Bound(vz_sp, -nav.climb_vspeed, -nav.descend_vspeed); // FIXME don't use nav settings Bound(vz_sp, -gih_params.climb_vspeed_quad, -gih_params.descend_vspeed_quad);
} }
return vz_sp; return vz_sp;
} }
@@ -804,6 +867,7 @@ void guidance_indi_propagate_filters(void)
update_butterworth_2_low_pass(&accely_filt, accely); update_butterworth_2_low_pass(&accely_filt, accely);
float airspeed = stateGetAirspeed_f(); float airspeed = stateGetAirspeed_f();
Bound(airspeed, 0.0f, 100.0f);
update_butterworth_2_low_pass(&guidance_indi_airspeed_filt, airspeed); update_butterworth_2_low_pass(&guidance_indi_airspeed_filt, airspeed);
} }
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+11 -8
View File
@@ -70,6 +70,7 @@ enum GuidanceIndiHybrid_VMode {
extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp); extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp);
extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode); extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode);
extern void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed);
struct guidance_indi_hybrid_params { struct guidance_indi_hybrid_params {
float pos_gain; float pos_gain;
@@ -80,30 +81,32 @@ struct guidance_indi_hybrid_params {
float liftd_asq; float liftd_asq;
float liftd_p80; float liftd_p80;
float liftd_p50; float liftd_p50;
float min_airspeed;
float max_airspeed;
float stall_protect_gain;
float climb_vspeed_fwd;
float descend_vspeed_fwd;
float climb_vspeed_quad;
float descend_vspeed_quad;
}; };
extern struct FloatVect3 sp_accel; extern struct FloatVect3 sp_accel;
extern struct FloatVect3 gi_speed_sp; extern struct FloatVect3 gi_speed_sp;
extern struct guidance_indi_hybrid_params gih_params;
extern float guidance_indi_pitch_pref_deg; extern float guidance_indi_pitch_pref_deg;
#if GUIDANCE_INDI_HYBRID_USE_WLS extern float gi_unbounded_airspeed_sp;
extern float Wu_gih[GUIDANCE_INDI_HYBRID_U];
extern float Wv_gih[GUIDANCE_INDI_HYBRID_V];
extern float du_min_gih[GUIDANCE_INDI_HYBRID_U];
extern float du_max_gih[GUIDANCE_INDI_HYBRID_U];
extern float du_pref_gih[GUIDANCE_INDI_HYBRID_U];
#endif
extern float guidance_indi_thrust_z_eff; extern float guidance_indi_thrust_z_eff;
extern struct guidance_indi_hybrid_params gih_params; extern struct guidance_indi_hybrid_params gih_params;
extern float guidance_indi_specific_force_gain; extern float guidance_indi_specific_force_gain;
extern float guidance_indi_max_airspeed;
extern bool take_heading_control; extern bool take_heading_control;
extern float guidance_indi_max_bank; extern float guidance_indi_max_bank;
extern float guidance_indi_min_pitch; extern float guidance_indi_min_pitch;
extern bool force_forward; ///< forward flight for hybrid nav extern bool force_forward; ///< forward flight for hybrid nav
extern bool guidance_indi_airspeed_filtering; extern bool guidance_indi_airspeed_filtering;
extern bool coordinated_turn_use_accel;
#endif /* GUIDANCE_INDI_HYBRID_H */ #endif /* GUIDANCE_INDI_HYBRID_H */
sw/airborne/firmwares/rotorcraft/navigation.c
+6 -3
View File
@@ -267,9 +267,12 @@ void nav_periodic_task(void)
bool nav_detect_ground(void) bool nav_detect_ground(void)
{ {
if (!autopilot.ground_detected) { return false; } if (autopilot.ground_detected) {
autopilot.ground_detected = false; autopilot.ground_detected = false;
return true; return true;
} else {
return false;
}
} }
bool nav_is_in_flight(void) bool nav_is_in_flight(void)
sw/airborne/modules/ctrl/eff_scheduling_rotwing.c
+61 -27
View File
@@ -130,6 +130,7 @@ struct rotwing_eff_sched_param_t eff_sched_p = {
.Ixx_wing = ROTWING_EFF_SCHED_IXX_WING, .Ixx_wing = ROTWING_EFF_SCHED_IXX_WING,
.Iyy_wing = ROTWING_EFF_SCHED_IYY_WING, .Iyy_wing = ROTWING_EFF_SCHED_IYY_WING,
.m = ROTWING_EFF_SCHED_M, .m = ROTWING_EFF_SCHED_M,
.DMdpprz_hover_pitch = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH,
.DMdpprz_hover_roll = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL, .DMdpprz_hover_roll = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
.hover_roll_pitch_coef = ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF, .hover_roll_pitch_coef = ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
.hover_roll_roll_coef = ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF, .hover_roll_roll_coef = ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
@@ -146,9 +147,28 @@ struct rotwing_eff_sched_param_t eff_sched_p = {
.k_lift_tail = ROTWING_EFF_SCHED_K_LIFT_TAIL .k_lift_tail = ROTWING_EFF_SCHED_K_LIFT_TAIL
}; };
float eff_sched_pusher_time = 0.002; int32_t rw_flap_offset = 0;
float roll_eff_scaling = 1.f; // for negative values, still should be low_lim < up_lim
inline float bound_or_zero(float value, float low_lim, float up_lim) {
float output = value;
if (low_lim > 0.f) {
if (value < low_lim) {
output = 0.f;
} else if (value > up_lim) {
output = up_lim;
}
} else {
if (value > up_lim) {
output = 0.f;
} else if (value < low_lim) {
output = low_lim;
}
}
return output;
}
float eff_sched_pusher_time = 0.002;
struct rotwing_eff_sched_var_t eff_sched_var; struct rotwing_eff_sched_var_t eff_sched_var;
@@ -204,8 +224,9 @@ void eff_scheduling_rotwing_init(void)
eff_sched_var.sinr3 = 0; eff_sched_var.sinr3 = 0;
// Set moment derivative variables // Set moment derivative variables
eff_sched_var.pitch_motor_dMdpprz = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH; float hover_thrust = 6000;
eff_sched_var.roll_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_roll[0] + (MAX_PPRZ/2.) * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust eff_sched_var.pitch_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_pitch[0] + 2*hover_thrust * eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.; // Dmdpprz hover pitch for hover thrust
eff_sched_var.roll_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_roll[0] + 2*hover_thrust * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust
eff_sched_var.cmd_elevator = 0; eff_sched_var.cmd_elevator = 0;
eff_sched_var.cmd_pusher = 0; eff_sched_var.cmd_pusher = 0;
@@ -282,44 +303,45 @@ void eff_scheduling_rotwing_update_airspeed(void)
void eff_scheduling_rotwing_update_hover_motor_effectiveness(void) void eff_scheduling_rotwing_update_hover_motor_effectiveness(void)
{ {
// Pitch motor effectiveness float cmd_quat[4];
float dM_dpprz[4];
// Quadratic thrust (and therefore moment) model of the hover propellers
for (uint8_t i = 0; i < 4; i++) {
cmd_quat[i] = actuator_state_filt_vect[i];
Bound(cmd_quat[i], 2500, MAX_PPRZ);
float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy; if(i==0 || i==2) { // pitch motors
dM_dpprz[i] = (eff_sched_p.DMdpprz_hover_pitch[0] + 2*cmd_quat[i] * eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.;
float cmd_right = actuator_state_filt_vect[1]; // Bound dM_dpprz to half and 2 times the hover effectiveness
float cmd_left = actuator_state_filt_vect[3]; Bound(dM_dpprz[i], eff_sched_var.pitch_motor_dMdpprz * 0.5, eff_sched_var.pitch_motor_dMdpprz * 2.0);
Bound(cmd_right, 3500, MAX_PPRZ); } else { // roll motors
Bound(cmd_left, 3500, MAX_PPRZ); dM_dpprz[i] = (eff_sched_p.DMdpprz_hover_roll[0] + 2*cmd_quat[i] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
Bound(dM_dpprz[i], eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
}
}
// Roll motor effectiveness // Roll motor effectiveness
float dM_dpprz_right = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_right * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; float dM_dpprz_right = dM_dpprz[1];
float dM_dpprz_left = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_left * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; float dM_dpprz_left = dM_dpprz[3];;
dM_dpprz_right = dM_dpprz_right * roll_eff_scaling;
dM_dpprz_left = dM_dpprz_left * roll_eff_scaling;
// Bound dM_dpprz to half and 2 times the hover effectiveness
Bound(dM_dpprz_right, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
Bound(dM_dpprz_left, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
float roll_motor_p_eff_right = -(dM_dpprz_right * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx; float roll_motor_p_eff_right = -(dM_dpprz_right * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
Bound(roll_motor_p_eff_right, -1, -0.00001); roll_motor_p_eff_right = bound_or_zero(roll_motor_p_eff_right, -1.f, -0.00001f);
float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx; float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
if(autopilot.in_flight) { if(autopilot.in_flight) {
float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx; float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx;
roll_motor_p_eff_left += roll_motor_airspeed_compensation; roll_motor_p_eff_left += roll_motor_airspeed_compensation;
} }
Bound(roll_motor_p_eff_left, 0.00001, 1); roll_motor_p_eff_left = bound_or_zero(roll_motor_p_eff_left, 0.00001f, 1.f);
float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy; float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy;
Bound(roll_motor_q_eff, 0, 1); Bound(roll_motor_q_eff, 0, 1);
// Update front pitch motor q effectiveness // Update front pitch motor q effectiveness
g1g2[1][0] = pitch_motor_q_eff; // pitch effectiveness front motor g1g2[1][0] = dM_dpprz[0] / eff_sched_var.Iyy; // pitch effectiveness front motor
// Update back motor q effectiveness // Update back motor q effectiveness
g1g2[1][2] = -pitch_motor_q_eff; // pitch effectiveness back motor g1g2[1][2] = - dM_dpprz[2] / eff_sched_var.Iyy; // pitch effectiveness back motor
// Update right motor p and q effectiveness // Update right motor p and q effectiveness
g1g2[0][1] = roll_motor_p_eff_right; // roll effectiveness right motor (no airspeed compensation) g1g2[0][1] = roll_motor_p_eff_right; // roll effectiveness right motor (no airspeed compensation)
@@ -376,6 +398,11 @@ void eff_scheduling_rotwing_update_aileron_effectiveness(void)
float eff_x_aileron = dMxdpprz / eff_sched_var.Ixx; float eff_x_aileron = dMxdpprz / eff_sched_var.Ixx;
Bound(eff_x_aileron, 0, 0.005) Bound(eff_x_aileron, 0, 0.005)
g1g2[0][6] = eff_x_aileron; g1g2[0][6] = eff_x_aileron;
float dMydpprz = 4.0*(eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr2 * eff_sched_var.cosr) / 1000000.;
float eff_y_aileron = dMydpprz / eff_sched_var.Iyy;
eff_y_aileron = bound_or_zero(eff_y_aileron, 0.00003f, 0.005f);
g1g2[1][6] = eff_y_aileron;
} }
void eff_scheduling_rotwing_update_flaperon_effectiveness(void) void eff_scheduling_rotwing_update_flaperon_effectiveness(void)
@@ -427,11 +454,18 @@ void stabilization_indi_set_wls_settings(void)
wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i]; wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i];
wls_stab_p.u_max[i] = MAX_PPRZ; wls_stab_p.u_max[i] = MAX_PPRZ;
wls_stab_p.u_pref[i] = act_pref[i]; wls_stab_p.u_pref[i] = act_pref[i];
if (i == 5) { if (i == 5) { // elevator
wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in preferred state to 0 for elevator wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator
wls_stab_p.u_min[i] = 0; // cmd 0 is lowest position for elevator wls_stab_p.u_min[i] = 0; // cmd 0 is lowest position for elevator
} }
if (i==8) { if (i == 7) { // flaperons
// If an offset is used, limit the max differential command to prevent unilateral saturation.
int32_t flap_saturation_limit = MAX_PPRZ - abs(rw_flap_offset);
BoundAbs(flap_saturation_limit, MAX_PPRZ);
wls_stab_p.u_min[i] = -flap_saturation_limit;
wls_stab_p.u_max[i] = flap_saturation_limit;
}
if (i==8) { // pusher
// dt (min to max) MAX_PPRZ / (dt * f) dt_min == 0.002 // dt (min to max) MAX_PPRZ / (dt * f) dt_min == 0.002
Bound(eff_sched_pusher_time, 0.002, 5.); Bound(eff_sched_pusher_time, 0.002, 5.);
float max_increment = MAX_PPRZ / (eff_sched_pusher_time * 500); float max_increment = MAX_PPRZ / (eff_sched_pusher_time * 500);
sw/airborne/modules/ctrl/eff_scheduling_rotwing.h
+4 -1
View File
@@ -36,6 +36,7 @@ struct rotwing_eff_sched_param_t {
float Iyy_wing; // wing MMOI around the spanwise direction of the wing [kgm²] float Iyy_wing; // wing MMOI around the spanwise direction of the wing [kgm²]
float m; // mass [kg] float m; // mass [kg]
float DMdpprz_hover_roll[2]; // Moment coeficients for roll motors (Scaled by 10000) float DMdpprz_hover_roll[2]; // Moment coeficients for roll motors (Scaled by 10000)
float DMdpprz_hover_pitch[2]; // Moment coeficients for pitch motors (Scaled by 10000)
float hover_roll_pitch_coef[2]; // Model coefficients to correct pitch effective for roll motors float hover_roll_pitch_coef[2]; // Model coefficients to correct pitch effective for roll motors
float hover_roll_roll_coef[2]; // Model coefficients to correct roll effectiveness for roll motors float hover_roll_roll_coef[2]; // Model coefficients to correct roll effectiveness for roll motors
float k_elevator[3]; float k_elevator[3];
@@ -50,6 +51,7 @@ struct rotwing_eff_sched_param_t {
float k_lift_fuselage; float k_lift_fuselage;
float k_lift_tail; float k_lift_tail;
}; };
extern struct rotwing_eff_sched_param_t eff_sched_p;
struct rotwing_eff_sched_var_t { struct rotwing_eff_sched_var_t {
float Ixx; // Total MMOI around roll axis [kgm²] float Ixx; // Total MMOI around roll axis [kgm²]
@@ -77,12 +79,13 @@ struct rotwing_eff_sched_var_t {
float airspeed2; float airspeed2;
}; };
extern float roll_eff_scaling; extern int32_t rw_flap_offset;
extern float rotation_angle_setpoint_deg; extern float rotation_angle_setpoint_deg;
extern int16_t rotation_cmd; extern int16_t rotation_cmd;
extern float eff_sched_pusher_time; extern float eff_sched_pusher_time;
extern float roll_eff_slider;
extern void eff_scheduling_rotwing_init(void); extern void eff_scheduling_rotwing_init(void);
extern void eff_scheduling_rotwing_periodic(void); extern void eff_scheduling_rotwing_periodic(void);
sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
+7 -1
View File
@@ -87,12 +87,18 @@ float nav_hybrid_line_gain = 1.0f;
#define NAV_HYBRID_NAV_CIRCLE_DIST 40.f #define NAV_HYBRID_NAV_CIRCLE_DIST 40.f
#endif #endif
#ifdef NAV_HYBRID_POS_GAIN #ifdef GUIDANCE_INDI_POS_GAIN
float nav_hybrid_pos_gain = GUIDANCE_INDI_POS_GAIN;
#elif defined(NAV_HYBRID_POS_GAIN)
float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN; float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN;
#else #else
float nav_hybrid_pos_gain = 1.0f; float nav_hybrid_pos_gain = 1.0f;
#endif #endif
#if defined(NAV_HYBRID_POS_GAIN) && defined(GUIDANCE_INDI_POS_GAIN)
#warning "NAV_HYBRID_POS_GAIN and GUIDANCE_INDI_POS_GAIN serve similar purpose and are both defined, using GUIDANCE_INDI_POS_GAIN"
#endif
#ifndef GUIDANCE_INDI_HYBRID #ifndef GUIDANCE_INDI_HYBRID
bool force_forward = 0.0f; bool force_forward = 0.0f;
#endif #endif
sw/airborne/modules/rotwing_drone/rotwing_state.c
+409 -602
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/rotwing_drone/rotwing_state.h
+54 -58
View File
@@ -28,70 +28,66 @@
#include "std.h" #include "std.h"
/** Rotwing States enum rotwing_states_t {
*/ ROTWING_STATE_FORCE_HOVER,
#define ROTWING_STATE_HOVER 0 // Wing is skewed in 0 degrees (quad) ROTWING_STATE_REQUEST_HOVER,
#define ROTWING_STATE_SKEWING 1 // WIng is skewing ROTWING_STATE_FORCE_FW,
#define ROTWING_STATE_FW 2 // Wing is skewed at 90 degrees (fixed wing), hover motors have full authority ROTWING_STATE_REQUEST_FW,
#define ROTWING_STATE_FW_HOV_MOT_IDLE 3 // Wing is skewed at 90 degrees (fixed wing), hover motors are forced to idle ROTWING_STATE_FREE,
#define ROTWING_STATE_FW_HOV_MOT_OFF 4 // Wing is skewed at 90 degrees (fixed wubg), hover motors are switched off
#define ROTWING_STATE_FREE 5 // This is a desired state for which the controller has to decide the desired state itself
/** Rotwing Configurations
*/
#define ROTWING_CONFIGURATION_HOVER 0 // UAV is in hover
#define ROTWING_CONFIGURATION_HYBRID 1 // UAV can fly forward and hover if airspeed low, hover motors on
#define ROTWING_CONFIGURATION_EFFICIENT 2 // UAV flies efficiently forward (FW)
#define ROTWING_CONFIGURATION_FREE 3 // UAV switched between efficient and hybrid dependent on deceleration
struct RotwingState {
uint8_t current_state;
uint8_t desired_state;
uint8_t requested_config;
}; };
#define ROTWING_STATE_WING_QUAD_SETTING 0 // Wing skew at 0 degrees union rotwing_bitmask_t {
#define ROTWING_STATE_WING_SCHEDULING_SETTING 1 // Wing skew handled by airspeed scheduler uint16_t value;
#define ROTWING_STATE_WING_FW_SETTING 2 // Wing skew at 90 degrees struct {
bool skew_angle_valid : 1; // Skew angle didn't timeout
#define ROTWING_STATE_PITCH_QUAD_SETTING 0 // Pitch at prefered hover bool hover_motors_enabled : 1; // Hover motors command is enabled
#define ROTWING_STATE_PITCH_TRANSITION_SETTING 1 // Pitch scheduled bool hover_motors_idle : 1; // Hover motors are idling (throttle < IDLE_THROTTLE)
#define ROTWING_STATE_PITCH_FW_SETTING 2 // Pitch at prefered forward bool hover_motors_running : 1; // Hover motors are running (RPM >= MIN_RPM)
bool pusher_motor_running : 1; // Pusher motor is running (RPM >= MIN_RPM)
struct RotWingStateSettings { bool skew_forced : 1; // Skew angle is forced
uint8_t wing_scheduler; };
bool hover_motors_active;
bool hover_motors_disable;
bool force_forward;
uint8_t preferred_pitch;
bool stall_protection;
float max_v_climb;
float max_v_descend;
float nav_max_speed;
}; };
struct RotWingStateSkewing { struct rotwing_state_t {
float wing_angle_deg_sp; // Wing angle setpoint in deg /* Control */
float wing_angle_deg; // Wing angle from sensor in deg enum rotwing_states_t state; // Current state
int32_t servo_pprz_cmd; // Wing rotation servo pprz cmd enum rotwing_states_t nav_state; // Desired navigation state (requested only by NAV and can be overruled by RC)
bool airspeed_scheduling; // Airspeed scheduling on or off bool hover_motors_enabled; // Hover motors enabled (> idle throttle)
bool force_rotation_angle; // Setting to force wing_angle_deg_sp
/* Skew */
float sp_skew_angle_deg; // Setpoint skew angle in degrees
float ref_model_skew_angle_deg; // Reference model skew angle in degrees
float meas_skew_angle_deg; // Measured skew angle in degrees
float meas_skew_angle_time; // Time of the last skew angle measurement
bool force_skew; // Force skew angle to a certain value by the GCS
int16_t skew_cmd; // Skewing command in pprz values
/* Airspeeds */
float fw_min_airspeed; // Minimum airspeed (stall+margin)
float cruise_airspeed; // Airspeed for cruising
float min_airspeed; // Minimum airspeed for bounding
float max_airspeed; // Maximum airspeed for bounding
/* RPM measurements*/
int32_t meas_rpm[5]; // Measured RPM of the hover and pusher motors
float meas_rpm_time[5]; // Time of the last RPM measurement
/* Sim failures */
bool fail_skew_angle; // Skew angle sensor failure
bool fail_hover_motor; // Hover motor failure
bool fail_pusher_motor; // Pusher motor failure
}; };
extern struct rotwing_state_t rotwing_state;
extern struct RotwingState rotwing_state; void rotwing_state_init(void);
extern struct RotWingStateSettings rotwing_state_settings; void rotwing_state_periodic(void);
extern struct RotWingStateSkewing rotwing_state_skewing; bool rotwing_state_hover_motors_running(void);
bool rotwing_state_pusher_motor_running(void);
bool rotwing_state_skew_angle_valid(void);
extern float rotwing_state_max_hover_speed; void rotwing_state_set(enum rotwing_states_t state);
bool rotwing_state_choose_circle_direction(uint8_t wp_id);
extern bool hover_motors_active; void rotwing_state_set_transition_wp(uint8_t wp_id);
extern bool bool_disable_hover_motors; void rotwing_state_update_WP_height(uint8_t wp_id, float height);
extern void init_rotwing_state(void);
extern void periodic_rotwing_state(void);
extern void request_rotwing_state(uint8_t state);
extern void rotwing_request_configuration(uint8_t configuration);
extern void rotwing_state_skew_actuator_periodic(void);
extern bool rotwing_state_hover_motors_running(void);
#endif // ROTWING_STATE_H #endif // ROTWING_STATE_H
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 27ddd6516e...3f994e7f9b
sw/ground_segment/python/rotwing_visualizer/rotwing_vis_o3d_app.py
+4 -4
View File
@@ -70,14 +70,14 @@ class AHRSRefQuatMessage(object):
class RotWingControllerMessage(object): class RotWingControllerMessage(object):
def __init__(self, msg): def __init__(self, msg):
self.wing_angle_deg = msg['wing_angle_deg'] self.meas_skew_angle_deg = msg['meas_skew_angle']
self.wing_angle_deg_sp = msg['wing_angle_deg_sp'] self.sp_skew_angle_deg = msg['sp_skew_angle']
def get_wing_angle(self): def get_wing_angle(self):
return np.deg2rad(float(self.wing_angle_deg)) return np.deg2rad(float(self.meas_skew_angle_deg))
def get_wing_angle_sp(self): def get_wing_angle_sp(self):
return np.deg2rad(float(self.wing_angle_deg_sp)) return np.deg2rad(float(self.sp_skew_angle_deg))
class ActuatorsMessage(object): class ActuatorsMessage(object):
def __init__(self, msg): def __init__(self, msg):