339 changes: 339 additions & 0 deletions conf/airframes/tudelft/rot_wing_v3_static.xml
View file

Large diffs are not rendered by default.

303 changes: 303 additions & 0 deletions conf/airframes/tudelft/rot_wing_v3_static_rotation_test.xml
View file
@@ -0,0 +1,303 @@
<!-- This is a Nedderdrone with Trailing edge motors
* Airframe: TUD00???
* Autopilot: Pixhawk 4 props and 1 pusher prop
* Datalink: Herelink
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->

<airframe name="RotatingWingV3">
<description>RotatingWingV3</description>

<firmware name="rotorcraft">
<target NAME="ap" BOARD="cube_orange">
<configure VALUE="500" NAME="PERIODIC_FREQUENCY"/>
<configure VALUE="SWD" NAME="FLASH_MODE"/>
<module TYPE="sbus" NAME="radio_control">
<configure VALUE="UART3" NAME="SBUS_PORT"/>
</module>
<module NAME="sys_mon"/>
<module NAME="flight_recorder"/>
<!--<module NAME="adc_generic">
<configure VALUE="ADC_4" NAME="ADC_CHANNEL_GENERIC1"/>
<configure VALUE="ADC_5" NAME="ADC_CHANNEL_GENERIC2"/>
</module>-->

<module TYPE="ms45xx_i2c" NAME="airspeed">
<define NAME="USE_I2C2"/>
<define VALUE="i2c2" NAME="MS45XX_I2C_DEV"/>
</module>
<module name="imu" type="cube"/>
<!--<module name="wing_rotation_controller"/>-->
<define VALUE="RADIO_AUX1" NAME="RADIO_TH_HOLD"/>
<define VALUE="RADIO_AUX2" NAME="RADIO_FMODE"/>
<define VALUE="RADIO_AUX3" NAME="RADIO_FBW_MODE"/>
<define VALUE="RADIO_AUX1" NAME="RADIO_KILL_SWITCH"/>
<define VALUE="MAG_LIS3MDL_SENDER_ID" NAME="INS_EKF2_MAG_ID"/>
<define VALUE="IMU_CUBE1_ID" NAME="INS_EKF2_ACCEL_ID"/>
<define VALUE="IMU_CUBE1_ID" NAME="INS_EKF2_GYRO_ID"/>
</target>

<target name="nps" board="pc">
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>

<!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
</target>

<module TYPE="transparent" NAME="telemetry">
<configure VALUE="B115200" NAME="MODEM_BAUD"/>
</module>

<module TYPE="uavcan" NAME="actuators">
<configure VALUE="TRUE" NAME="UAVCAN_USE_CAN1"/>
<configure VALUE="FALSE" NAME="UAVCAN_USE_CAN2"/>
</module>

<module TYPE="pwm" NAME="actuators"/>

<module name="imu" type="cube"/>

<module name="gps" type="ublox">
<configure name="GPS_PORT" value="UART4"/>
<configure name="UBX_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>

<module TYPE="indi" NAME="stabilization">
<configure name="INDI_NUM_ACT" value="4"/>
</module>

<module name="guidance" type="indi_hybrid"/>

<module TYPE="ekf2" NAME="ins"/>

<module NAME="air_data"/>

<module name="ctrl_eff_sched_rot_wing_v3"/>
<module name="wing_rotation_controller">
<define name="WING_ROTATION_POSITION_ADC_0" value="45950"/>
<define name="WING_ROTATION_POSITION_ADC_90" value="18800"/>
<define name="WING_ROTATION_DEADZONE_PPRZ_CMD" value="500"/>
<define name="WING_ROTATION_P_GAIN" value="-50000"/>
<define name="WING_ROTATION_MAX_CMD" value="9600"/>
</module>

<module NAME="mag_lis3mdl">
<define VALUE="TRUE" NAME="MODULE_LIS3MDL_UPDATE_AHRS"/>
<configure VALUE="I2C1" NAME="MAG_LIS3MDL_I2C_DEV"/>
</module>

<module name="sys_id_doublet">
<define name="DOUBLET_AXES" value="{0,1,2,3}"/>
<define name="DOUBLET_RADIO_CHANNEL" value="9"/>
</module>
<!--<module name="sys_id_chirp">
<define name="CHIRP_AXES" value="{0,1,2,3}"/>
<define name="CHIRP_RADIO_CHANNEL" value="9"/>
</module>-->
</firmware>

<servos DRIVER="Pwm">
<servo NO="0" NEUTRAL="1500" NAME="SERVO_1" MIN="1000" MAX="2000"/>
<servo NO="1" NEUTRAL="1500" NAME="SERVO_2" MIN="1000" MAX="2000"/>
<servo NO="2" NEUTRAL="1500" NAME="SERVO_3" MIN="1000" MAX="2000"/>
<servo NO="3" NEUTRAL="1500" NAME="SERVO_4" MIN="1000" MAX="2000"/>
<servo NO="4" NEUTRAL="1500" NAME="SERVO_5" MIN="1000" MAX="2000"/>
<servo NO="5" NEUTRAL="1500" NAME="SERVO_6" MIN="1000" MAX="2000"/>
<servo NO="6" NEUTRAL="1500" NAME="SERVO_7" MIN="1000" MAX="2000"/>
</servos>

<servos DRIVER="Uavcan1">
<servo NO="0" NEUTRAL="600" NAME="MOTOR_FRONT" MIN="0" MAX="8191"/>
<servo NO="1" NEUTRAL="600" NAME="MOTOR_RIGHT" MIN="0" MAX="8191"/>
<servo NO="2" NEUTRAL="600" NAME="MOTOR_BACK" MIN="0" MAX="8191"/>
<servo NO="3" NEUTRAL="600" NAME="MOTOR_LEFT" MIN="0" MAX="8191"/>
<servo NO="4" NEUTRAL="600" NAME="MOTOR_PUSH" MIN="0" MAX="8191"/>
<!-- <servo NO="9" NEUTRAL="0" NAME="SERVO_ELEVATOR" MIN="4000" MAX="-4000"/> -->
<servo NO="9" NEUTRAL="2986" NAME="SERVO_ELEVATOR" MIN="8191" MAX="-6794"/>
<servo NO="10" NEUTRAL="0" NAME="SERVO_RUDDER" MIN="-8191" MAX="8191"/>
</servos>

<commands>
<axis NAME="ROLL" FAILSAFE_VALUE="0"/>
<axis NAME="PITCH" FAILSAFE_VALUE="0"/>
<axis NAME="YAW" FAILSAFE_VALUE="0"/>
<axis NAME="THRUST" FAILSAFE_VALUE="0"/>
</commands>

<rc_commands>
<set VALUE="@THROTTLE" COMMAND="THRUST"/>
<set VALUE="@ROLL" COMMAND="ROLL"/>
<set VALUE="@PITCH" COMMAND="PITCH"/>
<set VALUE="@YAW" COMMAND="YAW"/>
</rc_commands>

<command_laws>
<let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<set VALUE="-9600" SERVO="MOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : @THRUST)" SERVO="MOTOR_RIGHT"/>
<set VALUE="-9600" SERVO="MOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : @THRUST)" SERVO="MOTOR_LEFT"/>
<set VALUE="-9600" SERVO="MOTOR_PUSH"/>
<set VALUE="0" SERVO="SERVO_ELEVATOR"/>
<set VALUE="0" SERVO="SERVO_RUDDER"/>
<set VALUE="wing_rotation.servo_pprz_cmd" SERVO="SERVO_1"/>
</command_laws>

<section NAME="MISC">
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 13.86 * adc)"/>
<define VALUE="TRUE" NAME="NO_RC_THRUST_LIMIT"/>
<define VALUE="3.5" NAME="NAV_CLIMB_VSPEED"/>
<define VALUE="-0.5" NAME="NAV_DESCEND_VSPEED"/>
<!--<define VALUE="50.0" NAME="ARRIVED_AT_WAYPOINT"/>-->
<define VALUE="20" NAME="NO_GPS_LOST_WITH_DATALINK_TIME"/>
<define VALUE="TRUE" NAME="NO_GPS_LOST_WITH_RC_VALID"/>
<define name="UART4_CR2" value="(USART_CR2_STOP1_BITS | USART_CR2_SWAP)"/>
</section>

<section name="IMU" prefix="IMU_">
<!--<define name="MAG_CALIB" value="{{.abi_id=3, .neutral={1696,-4095,3641}, .scale={{15263,26021,26309},{25440,40858,41277}},.current_scale={0,0,0}}}"/>-->
<define name="MAG_CALIB" value="{{.abi_id=3, .neutral={1638,-4050,3653}, .scale={{7385,2497,26093},{12188,3877,40843}},.current_scale={0,0,0}}}"/>


<define name="ACCEL_CALIB" value="{{.abi_id=20, .neutral={-57,-20,-6}, .scale={{3374,11057,64239},{647,2261,13135}}}, {.abi_id=21, .neutral={-35,-20,9}, .scale={{50356,47219,17359},{10281,9653,3561}}}, {.abi_id=22, .neutral={-17,-10,24}, .scale={{22979,34687,9697},{2353,3547,1001}}}}"/>

<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>

<section PREFIX="STABILIZATION_ATTITUDE_" NAME="STABILIZATION_ATTITUDE">
<define VALUE="60." UNIT="deg" NAME="SP_MAX_PHI"/>
<define VALUE="60." UNIT="deg" NAME="SP_MAX_THETA"/>
<define VALUE="90." UNIT="deg/s" NAME="SP_MAX_R"/>
<define VALUE="200" NAME="DEADBAND_R"/>
<define VALUE="45" UNIT="deg" NAME="SP_PSI_DELTA_LIMIT"/>
<define VALUE="800" UNIT="deg/s" NAME="REF_OMEGA_P"/>
<define VALUE="0.85" NAME="REF_ZETA_P"/>
<define VALUE="300." UNIT="deg/s" NAME="REF_MAX_P"/>
<define VALUE="RadOfDeg(7000.)" NAME="REF_MAX_PDOT"/>
<define VALUE="800" UNIT="deg/s" NAME="REF_OMEGA_Q"/>
<define VALUE="0.85" NAME="REF_ZETA_Q"/>
<define VALUE="300." UNIT="deg/s" NAME="REF_MAX_Q"/>
<define VALUE="RadOfDeg(7000.)" NAME="REF_MAX_QDOT"/>
<define VALUE="500" UNIT="deg/s" NAME="REF_OMEGA_R"/>
<define VALUE="0.85" NAME="REF_ZETA_R"/>
<define VALUE="180." UNIT="deg/s" NAME="REF_MAX_R"/>
<define VALUE="RadOfDeg(1800.)" NAME="REF_MAX_RDOT"/>
<define VALUE="500" NAME="PHI_PGAIN"/>
<define VALUE="230" NAME="PHI_DGAIN"/>
<define VALUE="10" NAME="PHI_IGAIN"/>
<define VALUE="500" NAME="THETA_PGAIN"/>
<define VALUE="230" NAME="THETA_DGAIN"/>
<define VALUE="10" NAME="THETA_IGAIN"/>
<define VALUE="700" NAME="PSI_PGAIN"/>
<define VALUE="200" NAME="PSI_DGAIN"/>
<define VALUE="10" NAME="PSI_IGAIN"/>
<define VALUE="0" NAME="PHI_DDGAIN"/>
<define VALUE="0" NAME="THETA_DDGAIN"/>
<define VALUE="0" NAME="PSI_DDGAIN"/>
</section>

<section PREFIX="STABILIZATION_INDI_" NAME="STABILIZATION_ATTITUDE_INDI">
<define VALUE="{0.00, -12, 0.0, 12}" NAME="G1_ROLL"/>
<define VALUE="{3.7, 0.0, -3.7, 0.0}" NAME="G1_PITCH"/>
<define VALUE="{-0.49, 0.45, -0.49, 0.45}" NAME="G1_YAW"/>
<define VALUE="{-0.55, -0.45, -0.55, -0.45}" NAME="G1_THRUST"/>
<define VALUE="{0.0, 0.0, 0.0, 0.0}" NAME="G2"/>
<!--<define VALUE="{0.00, -30.0, 0.0, 30.0}" NAME="G1_ROLL"/>
<define VALUE="{6.0, 0.0, -6.0, 0.0}" NAME="G1_PITCH"/>
<define VALUE="{-1.1, 1.1, -1.1, 1.1}" NAME="G1_YAW"/>
<define VALUE="{-0.5, -0.5, -0.5, -0.5}" NAME="G1_THRUST"/>
<define VALUE="{0.0, 0.0, 0.0, 0.0}" NAME="G2"/>-->
<define VALUE="50.0" NAME="REF_ERR_P"/>
<define VALUE="30.0" NAME="REF_ERR_Q"/>
<define VALUE="60.0" NAME="REF_ERR_R"/>
<define VALUE="6.0" NAME="REF_RATE_P"/>
<define VALUE="6.0" NAME="REF_RATE_Q"/>
<define VALUE="6.0" NAME="REF_RATE_R"/>
<define VALUE="50.0" UNIT="deg/s" NAME="MAX_R"/>
<define VALUE="20.0" NAME="FILT_CUTOFF_P"/>
<define VALUE="20.0" NAME="FILT_CUTOFF_Q"/>
<define VALUE="20.0" NAME="FILT_CUTOFF_R"/>
<define VALUE="2.5" NAME="FILT_CUTOFF"/>
<define VALUE="2.5" NAME="FILT_CUTOFF_RDOT"/>
<define VALUE="2.5" NAME="ESTIMATION_FILT_CUTOFF"/>
<define VALUE="TRUE" NAME="FILTER_YAW_RATE"/>
<define VALUE="{0.03, 0.025, 0.03, 0.025}" NAME="ACT_DYN"/>
<define VALUE="{0, 0, 0, 0}" NAME="ACT_IS_SERVO"/>
<define VALUE="{1000, 1000, 1, 100}" NAME="WLS_PRIORITIES"/>
<define VALUE="FALSE" NAME="USE_ADAPTIVE"/>
<define VALUE="0.001" NAME="ADAPTIVE_MU"/>
</section>

<section PREFIX="GUIDANCE_V_" NAME="GUIDANCE_V">
<define VALUE="310" NAME="HOVER_KP"/>
<define VALUE="130" NAME="HOVER_KD"/>
<define VALUE="10" NAME="HOVER_KI"/>
<define VALUE="0.42" NAME="NOMINAL_HOVER_THROTTLE"/>
<define VALUE="FALSE" NAME="ADAPT_THROTTLE_ENABLED"/>
</section>

<section PREFIX="GUIDANCE_H_" NAME="GUIDANCE_H">
<define VALUE="30" UNIT="deg" NAME="MAX_BANK"/>
<define VALUE="TRUE" NAME="USE_SPEED_REF"/>
<define VALUE="60" NAME="PGAIN"/>
<define VALUE="100" NAME="DGAIN"/>
<define VALUE="0" NAME="AGAIN"/>
<define VALUE="20" NAME="IGAIN"/>
</section>

<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<define name="POS_GAIN" value="0.5"/><!--tuned at 10m/s ///1.0 1.5-->
<define name="POS_GAINZ" value="0.5"/> <!--tuned at 10m/s ///1.7 1.0-->
<define name="SPEED_GAIN" value="1.0"/> <!--tuned at 10m/s ///2.0 1.4-->
<define name="SPEED_GAINZ" value="1.0"/> <!--tuned at 10m/s ///2.1 2.4-->
<define name="FILTER_CUTOFF" value="2.5"/>
<define name="HEADING_BANK_GAIN" value="5."/>
<define name="MAX_AIRSPEED" value="10.0"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<!--<define name="MIN_THROTTLE" value="0."/>
<define name="MIN_PITCH" value="-35."/>
<define name="MAX_PITCH" value="35."/>
<define name="MIN_THROTTLE_FWD" value="0."/>
<define name="FWD_SIDESLIP_GAIN" value="0.32"/>
<define name="LINE_GAIN" value="0.085"/>-->
</section>

<section name="FORWARD">
<!-- This is the pitch angle that the drone will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="0.0" unit="deg"/>
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/>
</section>

<section NAME="AUTOPILOT">
<define VALUE="AP_MODE_ATTITUDE_DIRECT" NAME="MODE_MANUAL"/>
<define VALUE="AP_MODE_HOVER_Z_HOLD" NAME="MODE_AUTO1"/>
<define VALUE="AP_MODE_NAV" NAME="MODE_AUTO2"/>
<define VALUE="AP_MODE_NAV" NAME="MODE_STARTUP"/>
</section>

<section NAME="BAT">
<define VALUE="18.0" UNIT="V" NAME="CATASTROPHIC_BAT_LEVEL"/>
<define VALUE="18.6" UNIT="V" NAME="CRITIC_BAT_LEVEL"/>
<define VALUE="19.2" UNIT="V" NAME="LOW_BAT_LEVEL"/>
<define VALUE="25.2" UNIT="V" NAME="MAX_BAT_LEVEL"/>
<define VALUE="6" NAME="BAT_NB_CELLS"/>
</section>

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwingv3" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="4"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
</airframe>
497 changes: 497 additions & 0 deletions conf/airframes/tudelft/rot_wing_v3b.xml
View file

Large diffs are not rendered by default.

302 changes: 302 additions & 0 deletions conf/airframes/tudelft/rotating_wing.xml
View file
@@ -0,0 +1,302 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">

<!-- This is a Nedderdrone
* Airframe: TUD00328
* Autopilot: Pixhawk 4
* FBW: Pixhawk 4
* Actuators: 12x T-Motor ESC + Motors and 8x Servos (all CAN)
* Datalink: Doodlelabs 2.4GHz
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->

<airframe name="RotatingWing">
<description>RotatingWing</description>

<firmware name="rotorcraft">
<target name="ap" board="cube_orange">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="FLASH_MODE" value="SWD"/>
<!--configure name="RTOS_DEBUG" value="1"/-->

<module name="radio_control" type="sbus">
<configure name="SBUS_PORT" value="UART3"/>
</module>

<module name="airspeed" type="ms45xx_i2c">
<define name="USE_I2C4"/>
<define name="MS45XX_I2C_DEV" value="i2c4"/>
</module>

<!--module name="scheduling_indi_simple"/-->

<module name="sys_mon"/>

<!-- Forward FuelCell data back to the GCS -->
<!--module name="generic_uart_sensor"/-->

<!-- Logger -->
<module name="flight_recorder"/>

<!--define name="ADC_CURRENT_DISABLE" value="TRUE"/-->
<module name="adc_generic">
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_4"/>
<configure name="ADC_CHANNEL_GENERIC2" value="ADC_5"/>
</module>

<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>

<!-- Use the external mag (not in NPS target as then it needs to listen to all) -->
<define name="INS_EKF2_MAG_ID" value="MAG_LIS3MDL_SENDER_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
</target>

<target name="nps" board="pc">
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>

<!--module name="scheduling_indi_simple"/-->

<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
</module>

<!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
</target>

<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
</module>

<module name="actuators" type="uavcan">
<configure name="UAVCAN_USE_CAN1" value="TRUE"/>
<configure name="UAVCAN_USE_CAN2" value="FALSE"/>
</module>
<module name="actuators" type="pwm"/>
<module name="imu" type="cube"/>
<module name="gps" type="ublox"/>
<module name="gps" type="ubx_ucenter"/>
<module name="stabilization" type="indi"/>
<module name="ins" type="ekf2" />

<module name="air_data"/>

<!-- Internal MAG -->
<!--module name="mag_ist8310">
<define name="MODULE_IST8310_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_IST8310_I2C_DEV" value="I2C3"/>
</module-->
<!-- External MAG on GPS -->
<module name="mag_lis3mdl">
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
<define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
<define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
</module>
<!--module name="lidar" type="tfmini">
<configure name="TFMINI_PORT" value="UART4"/>
<configure name="USE_TFMINI_AGL" value="FALSE"/>
</module-->

<module name="motor_mixing"/>
</firmware>

<!-- PWM (servos) -->
<servos driver="Pwm">
<servo name="SERVO_1" no="0" min="1000" neutral="1100" max="2000"/>
<servo name="SERVO_2" no="1" min="1000" neutral="1100" max="2000"/>
<servo name="SERVO_3" no="2" min="1000" neutral="1100" max="2000"/>
<servo name="SERVO_4" no="3" min="1000" neutral="1100" max="2000"/>
<servo name="SERVO_5" no="4" min="1000" neutral="1100" max="2000"/>
<servo name="SERVO_6" no="5" min="1000" neutral="1100" max="2000"/>
<servo name="SERVO_7" no="6" min="1000" neutral="1100" max="2000"/>
</servos>

<!-- CAN BUS 1 (All motors) -->
<servos driver="Uavcan1">
<servo name="MOTOR_FWD" no="0" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_FRONT" no="1" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_RIGHT" no="2" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_BACK" no="3" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_LEFT" no="4" min="0" neutral="600" max="8191"/>
</servos>

<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>

<rc_commands>
<set command="THRUST" value="@THROTTLE" />
<set command="ROLL" value="@ROLL" />
<set command="PITCH" value="@PITCH" />
<set command="YAW" value="@YAW" />
</rc_commands>

<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TYPE" value="QUAD_PLUS"/>
<define name="REVERSE" value="true"/>
</section>

<command_laws>
<!--call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/-->
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>

<set servo="MOTOR_FWD" value="-9600"/>
<set servo="MOTOR_FRONT" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_RIGHT" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_BACK" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_LEFT" value="($th_hold? -9600 : actuators_pprz[3])"/>
</command_laws>

<section name="MISC">
<!--define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 18.9040120162 * adc)"/--><!-- TODO: verify/calibrate -->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>

<!-- Basic navigation settings -->
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
<define name="NAV_DESCEND_VSPEED" value="-0.5"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>

<!-- Avoid GPS loss behavior when having RC or datalink -->
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
</section>

<section name="IMU" prefix="IMU_">
<define name="MAG_CALIB" value="{{.abi_id=3, .neutral={-1401,3860,3409}, .scale={{1879,21097,37627},{3255,34295,62521}}}}"/>

<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>

<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="80." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>

<!-- reference -->
<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.)"/>

<!-- feedback -->
<define name="PHI_PGAIN" value="500"/>
<define name="PHI_DGAIN" value="230"/>
<define name="PHI_IGAIN" value="10"/>
<define name="THETA_PGAIN" value="500"/>
<define name="THETA_DGAIN" value="230"/>
<define name="THETA_IGAIN" value="10"/>
<define name="PSI_PGAIN" value="700"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>

<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="0"/>
</section>

<section prefix="STABILIZATION_INDI_" name="STABILIZATION_ATTITUDE_INDI">
<define name="G1_ROLL" value="{0.00, -13.6, 0.00, 13.6}"/>
<define name="G1_PITCH" value="{2.9, -0.7, -1.4, -0.7}"/>
<define name="G1_YAW" value="{0.3, -0.3, 0.3, -0.3}"/>
<define name="G1_THRUST" value="{-0.585, -0.585, -0.585, -0.585}"/>
<define name="G2" value="{-0.014, 0.014, -0.014, 0.014}"/>

<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="30.0"/>
<define name="REF_ERR_R" value="20.0"/>
<define name="REF_RATE_P" value="6.0"/>
<define name="REF_RATE_Q" value="6.0"/>
<define name="REF_RATE_R" value="6.0"/>

<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="50.0" unit="deg/s"/>

<!-- filter parameters -->
<define name="FILT_CUTOFF_P" value="20.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="20.0"/>

<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="2.5"/>
<define name="FILT_CUTOFF_RDOT" value="2.5"/>
<define name="ESTIMATION_FILT_CUTOFF" value="2.5"/>

<define name="FILTER_YAW_RATE" value="TRUE"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028}"/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 0}"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="TRUE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<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"/>
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="0"/>
<define name="IGAIN" value="20"/>
</section>

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,ail1,ail2,ail3,ail4,flap1,flap2,flap3,flap4" type="string[]"/>
<define name="JSBSIM_MODEL" value="nederdrone" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</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="BAT_NB_CELLS" value="4"/>
</section>

</airframe>
168 changes: 168 additions & 0 deletions conf/flight_plans/tudelft/rotating_wing25kg_EHVB.xml
View file
@@ -0,0 +1,168 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="60" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="500" name="Rotating wing Valkenburg" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
</header>
<waypoints>
<waypoint name="HOME" x="12.6" y="-48.7"/>
<waypoint name="CLIMB" x="62.6" y="-80.4"/>
<waypoint name="trans" x="100." y="100."/>
<waypoint name="decel" x="100." y="100."/>
<waypoint name="end_trans" x="100." y="100."/>
<waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/>
<!--<waypoint name="p1" lat="52.1674262" lon="4.4141448"/>-->
<waypoint name="p1" lat="52.1684116" lon="4.4149282"/>
<waypoint name="p2" lat="52.1675165" lon="4.4147158"/>
<waypoint name="p3" lat="52.1688983" lon="4.4139655"/>
<waypoint name="p4" lat="52.1687661" lon="4.4133220"/>
<waypoint name="circ" lat="52.1684116" lon="4.4149282"/>
<waypoint name="TD" lat="52.1681602" lon="4.4127708"/>
<waypoint name="APP" lat="52.1679567" lon="4.4136344"/>
<waypoint name="FOLLOW" lat="52.16850964562752" lon="4.413635008734417"/>
<waypoint lat="52.169189" lon="4.410820" name="C1"/>
<waypoint lat="52.168049" lon="4.406923" name="C2"/>
<waypoint lat="52.166515" lon="4.408235" name="C3"/>
<waypoint lat="52.163255" lon="4.407668" name="C4"/>
<waypoint lat="52.161908" lon="4.410082" name="C5"/>
<waypoint lat="52.162641" lon="4.416992" name="C6"/>
<waypoint lat="52.164861" lon="4.427268" name="C7"/>
<waypoint lat="52.170422" lon="4.427511" name="C8"/>
<waypoint lat="52.172276" lon="4.424011" name="C9"/>
</waypoints>
<sectors>
<sector color="red" name="Flyzone">
<corner name="C1"/>
<corner name="C2"/>
<corner name="C3"/>
<corner name="C4"/>
<corner name="C5"/>
<corner name="C6"/>
<corner name="C7"/>
<corner name="C8"/>
<corner name="C9"/>
</sector>
</sectors>
<modules>
<!--<module name="follow_me"/>-->
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<set value="false" var="force_forward"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="false" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="false" var="force_forward"/>
<exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>2" vmode="throttle"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="false" var="force_forward"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<set value="false" var="force_forward"/>
<stay wp="p1"/>
</block>
<block name="go_p2">
<set value="false" var="force_forward"/>
<call_once fun="nav_set_heading_deg(90)"/>
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="line_p1_p2">
<set value="false" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<stay wp="p2"/>
</block>
<block name="line_trans_fwd">
<set value="false" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_p1)"/>
<stay wp="end_trans"/>
</block>
<block name="line_trans_quad">
<set value="false" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_p1)"/>
<stay wp="end_trans"/>
</block>
<block name="route">
<set value="false" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/>
</block>
<block name="Oval">
<set value="false" var="force_forward"/>
<oval p1="p1" p2="p2" radius="100"/>
</block>
<block name="Circle_CW">
<set value="false" var="force_forward"/>
<circle radius="100" wp="circ"/>
</block>
<block name="Circle_CCW">
<set value="false" var="force_forward"/>
<circle radius="-100" wp="circ"/>
</block>
<block name="Circle_CW_fwd">
<set value="true" var="force_forward"/>
<circle radius="100" wp="circ"/>
</block>
<block name="Circle_CCW_fwd">
<set value="true" var="force_forward"/>
<circle radius="-100" wp="circ"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="false" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="false" var="force_forward"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="false" var="force_forward"/>
<exception cond="!(stateGetPositionEnu_f()->z > 12.0)" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="false" var="force_forward"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<exception cond="!(stateGetPositionEnu_f()->z > 2.0)" deroute="flare_low"/>
</block>
<block name="flare_low">
<set value="false" var="force_forward"/>
<!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<!-- <call_once fun="NavStartDetectGround()"/> -->
<!-- <stay climb="-0.5" vmode="climb" wp="TD"/> -->
</block>
<block name="landed">
<set value="false" var="force_forward"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
127 changes: 127 additions & 0 deletions conf/flight_plans/tudelft/rotating_wing_EHR8.xml
View file
@@ -0,0 +1,127 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="60" ground_alt="0" lat0="52.965523" lon0="4.474033" max_dist_from_home="600000" name="EHR8 plan" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="CLIMB" x="0.0" y="55.0"/>
<waypoint name="STDBY" x="16.0" y="55.0"/>
<waypoint name="p1" x="3.6" y="-139"/>
<waypoint name="TD" x="55.6" y="-10.9"/>
<waypoint name="FOLLOW" x="60.5" y="13.5"/>
<waypoint name="APP" x="40.5" y="13.5"/>
<!-- EHR8 -->
<waypoint name="C1" lat="52.96166667" lon="4.740277778"/>
<waypoint name="C2" lat="52.86666667" lon="4.716666667"/>
<waypoint name="C3" lat="52.78333333" lon="4.683333333"/>
<waypoint name="C4" lat="52.75" lon="4.533333333"/>
<waypoint name="C5" lat="52.775" lon="4.433333333"/>
<waypoint name="C6" lat="52.81666667" lon="4.35"/>
<waypoint name="C7" lat="53.08333333" lon="4.35"/>
<waypoint name="C8" lat="53.10277778" lon="4.515555556"/>
<waypoint name="C9" lat="53.04972222" lon="4.679444444"/>
</waypoints>
<sectors>
<sector color="orange" name="EHR8">
<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>
<!--sector color="red" name="RES">
<corner name="R1"/>
<corner name="R2"/>
<corner name="R3"/>
<corner name="R4"/>
</sector-->
</sectors>
<modules>
<module name="follow_me">
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_STDBY"/>
</module>
</modules>
<exceptions>
<exception cond="Or(!InsideEHR8(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Holding point"/>
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<set value="false" var="force_forward"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="false" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="false" var="force_forward"/>
<exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Approach APP"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.70" until="stage_time>2" vmode="throttle"/>
</block>
<block name="Approach APP">
<set value="false" var="force_forward"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="false" var="force_forward"/>
<stay wp="STDBY"/>
</block>
<block name="follow_module">
<set value="false" var="force_forward"/>
<stay pre_call="follow_me_set_wp(WP_FOLLOW, 0)" wp="FOLLOW"/>
</block>
<block name="Circle_CW">
<set value="false" var="force_forward"/>
<circle radius="100" wp="FOLLOW"/>
</block>
<block name="Circle_CCW">
<set value="false" var="force_forward"/>
<circle radius="-100" wp="FOLLOW"/>
</block>
<block name="Circle_CW_fwd">
<set value="true" var="force_forward"/>
<circle radius="100" wp="FOLLOW"/>
</block>
<block name="Circle_CCW_fwd">
<set value="true" var="force_forward"/>
<circle radius="-100" wp="FOLLOW"/>
</block>
<block name="Approach land">
<set value="false" var="force_forward"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
<exception cond="agl_dist_valid @AND !(agl_dist_value > 3.0)" deroute="Approach flare"/>
</block>
<block name="Approach flare">
<set value="false" var="force_forward"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call_once fun="NavStartDetectGround()"/>
</block>
<block name="landed">
<set value="false" var="force_forward"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
227 changes: 227 additions & 0 deletions conf/flight_plans/tudelft/rotating_wing_EHVB.xml
View file
@@ -0,0 +1,227 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="70" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="1070" name="Rotating wing Valkenburg" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
</header>
<waypoints>
<waypoint name="HOME" x="12.6" y="-48.7"/>
<waypoint name="CLIMB" x="62.6" y="-80.4"/>
<waypoint name="trans" x="100." y="100."/>
<waypoint name="decel" x="100." y="100."/>
<waypoint name="end_trans" x="100." y="100."/>
<waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/>
<waypoint name="begin_trans" lat="52.168412" lon="4.4149282"/>
<!--<waypoint name="p1" lat="52.1674262" lon="4.4141448"/>-->
<waypoint name="p1" lat="52.1672408" lon="4.4144528"/>
<waypoint name="p2" lat="52.1682897" lon="4.4138441"/>
<waypoint name="p3" lat="52.1687878" lon="4.4155648"/>
<waypoint name="p4" lat="52.1678559" lon="4.4164701"/>
<waypoint name="circ" lat="52.1684116" lon="4.4149282"/>
<waypoint name="TD" lat="52.1681602" lon="4.4127708"/>
<waypoint name="APP" lat="52.1679567" lon="4.4136344"/>
<waypoint name="FOLLOW" lat="52.16850964562752" lon="4.413635008734417"/>
<waypoint lat="52.169189" lon="4.410820" name="C1"/>
<waypoint lat="52.168049" lon="4.406923" name="C2"/>
<waypoint lat="52.166515" lon="4.408235" name="C3"/>
<waypoint lat="52.163255" lon="4.407668" name="C4"/>
<waypoint lat="52.161908" lon="4.410082" name="C5"/>
<waypoint lat="52.162641" lon="4.416992" name="C6"/>
<waypoint lat="52.164861" lon="4.427268" name="C7"/>
<waypoint lat="52.170422" lon="4.427511" name="C8"/>
<waypoint lat="52.172276" lon="4.424011" name="C9"/>
</waypoints>
<sectors>
<sector color="red" name="Flyzone">
<corner name="C1"/>
<corner name="C2"/>
<corner name="C3"/>
<corner name="C4"/>
<corner name="C5"/>
<corner name="C6"/>
<corner name="C7"/>
<corner name="C8"/>
<corner name="C9"/>
</sector>
</sectors>
<modules>
<!--<module name="follow_me"/>-->
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<set value="false" var="force_forward"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Standby"/>
<call_once fun="autopilot_set_in_flight(true)"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>2" vmode="throttle"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Approach APP">
<set value="false" var="force_forward"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<!--<block name="follow_module">
<set value="false" var="force_forward"/>
<stay pre_call="follow_me_set_wp(WP_FOLLOW, 0)" wp="FOLLOW"/>
</block>-->
<block name="stay_begin-trans">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<stay wp="begin_trans" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="line_trans_fwd">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<call_once fun="NavSetWaypointHere(WP_begin_trans)"/>
<go wp="end_trans"/>
<deroute block="end_transition"/>
</block>
<block name="end_transition">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<stay wp="end_trans"/>
</block>
<block name="transition_CW">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<stay wp="end_trans"/>
<exception cond="rot_wing_a.transitioned @AND (wing_rotation.wing_angle_deg > 75.0)" deroute="Circle_CW_fwd"/>
</block>
<block name="transition_CCW">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<stay wp="end_trans"/>
<exception cond="rot_wing_a.transitioned @AND (wing_rotation.wing_angle_deg > 75.0)" deroute="Circle_CCW_fwd"/>
</block>
<block name="route">
<set value="true" var="force_forward"/>
<set value="30" var="nav_max_speed"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<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">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<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">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<circle radius="100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<circle radius="-100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<circle radius="150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CCW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<circle radius="-150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Transition_quad">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(true)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<exception cond="!(stateGetPositionEnu_f()->z > 12.0)" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(stateGetPositionEnu_f()->z > 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND !(agl_dist_value > 0.24)" deroute="flare_low"/>
</block>
<block name="flare_low">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler_nav(false)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
210 changes: 210 additions & 0 deletions conf/flight_plans/tudelft/rotating_wing_Harde.xml
View file
@@ -0,0 +1,210 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="180" ground_alt="60" lat0="52.3931434" lon0="5.9205099" max_dist_from_home="9000" name="Rotating wing Harde" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
</header>
<waypoints>
<waypoint name="HOME" x="12.6" y="-48.7"/>
<waypoint name="CLIMB" x="62.6" y="-80.4"/>
<waypoint name="trans" x="100." y="100."/>
<waypoint name="decel" x="100." y="100."/>
<waypoint name="end_trans" x="100." y="100."/>
<waypoint name="STDBY" lat="52.3940343" lon="5.9229692"/>
<waypoint name="begin_trans" lat="52.3941568" lon="5.9231750"/>
<waypoint name="p1" lat="52.3963465" lon="5.9267684"/>
<waypoint name="p2" lat="52.3994182" lon="5.9333586"/>
<waypoint name="p3" lat="52.4033999" lon="5.9292886"/>
<waypoint name="p4" lat="52.3999490" lon="5.9231485"/>
<waypoint name="circ" lat="52.39535877874752" lon="5.924914921610622"/>
<waypoint name="TD" lat="52.39306725044085" lon="5.92060993591204"/>
<waypoint lat="52.4034734" lon="5.9067552" name="C1"/>
<waypoint lat="52.3878517" lon="5.9212118" name="C2"/>
<waypoint lat="52.4054122" lon="5.9581765" name="C3"/>
<waypoint lat="52.4179727" lon="6.0083051" name="C4"/>
<waypoint lat="52.442091206182084" lon="5.987117813607398" name="C5"/>
</waypoints>
<sectors>
<sector color="red" name="Flyzone">
<corner name="C1"/>
<corner name="C2"/>
<corner name="C3"/>
<corner name="C4"/>
<corner name="C5"/>
</sector>
</sectors>
<modules>
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby')) @AND !(nav_block == IndexOfBlock('Link_loss'))" deroute="Link_loss"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<set value="false" var="force_forward"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>2" vmode="throttle"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Link_loss">
<set value="false" var="force_forward"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="stay_begin-trans">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<stay wp="begin_trans" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="line_trans_fwd">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<call_once fun="NavSetWaypointHere(WP_begin_trans)"/>
<go wp="end_trans"/>
<deroute block="end_transition"/>
</block>
<block name="end_transition">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<stay wp="end_trans"/>
</block>
<block name="transition_CW">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<stay wp="end_trans"/>
<exception cond="rot_wing_a.transitioned @AND (wing_rotation.wing_angle_deg > 75.0)" deroute="Circle_CW_fwd"/>
</block>
<block name="transition_CCW">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<stay wp="end_trans"/>
<exception cond="rot_wing_a.transitioned @AND (wing_rotation.wing_angle_deg > 75.0)" deroute="Circle_CCW_fwd"/>
</block>
<block name="route">
<set value="true" var="force_forward"/>
<set value="30" var="nav_max_speed"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<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">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<go wp="p2"/>
<go wp="p3"/>
<go wp="p4"/>
<go wp="p1"/>
<deroute block="small_route"/>
</block>
<block name="Circle_CW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<circle radius="100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Circle_CCW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<circle radius="-100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<circle radius="150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CCW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<circle radius="-150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Transition_quad">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<exception cond="!(stateGetPositionEnu_f()->z > 12.0)" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(stateGetPositionEnu_f()->z > 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND !(agl_dist_value > 0.24)" deroute="flare_low"/>
</block>
<block name="flare_low">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
161 changes: 161 additions & 0 deletions conf/flight_plans/tudelft/rotating_wing_terschelling.xml
View file
@@ -0,0 +1,161 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="60" ground_alt="0" lat0="53.398848" lon0="5.258644" max_dist_from_home="5000" name="Rotating wing Terschelling" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
</header>
<waypoints>
<waypoint name="HOME" x="-657.7" y="-25.2"/>
<waypoint name="CLIMB" x="-476.9" y="77.1"/>
<waypoint name="STDBY" x="-688.0" y="13.9"/>
<waypoint name="p1" x="-1047.6" y="27.2"/>
<waypoint name="p2" x="-1091.2" y="272.9"/>
<waypoint name="p3" x="-718.0" y="345.1"/>
<waypoint name="p4" x="-641.6" y="127.3"/>
<waypoint name="TD" x="-472.8" y="169.5"/>
<waypoint name="FOLLOW" x="-593.9" y="379.7"/>
<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"/>
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<set value="false" var="force_forward"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="false" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="false" var="force_forward"/>
<exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>2" vmode="throttle"/>
</block>
<!-- <block name="Approach APP">
<set value="false" var="force_forward"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> -->
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="false" var="force_forward"/>
<stay wp="STDBY"/>
</block>
<block name="follow_module">
<set value="false" var="force_forward"/>
<stay pre_call="follow_me_set_wp(WP_FOLLOW, 0)" wp="FOLLOW"/>
</block>
<block name="stay_p1">
<set value="false" var="force_forward"/>
<stay wp="p1"/>
</block>
<block name="go_p2">
<set value="false" var="force_forward"/>
<call_once fun="nav_set_heading_deg(90)"/>
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="line_p1_p2">
<set value="false" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<stay wp="p2"/>
</block>
<block name="route">
<set value="false" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/>
</block>
<block name="Oval">
<set value="false" var="force_forward"/>
<oval p1="p1" p2="p2" radius="100"/>
</block>
<block name="Circle_CW">
<set value="false" var="force_forward"/>
<circle radius="100" wp="p1"/>
</block>
<block name="Circle_CCW">
<set value="false" var="force_forward"/>
<circle radius="-100" wp="p1"/>
</block>
<block name="Circle_CW_fwd">
<set value="true" var="force_forward"/>
<circle radius="100" wp="p1"/>
</block>
<block name="Circle_CCW_fwd">
<set value="true" var="force_forward"/>
<circle radius="-100" wp="p1"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="false" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="false" var="force_forward"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="false" var="force_forward"/>
<exception cond="!(stateGetPositionEnu_f()->z > 12.0)" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="false" var="force_forward"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(stateGetPositionEnu_f()->z > 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND !(agl_dist_value > 0.20)" deroute="flare_low"/>
</block>
<block name="flare_low">
<set value="false" var="force_forward"/>
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<set value="false" var="force_forward"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
295 changes: 295 additions & 0 deletions conf/flight_plans/tudelft/rotating_wing_troia_low.xml
View file
@@ -0,0 +1,295 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="100" ground_alt="0" lat0="38.474472222222225" lon0="-8.871222222222222" max_dist_from_home="80000" name="Nederdrone Troia" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
</header>
<waypoints>
<waypoint lat="38.474472222222225" lon="-8.871222222222222" name="HOME"/>
<waypoint lat="38.476515" lon="-8.868380" name="CLIMB"/>
<waypoint lat="38.4742" lon="-8.86866" name="STDBY"/>
<!-- <waypoint lat="38.476940" lon="-8.867501" name="p1"/>
<waypoint lat="38.481886" lon="-8.856215" name="p2"/>
<waypoint lat="38.478809" lon="-8.851799" name="p3"/>
<waypoint lat="38.480182" lon="-8.853301" name="p4"/> -->
<waypoint lat="38.476940" lon="-8.867501" name="trans"/>
<waypoint lat="38.481886" lon="-8.856215" name="decel"/>
<waypoint lat="38.478809" lon="-8.851799" name="end_trans"/>
<waypoint lat="38.4753" lon="-8.86665" name="begin_trans"/>
<waypoint lat="38.4743" lon="-8.86683" name="circ"/>
<waypoint lat="38.4746" lon="-8.86871" name="s1"/>
<waypoint lat="38.4727" lon="-8.86733" name="s2"/>
<waypoint lat="38.4739" lon="-8.86469" name="s3"/>
<waypoint lat="38.4757" lon="-8.86625" name="s4"/>
<waypoint lat="38.480073" lon="-8.872210" name="n1"/>
<waypoint lat="38.483435" lon="-8.865682" name="n2"/>
<waypoint lat="38.480763" lon="-8.863378" name="n3"/>
<waypoint lat="38.477503" lon="-8.869605" name="n4"/>
<waypoint lat="38.4743" lon="-8.86995" name="TD"/>
<waypoint lat="38.474497" lon="-8.870086" name="APP"/>
<waypoint lat="38.499227" lon="-8.858617" name="1-1"/>
<waypoint lat="38.476154" lon="-8.833249" name="1-2"/>
<waypoint lat="38.469040" lon="-8.861433" name="1-3"/>
<waypoint lat="38.467811" lon="-8.879136" name="1-4"/>
<waypoint lat="38.464900" lon="-8.884918" name="1-5"/>
<waypoint lat="38.471784" lon="-8.899161" name="1-6"/>
<waypoint lat="38.478446" lon="-8.885780" name="1-7"/>
<waypoint lat="38.480130" lon="-8.886261" name="1-8"/>
<waypoint lat="38.467526" lon="-8.888318" name="_2-1"/>
<waypoint lat="38.466563" lon="-8.887256" name="_2-2"/>
<waypoint lat="38.470725" lon="-8.881204" name="_2-4"/>
<waypoint lat="38.469633" lon="-8.880184" name="_2-3"/>
<waypoint lat="38.493038" lon="-8.858636" name="1-1_soft"/>
<waypoint lat="38.478620" lon="-8.845008" name="1-2_soft"/>
<waypoint lat="38.470562" lon="-8.867834" name="1-3_soft"/>
<waypoint lat="38.476390" lon="-8.887219" name="1-5_soft"/>
<waypoint lat="38.468803" lon="-8.880064" name="1-4_soft"/>
<waypoint lat="38.468493" lon="-8.886115" name="2-1_soft"/>
<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.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"/>
</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="Corridor">
<corner name="_2-1"/>
<corner name="_2-2"/>
<corner name="_2-3"/>
<corner name="_2-4"/>
</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>
</sectors>
<modules>
<module name="nav" type="survey_rectangle_rotorcraft">
<define name="RECTANGLE_SURVEY_DEFAULT_SWEEP" value="100"/>
</module>
</modules>
<exceptions>
<!-- GPS Loss (8 seconds -> gps_loss) -->
<exception cond="!GpsFixValid() @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('gps_lost'))" deroute="gps_lost"/>
<!-- Hard geofence (red -> Holding point) -->
<exception cond="And( Or(!InsideA1(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 300),
Or(!InsideA3(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 304.8))
@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 SOUTH) -->
<exception cond="And( Or(!InsideA1_soft(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 300),
Or(!InsideA3(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 304.8))
@AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('GO SOUTH'))" deroute="Standby"/>
<!-- Datalink loss (25 seconds -> GO SOUTH) -->
<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('Link_loss'))" deroute="Link_loss"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<set value="false" var="force_forward"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<set value="false" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>5" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="false" var="force_forward"/>
<call_once fun="autopilot_set_in_flight(true)"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Climb"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>1" vmode="throttle"/>
</block>
<block name="Climb">
<exception cond="stateGetPositionEnu_f()->z > 10.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="stay_begin-trans">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<stay wp="begin_trans" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="transition_CW">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<stay wp="end_trans"/>
<exception cond="rot_wing_a.transitioned @AND (wing_rotation.wing_angle_deg > 75.0)" deroute="Circle_CW_fwd"/>
</block>
<block name="transition_CCW">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<stay wp="end_trans"/>
<exception cond="rot_wing_a.transitioned @AND (wing_rotation.wing_angle_deg > 75.0)" deroute="Circle_CCW_fwd"/>
</block>
<block name="GO SOUTH">
<set value="true" var="force_forward"/>
<set value="30" var="nav_max_speed"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<go from="s1" hmode="route" wp="s2"/>
<go from="s2" hmode="route" wp="s3"/>
<go from="s3" hmode="route" wp="s4"/>
<go from="s4" hmode="route" wp="s1"/>
<deroute block="GO SOUTH"/>
</block>
<block name="GO NORTH">
<set value="true" var="force_forward"/>
<set value="30" var="nav_max_speed"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<go from="n1" hmode="route" wp="n2"/>
<go from="n2" hmode="route" wp="n3"/>
<go from="n3" hmode="route" wp="n4"/>
<go from="n4" hmode="route" wp="n1"/>
<deroute block="GO NORTH"/>
</block>
<block name="Approach APP">
<set value="false" var="force_forward"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<block name="Link_loss">
<set value="false" var="force_forward"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="gps_lost">
<exception cond="GpsFixValid()" deroute="GO NORTH"/>
<attitude pitch="0" roll="0" climb="-1.0" vmode="climb"/>
</block>
<block name="Circle_CW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<circle radius="100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Circle_CCW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<circle radius="-100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<circle radius="150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CCW_fwd">
<set value="30" var="nav_max_speed"/>
<set value="true" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<circle radius="-150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Transition_quad">
<set value="30" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(true)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<exception cond="!(stateGetPositionEnu_f()->z > 12.0)" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(stateGetPositionEnu_f()->z > 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND !(agl_dist_value > 0.24)" deroute="flare_low"/>
</block>
<block name="flare_low">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<set value="7" var="nav_max_speed"/>
<set value="false" var="force_forward"/>
<call fun="set_wing_rotation_scheduler(false)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
3,456 changes: 3,456 additions & 0 deletions conf/messages.xml
View file

Large diffs are not rendered by default.

33 changes: 33 additions & 0 deletions conf/modules/ctrl_eff_sched_rot_wing_v3b.xml
View file
@@ -0,0 +1,33 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ctrl_eff_sched_rot_wing_v3b" dir="ctrl">
<doc>
<description>Crtl effectiveness scheduler for the rotating wing drone V3B</description>
</doc>

<settings>
<dl_settings>
<dl_settings NAME="EffSched">
<dl_setting var="hover_motors_active" min="0" step="1" max="1" values="FALSE|TRUE" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="h_motors_active"/>
<dl_setting var="bool_disable_hover_motors" min="0" step="1" max="1" values="FALSE|TRUE" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="h_motors_disable"/>
<dl_setting var="wing_rotation_sched_activated" min="0" step="1" max="1" values="FALSE|TRUE" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="rot_sched_on"/>
<dl_setting var="pusher_sched_activated" min="0" step="1" max="1" values="FALSE|TRUE" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="push_sched_on"/>
<dl_setting var="pitch_angle_set" min="-10.0" step="0.1" max="10.0" shortname="pitch_angle"/>
<dl_setting var="lift_d_multiplier" min="0.1" step="0.01" max="5" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="lift_mult"/>
<dl_setting var="g1_p_multiplier" min="0.1" step="0.01" max="5" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="eff_mult_p"/>
<dl_setting var="g1_q_multiplier" min="0.1" step="0.01" max="5" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="eff_mult_q"/>
<dl_setting var="g1_r_multiplier" min="0.1" step="0.01" max="5" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="eff_mult_r"/>
<dl_setting var="g1_t_multiplier" min="0.1" step="0.01" max="5" module="ctrl/ctrl_eff_sched_rot_wing_v3b" shortname="eff_mult_t"/>
</dl_settings>
</dl_settings>
</settings>

<header>
<file name="ctrl_eff_sched_rot_wing_v3b.h"/>
</header>
<init fun="init_eff_scheduling()"/>
<periodic fun="event_eff_scheduling()" freq="20"/>
<makefile>
<file name="ctrl_eff_sched_rot_wing_v3b.c"/>
<test/>
</makefile>
</module>
151 changes: 151 additions & 0 deletions conf/modules/ekf_aw.xml
View file
@@ -0,0 +1,151 @@
<!DOCTYPE module SYSTEM "module.dtd">

<module name="ekf_aw" dir="meteo">
<doc>
<description>
Airspeed and Wind Estimator with EKF
Frédéric Larocque
</description>
<define name="PERIODIC_FREQUENCY_AIRSPEED_EKF" value="25" description="Airspeed periodic frequency"/>
<define name="PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH" value="100" description="Airspeed periodic fetch frequency"/>

<define name="EKF_AW_P0_V_BODY" value="1e-00f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1e-00f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1e-00f" description="Initial covariance offset"/>

<define name="EKF_AW_Q_ACCEL" value="1e-00f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1e-00f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1e-00f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1e-00f" description="Offset process noise"/>

<define name="EKF_AW_R_V_GND" value="1e-00f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1e-00f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1e-00f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1e-00f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1e-00f" description="Pitot Tube Velocity measurement noise"/>

<define name="EKF_AW_WING_INSTALLED" value="false" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED" value="false" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_BETA" value="false" description="Use beta to estimate sideforce"/>
<define name="EKF_AW_PROPAGATE_OFFSET" value="false" description="Propagate the offset state"/>

<define name="EKF_AW_VEHICLE_MASS" value="0.0" description="Mass of the vehicle"/>

<define name="EKF_AW_K1_FX_DRAG" value="0.f" description="Drag coefficient linear to u used if use model based is set to false"/>
<define name="EKF_AW_K2_FX_DRAG" value="0.f" description="Drag coefficient linear to u^2 used if use model based is set to false"/>

<define name="EKF_AW_K_FY_BETA" value="1e-00f" description="Fy v (side velocity) coefficient"/>
<define name="EKF_AW_K_FY_V" value="1e-00f" description="Fy beta coefficient"/>

<define name="EKF_AW_K1_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K3_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K4_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>

<define name="EKF_AW_K1_FX_HOVER" value="1e-00f" description="Fx Hover Prop Coeff"/>
<define name="EKF_AW_K2_FX_HOVER" value="1e-00f" description="Fx Hover Prop Coeff"/>

<define name="EKF_AW_K1_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K2_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K3_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K4_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K5_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>

<define name="EKF_AW_K1_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>
<define name="EKF_AW_K2_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>
<define name="EKF_AW_K3_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>

<define name="EKF_AW_K1_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>
<define name="EKF_AW_K2_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>
<define name="EKF_AW_K3_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>

<define name="EKF_AW_K1_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K2_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K3_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K4_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>

<define name="EKF_AW_K1_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K2_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K3_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K4_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>

<define name="EKF_AW_K1_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K2_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K3_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K4_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K5_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>

<define name="EKF_AW_K1_FZ_ELEV" value="1e-00f" description="Fz Elevator Coeff"/>
<define name="EKF_AW_K2_FZ_ELEV" value="1e-00f" description="Fz Elevator Coeff"/>

<define name="EKF_AW_ELEV_MAX_ANGLE" value="1e-00f" description="Maximum elevator angle"/>
<define name="EKF_AW_ELEV_MIN_ANGLE" value="1e-00f" description="Minimum elevator angle"/>
<define name="EKF_AW_AOA_MAX_ANGLE" value="1e-00f" description="Maximum angle of attack"/>
<define name="EKF_AW_AOA_MIN_ANGLE" value="1e-00f" description="Minimum angle of attack"/>
</doc>
<settings>
<dl_settings>
<dl_settings name="EKF_Airspeed_Wind">
<dl_setting min="0" max="1" step="1" var="ekf_aw.override_start" module="meteo/ekf_aw_wrapper" shortname="Override Start"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw.override_quick_convergence" module="meteo/ekf_aw_wrapper" shortname="Override Quick Convergence"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw.reset" module="meteo/ekf_aw_wrapper" shortname="Reset States and Health" handler="reset"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw_params.propagate_offset" module="meteo/ekf_aw" shortname="propagate offset"/>

<dl_setting min="1E-1" max="8" step="1E-1" var="ekf_aw_params.vehicle_mass" module="meteo/ekf_aw" shortname="Vehicle Mass"/>

<dl_setting min="-1" max="0" step="1E-3" var="ekf_aw_params.k_fx_drag[0]" module="meteo/ekf_aw" shortname="k_drag_lin"/>
<dl_setting min="-1" max="0" step="1E-3" var="ekf_aw_params.k_fx_drag[1]" module="meteo/ekf_aw" shortname="k_drag_quad"/>

<dl_setting min="-5E-2" max="0" step="1E-4" var="ekf_aw_params.k_fy_beta" module="meteo/ekf_aw" shortname="k_beta"/>
<dl_setting min="-5E-2" max="0" step="1E-4" var="ekf_aw_params.k_fy_v" module="meteo/ekf_aw" shortname="k_v"/>

<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_accel" module="meteo/ekf_aw" shortname="Q accel" handler="update_Q_accel"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_gyro" module="meteo/ekf_aw" shortname="Q gyro" handler="update_Q_gyro"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_mu" module="meteo/ekf_aw" shortname="Q wind" handler="update_Q_mu"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_k" module="meteo/ekf_aw" shortname="Q offset" handler="update_Q_k"/>

<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_V_gnd" module="meteo/ekf_aw" shortname="R V_gnd" handler="update_R_V_gnd"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[0]" module="meteo/ekf_aw" shortname="R Accel Filt x" handler="update_R_accel_filt_x"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[1]" module="meteo/ekf_aw" shortname="R Accel Filt y" handler="update_R_accel_filt_y"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[2]" module="meteo/ekf_aw" shortname="R Accel Filt z" handler="update_R_accel_filt_z"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_V_pitot" module="meteo/ekf_aw" shortname="R Pitot" handler="update_R_V_pitot"/>

<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.x" module="meteo/ekf_aw_wrapper" shortname="Wind Guess N" handler="set_wind_N"/>
<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.y" module="meteo/ekf_aw_wrapper" shortname="Wind Guess E" handler="set_wind_E"/>
<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.z" module="meteo/ekf_aw_wrapper" shortname="Wind Guess D" handler="set_wind_D"/>

<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.x" module="meteo/ekf_aw_wrapper" shortname="Offset Guess x" handler="set_offset_x"/>
<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.y" module="meteo/ekf_aw_wrapper" shortname="Offset Guess y" handler="set_offset_y"/>
<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.z" module="meteo/ekf_aw_wrapper" shortname="Offset Guess z" handler="set_offset_z"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="ekf_aw_wrapper.h"/>
</header>
<init fun="ekf_aw_wrapper_init()"/>
<periodic fun="ekf_aw_wrapper_periodic()" freq="25"/>
<periodic fun="ekf_aw_wrapper_fetch()" freq="100"/>
<makefile target="ap|nps">
<file name="ekf_aw.cpp"/>
<file name="ekf_aw_wrapper.c"/>
<include name="$(PAPARAZZI_SRC)/sw/ext/matrix/"/>
</makefile>
</module>

<!--Your comment
Target NPS or AP
<configure name="CXXSTANDARD" value="-std=c++14"/>
<flag name="LDFLAGS" value="lstdc++" />
Target AP
<define name="EIGEN_NO_DEBUG"/> Removes debug checks and should lead to faster code execution
<flag name="CXXFLAGS" value="Wno-bool-compare"/>
<flag name="CXXFLAGS" value="Wno-logical-not-parentheses"/>
<datalink message="PING" fun="datalink_parse_PING(dev, trans, buf)"/>
-->

25 changes: 25 additions & 0 deletions conf/modules/rot_wing_automation.xml
View file
@@ -0,0 +1,25 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="rot_wing_automation" dir="rot_wing_drone">
<doc>
<description>Fucntions to automate the navigation and guidance of the rotating wing drone</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="transition">
<dl_setting var="rot_wing_a.trans_accel" min="0.1" max="5.0" step="0.1" shortname="trans_accel"/>
<dl_setting var="rot_wing_a.trans_decel" min="0.1" max="5.0" step="0.1" shortname="trans_decel"/>
<dl_setting var="rot_wing_a.trans_length" min="10" max="500." step="1.0" shortname="trans_distance"/>
<dl_setting var="rot_wing_a.trans_airspeed" min="10.0" max="20.0" step="0.1" shortname="trans_airspeed"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="rot_wing_automation.h"/>
</header>
<init fun="init_rot_wing_automation()"/>
<periodic fun="periodic_rot_wing_automation()" freq="10."/>
<makefile>
<file name="rot_wing_automation.c"/>
<test/>
</makefile>
</module>
15 changes: 15 additions & 0 deletions conf/modules/rotwing_state.xml
View file
@@ -0,0 +1,15 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="rotwing_state" dir="rot_wing_drone">
<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>
</doc>
<header>
<file name="rotwing_state.h"/>
</header>
<init fun="init_rotwing_state()"/>
<periodic fun="periodic_rotwing_state()" freq="10"/>
<makefile>
<file name="rotwing_state.c"/>
<test/>
</makefile>
</module>
31 changes: 31 additions & 0 deletions conf/modules/wing_rotation_controller_v3b.xml
View file
@@ -0,0 +1,31 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="wing_rotation_controller_v3b" dir="rot_wing_drone">
<doc>
<description>Module to control wing rotation servo command based on prefered angle setpoint</description>
</doc>
<settings>
<dl_settings NAME="wing_rotation">
<dl_settings NAME="wing_rot">
<dl_setting MAX="90" MIN="0" STEP="1" VAR="wing_rotation.wing_angle_deg_sp" shortname="wing_sp_deg"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="wing_rotation.airspeed_scheduling" shortname="airspeed_sched"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="wing_rotation.force_rotation_angle" shortname="force_skew"/>
<dl_setting MAX="18" MIN="14" STEP="0.1" VAR="wing_rotation.forward_airspeed" shortname="forward_airspeed"/>
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation.wing_rotation_first_order_dynamics" shortname="first_dyn"/>
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation.wing_rotation_second_order_dynamics" shortname="second_dyn"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>rot_wing_automation</depends>
</dep>
<header>
<file name="wing_rotation_controller_v3b.h"/>
</header>
<init fun="wing_rotation_init()"/>
<periodic fun="wing_rotation_periodic()" freq="1.0"/>
<periodic fun="wing_rotation_event()"/>
<makefile>
<file name="wing_rotation_controller_v3b.c"/>
<test/>
</makefile>
</module>
2 changes: 1 addition & 1 deletion conf/radios/crossfire_sbus.xml
View file
Expand Up @@ -9,7 +9,7 @@
<channel function="AUX1" min="1100" neutral="1500" max="1900" average="1"/> <!-- TH_HOLD -->
<channel function="AUX2" min="1100" neutral="1500" max="1900" average="1"/> <!-- FMODE -->
<channel function="AUX3" min="1100" neutral="1500" max="1900" average="1"/> <!-- FBW_MODE -->
<channel function="AUX4" min="1100" neutral="1500" max="1900" average="1"/>
<channel function="AUX4" min="1100" neutral="1100" max="1900" average="1"/>
<channel function="AUX5" min="1100" neutral="1500" max="1900" average="1"/>
<channel function="AUX6" min="1100" neutral="1500" max="1900" average="1"/>
<channel function="AUX7" min="1100" neutral="1500" max="1900" average="1"/>
Expand Down