Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 01:53:48 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[rotwing] V3B Delivery (#3449)
Issues due date / Add labels to issues (push) Has been cancelled
Details
Doxygen / build (push) Has been cancelled
Details

Browse Source 
* rebase with master

* undo double define during rebase

* revert eff sched changes
This commit is contained in:
NoahWe
2025-08-15 16:00:32 +02:00
committed by GitHub
parent c86ab15f0c
commit e42a7ef901
24 changed files with 650 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rotwing5.xml
+1 -3
View File
@@ -158,9 +158,7 @@
<module name="sys_id_auto_doublets"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks">
<define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
</module>
<module name="preflight_checks"/>
<module name="agl_dist"/>
<module name="approach_moving_target"/>
conf/airframes/tudelft/rotwing6.xml
+1 -3
View File
@@ -158,9 +158,7 @@
<module name="sys_id_auto_doublets"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks">
<define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
</module>
<module name="preflight_checks"/>
<module name="agl_dist"/>
<module name="approach_moving_target"/>
conf/airframes/tudelft/rotwing_25kg_common.xml
+4 -1
View File
@@ -2,7 +2,7 @@
<airframe>
<section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_">
<section name="CTRL_EFF_SCHED" prefix="ROTWING_EFF_SCHED_">
<define name="IXX_BODY" value="0.3953"/>
<define name="IYY_BODY" value="8.472"/>
<define name="IZZ" value="10.18"/>
@@ -58,6 +58,9 @@
<!-- Air data -->
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE"/>
<!-- SD logger -->
<define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
conf/airframes/tudelft/rotwing_7kg_common.xml
+7 -7
View File
@@ -1,7 +1,6 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe>
<section name="AP_FAILSAFE">
<!-- <define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/> -->
<define name="NO_GPS_LOST_WITH_RC_VALID" value="FALSE"/>
@@ -56,7 +55,8 @@
<!-- 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"/>
<define name="GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD" value="-12.0"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<!-- Flight plan defines -->
<define name="FLARE_HEIGHT" value="12"/>
@@ -72,7 +72,7 @@
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- Limits -->
<define name="SP_MAX_PHI" value="45." unit="deg" />
<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"/>
@@ -116,13 +116,13 @@
<!-- 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_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="{20.0, 20.0, 20.0, 20.0, 52.7, 52.7, 52.7, 52.7, 30.0}"/>
<define name="ACT_FREQ" value="{22.0, 22.0, 22.0, 22.0, 52.7, 52.7, 52.7, 52.7, 30.0}"/>
<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}"/>
@@ -162,7 +162,7 @@
<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="NOMINAL_HOVER_THROTTLE" value="0.60"/> <!-- 0.6 (~5800 PPRZ) for V3D+ drones -->
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
<!-- Reference -->
@@ -192,7 +192,7 @@
<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="STATE_MIN_FW_DIST" value="150"/> <!-- Minimum distance to switch to fixed wing mode -->
<define name="STATE_MIN_FW_DIST" value="100"/> <!-- Switch to quad once the drone is within this radius of Standby -->
<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 -->
conf/airframes/tudelft/rotwing_v3b.xml
+13 -14
View File
@@ -106,7 +106,6 @@
<module name="imu" type="cube"/>
<module name="ins" type="ekf2"/>
<module name="parachute"/>
<module name="ekf_aw"/>
<!-- Actuators on dual CAN bus -->
@@ -159,7 +158,7 @@
<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="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="SERVO_ELEVATOR" min="5000" neutral="5000" max="-5500"/>
<servo no="6" name="SERVO_RUDDER" min="-8191" neutral="0" max="8191"/>
</servos>
@@ -262,7 +261,7 @@
<define name="PFC_ACTUATORS" type="array">
<!-- Aerodynamic -->
<field type="struct">
<field name="feedback_id" value="SERVO_SERVO_ELEVATOR_IDX"/>
<field name="feedback_id" value="255"/>
<field name="feedback_id2" value="255"/>
<field name="low" value="-4500"/>
<field name="high" value="4500"/>
@@ -271,7 +270,7 @@
<field name="timeout" value="1"/>
</field>
<field type="struct">
<field name="feedback_id" value="SERVO_SERVO_RUDDER_IDX"/>
<field name="feedback_id" value="255"/>
<field name="feedback_id2" value="255"/>
<field name="low" value="-4500"/>
<field name="high" value="4500"/>
@@ -280,7 +279,7 @@
<field name="timeout" value="1"/>
</field>
<field type="struct">
<field name="feedback_id" value="SERVO_AIL_LEFT_IDX"/>
<field name="feedback_id" value="255"/>
<field name="feedback_id2" value="255"/>
<field name="low" value="-4500"/>
<field name="high" value="4500"/>
@@ -289,7 +288,7 @@
<field name="timeout" value="1"/>
</field>
<field type="struct">
<field name="feedback_id" value="SERVO_FLAP_LEFT_IDX"/>
<field name="feedback_id" value="255"/>
<field name="feedback_id2" value="255"/>
<field name="low" value="-4500"/>
<field name="high" value="4500"/>
@@ -298,7 +297,7 @@
<field name="timeout" value="1"/>
</field>
<field type="struct">
<field name="feedback_id" value="SERVO_FLAP_RIGHT_IDX"/>
<field name="feedback_id" value="255"/>
<field name="feedback_id2" value="255"/>
<field name="low" value="-4500"/>
<field name="high" value="4500"/>
@@ -307,7 +306,7 @@
<field name="timeout" value="1"/>
</field>
<field type="struct">
<field name="feedback_id" value="SERVO_AIL_RIGHT_IDX"/>
<field name="feedback_id" value="255"/>
<field name="feedback_id2" value="255"/>
<field name="low" value="-4500"/>
<field name="high" value="4500"/>
@@ -318,7 +317,7 @@
<!-- Rotation -->
<field type="struct">
<field name="feedback_id" value="SERVO_ROTATION_MECH_IDX"/>
<field name="feedback_id" value="255"/>
<field name="feedback_id2" value="255"/>
<field name="low" value="-9600"/>
<field name="high" value="9600"/>
@@ -370,7 +369,7 @@
<field name="low" value="-9600"/>
<field name="high" value="2000"/>
<field name="low_feedback" value="0"/>
<field name="high_feedback" value="2800"/>
<field name="high_feedback" value="2500"/>
<field name="timeout" value="3"/>
</field>
</define>
@@ -395,7 +394,8 @@
<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="wing assembly">Check all four wing bolts present and tightened</item>
<item name="wing connection">Check wing power and CAN lines connected</item>
<item name="inspection">Inspect airframe condition</item>
<item name="attitude">Check attitude and heading</item>
<item name="airspeed">Airspeed sensor calibration</item>
@@ -403,15 +403,14 @@
<item name="actuators">Automated actuator check</item>
<item name="flight plan">Check flight plan</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="announce">Announce flight to other airspace users</item>
</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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/airframes/tudelft/rotwing_v3d.xml
+2 -2
View File
@@ -457,8 +457,8 @@
<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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/airframes/tudelft/rotwing_v3e.xml
+2 -2
View File
@@ -419,8 +419,8 @@
<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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/airframes/tudelft/rotwing_v3f.xml
+2 -2
View File
@@ -403,8 +403,8 @@
<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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/airframes/tudelft/rotwing_v3g.xml
+2 -2
View File
@@ -416,8 +416,8 @@
<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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/airframes/tudelft/rotwing_v3h.xml
+3 -3
View File
@@ -71,7 +71,7 @@
</module>
<module name="airspeed" type="ms45xx_i2c">
<configure name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.90"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.6580"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.25"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
@@ -403,8 +403,8 @@
<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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/airframes/tudelft/rotwing_v3i.xml
+2 -2
View File
@@ -452,8 +452,8 @@
<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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/airframes/tudelft/rotwing_v3j.xml
+2 -2
View File
@@ -455,8 +455,8 @@
<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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/airframes/tudelft/rotwing_v3k.xml
+2 -2
View File
@@ -430,8 +430,8 @@
<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="CRITIC_BAT_LEVEL" value="21.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="22.08" 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"/>
conf/autopilot/rotorcraft_rotwing.xml
+268
View File
@@ -0,0 +1,268 @@
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
<autopilot name="Rotating Wing Autopilot">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<includes>
<include name="generated/airframe.h"/>
<include name="autopilot.h"/>
<include name="autopilot_rc_helpers.h"/>
<include name="navigation.h"/>
<include name="guidance.h"/>
<include name="stabilization.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
<include name="modules/rotwing_drone/rotwing_state.h"/>
<include name="modules/nav/ground_detect.h"/>
<!-- Failsafe variables -->
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20" cond="ifndef NO_GPS_LOST_WITH_DATALINK_TIME"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="FALSE" cond="ifndef NO_GPS_LOST_WITH_RC_VALID"/>
<define name="RC_LOST_MODE" value="AP_MODE_NAV" cond="ifndef RC_LOST_MODE"/>
<define name="FAILSAFE_DESCENT_SPEED" value="1.0" cond="ifndef FAILSAFE_DESCENT_SPEED"/>
<!-- Modes -->
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT" cond="ifndef MODE_MANUAL"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
<define name="RCLost()" value="(radio_control.status != RC_OK)"/> <!-- !=RC_OK might be a little strict -->
<define name="DLModeNav()" value="(autopilot_mode_auto2 == AP_MODE_NAV)"/>
<define name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
<define name="DLModeModule()" value="(MODE_AUTO1 == AP_MODE_MODULE)"/>
<define name="DLModeAZH()" value="(MODE_AUTO1 == AP_MODE_ATTITUDE_Z_HOLD)"/>
<define name="SetThrottleValue()" value="autopilot.throttle = commands[COMMAND_THRUST];"/>
</includes>
<settings>
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
<dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
</settings>
<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
<call fun="SetThrottleValue()"/>
</control_block>
<control_block name="run_attitude_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="run_rate_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="run_guidance_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="update_v_sp_gcs">
<call fun="guidance_v_set_z(-nav.nav_altitude)"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
</control_block>
<exceptions>
<exception cond="nav.too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions>
<!-- Kill throttle mode 0-->
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<call fun="stabilization.cmd[COMMAND_THRUST] = 0"/>
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>
<!-- Failsafe mode 1-->
<mode name="FAILSAFE" shortname="FAIL">
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
<call fun="NavStartDetectGround()"/>
</on_enter>
<control>
<call fun="NavVerticalThrottleMode(MAX_PPRZ*(0.5))"/>
<call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost() && !(agl_dist_valid && agl_dist_value_filtered < 5.0)" deroute="$LAST_MODE"/>
<exception cond="NavDetectGround()" deroute="Kill"/>
<exception cond="!nav_is_in_flight()" deroute="Kill"/>
<exception cond="ground_detect()" deroute="Kill"/>
</mode>
<!-- Home mode 2-->
<mode name="HOME">
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight() && !NO_GPS_LOST_WITH_RC_VALID && (datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME)" deroute="FAILSAFE"/>
</mode>
<!-- Rate mode 3-->
<mode name="RATE_DIRECT" shortname="RATE">
<!-- <select cond=""/> -->
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_RATE, 0)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="run_rate_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- Attitude mode 4-->
<mode name="ATTITUDE_DIRECT" shortname="ATT_QUAD">
<select cond="RCMode0()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- Rate climb rc mode 5-->
<mode name="RATE_RC_CLIMB" shortname="RRCC">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- ATTITUDE_RC_CLIMB: 6 -->
<mode name="ATTITUDE_RC_CLIMB" shortname="ATTITUDE_RC_CLIMB">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- ATTITUDE_CLIMB: 7 -->
<mode name="ATTITUDE_CLIMB" shortname="ATTITUDE_CLIMB">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- RATE_Z_HOLD: 8 -->
<mode name="RATE_Z_HOLD" shortname="RATE_Z_HOLD">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- ATTITUDE_Z_HOLD: 9 -->
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- HOVER_DIRECT: 10 -->
<mode name="HOVER_DIRECT" shortname="HOVER_DIRECT">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- HOVER_CLIMB: 11 -->
<mode name="HOVER_CLIMB" shortname="HOVER_CLIMB">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- HOVER_Z_HOLD: 12 -->
<mode name="HOVER_Z_HOLD" shortname="HOVER_Z_HOLD">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- NAV: 13 -->
<mode name="NAV">
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="update_v_sp_gcs"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight() && !NO_GPS_LOST_WITH_RC_VALID && (datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME)" deroute="FAILSAFE"/>
</mode>
<!-- RC_DIRECT: 14 -->
<mode name="RC_DIRECT" shortname="RC_DIRECT">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- CARE_FREE_DIRECT: 15 -->
<mode name="CARE_FREE_DIRECT" shortname="CARE_FREE_DIRECT">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- FORWARD: 16 -->
<mode name="FORWARD" shortname="FW">
<select cond="RCMode1()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_FORWARD)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- MODULE: 17 -->
<mode name="MODULE" shortname="MODULE">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- FLIP: 18 -->
<mode name="FLIP" shortname="FLIP">
<exception cond="RCLost()" deroute="NAV"/>
</mode>
<!-- GUIDED: 19 -->
<mode name="GUIDED">
<exception cond="GpsIsLost() && autopilot_in_flight() && !NO_GPS_LOST_WITH_RC_VALID && (datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME)" deroute="FAILSAFE"/>
</mode>
</state_machine>
</autopilot>
conf/flight_plans/tudelft/rotwing_EHVB.xml
+33 -32
View File
@@ -133,31 +133,32 @@
<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" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
<stay wp="STDBY"/>
<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_dist" strip_button="Standby Dist" strip_icon="home.png" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
<stay wp="STDBY"/>
<stay wp="STDBY"/>
</block>
<block name="Standby_free" strip_button="Standby Free" strip_icon="home.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/>
</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"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p1"/>
</block>
<block name="stay_p2" strip_button="Stay P2" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p2"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p2"/>
</block>
<block name="stay_p3" strip_button="Stay P3" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p3"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p3"/>
</block>
<block name="stay_p4" strip_button="Stay P4" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p4"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p4"/>
</block>
<!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
@@ -209,35 +210,35 @@
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> -->
<block name="land here">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
<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"/>
<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 flare_height" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="GetPosHeight() @LT flare_height" 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"/>
<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"/>
<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"/>
<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/flight_plans/tudelft/rotwing_generic.xml
+235
View File
@@ -0,0 +1,235 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="50" ground_alt="0" lat0="52.962558" lon0="4.793334" max_dist_from_home="5000" name="Rotating Wing Generic" 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="20"/>
<waypoint name="p1" x="65" y="75"/>
<waypoint name="p2" x="200" y="120"/>
<waypoint name="p3" x="250" y="-30"/>
<waypoint name="p4" x="120" y="-75"/>
<waypoint name="circ" x="160" y="25"/>
<waypoint name="TD" x="10" y="-1"/>
<!-- <waypoint name="APP" x="70" y="-25"/>
<waypoint name="FOLLOW" x="300" y="80"/> -->
<!-- t'Harde -->
<waypoint x="1500" y="0" name="C1"/>
<waypoint x="750" y="-1299" name="C2"/>
<waypoint x="-750" y="-1299" name="C3"/>
<waypoint x="-1500" y="0" name="C4"/>
<waypoint x="-750" y="1299" name="C5"/>
<waypoint x="750" y="1299" name="C6"/>
<waypoint x="1000" y="0" name="S1"/>
<waypoint x="500" y="-866" name="S2"/>
<waypoint x="-500" y="-866" name="S3"/>
<waypoint x="-1000" y="0" name="S4"/>
<waypoint x="-500" y="866" name="S5"/>
<waypoint x="500" y="866" name="S6"/>
</waypoints>
<sectors>
<sector color="red" name="HardGeofence">
<corner name="C1"/>
<corner name="C2"/>
<corner name="C3"/>
<corner name="C4"/>
<corner name="C5"/>
<corner name="C6"/>
</sector>
<sector color="orange" name="SoftGeofence">
<corner name="S1"/>
<corner name="S2"/>
<corner name="S3"/>
<corner name="S4"/>
<corner name="S5"/>
<corner name="S6"/>
</sector>
</sectors>
<variables>
<variable var="liftoff_pitch"/>
<variable var="liftoff_roll"/>
<variable var="liftoff_pitch_limit" init="LIFTOFF_PITCH_LIMIT_DEG"/>
<variable var="liftoff_roll_limit" init="LIFTOFF_ROLL_LIMIT_DEG"/>
<variable var="stage_timer_msec"/>
<variable var="hybrid_height" init="HYBRID_HEIGHT"/>
<variable var="transition_height" init="TRANSITION_HEIGHT"/>
<variable var="flare_height" init="FLARE_HEIGHT"/>
<variable var="geofence_soft_height" init="250" min="0" step="10" max="5000" shortname="geofence_soft_height"/>
<variable var="geofence_hard_height" init="500" min="0" step="10" max="5000" shortname="geofence_hard_height"/>
</variables>
<modules>
<module name="follow_me">
<define name="FOLLOW_ME_MOVING_WPS" value="WP_STDBY,WP_circ,WP_TD"/>
</module>
</modules>
<exceptions>
<!-- Outside of hard (red) geofence kill -->
<exception cond="Or(!InsideHardGeofence(GetPosX(), GetPosY()), GetPosHeight() @GT geofence_hard_height) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<!-- Outside of soft (orange) geofence go to standby-->
<exception cond="Or(!InsideSoftGeofence(GetPosX(), GetPosY()), GetPosHeight() @GT geofence_soft_height) @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"/>
<!-- Datalink timeout go to standby free -->
<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"/>
<!-- Bat low return to standby-->
<exception cond="(electrical.bat_low @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND !(nav_block >= IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="Standby"/>
<!-- Bat critical land here-->
<exception cond="(electrical.bat_critical @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND !(nav_block >= IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="land here"/>
</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="NavSetGroundReferenceHere()"/>
</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="Take-off" 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" strip_button="Levelling Check">
<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 liftoff_roll_limit) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT liftoff_pitch_limit) @AND (stage_time > 2)" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Lift-off" 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 hybrid_height" 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" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
<stay wp="STDBY"/>
</block>
<block name="Standby_free" strip_button="Standby Free" strip_icon="home.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/>
</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" strip_button="Stay P2" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p2"/>
</block>
<block name="stay_p3" strip_button="Stay P3" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p3"/>
</block>
<block name="stay_p4" strip_button="Stay P4" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p4"/>
</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="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="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="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="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="route_p1"/>
</block>
<block name="Circle_CW_fwd" strip_button="Circle CW" 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="Circle CCW" 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="Circle Auto" strip_icon="circle-left.png"> <!-- Comment out later -->
<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="Survey S1-S2 25m NS" strip_button="Sweep North South p1-p2">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<survey_rectangle grid="100" orientation="NS" wp1="p1" wp2="p2"/>
</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="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 flare_height" 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/joystick/tudelft/rotwing_taranis_x9d_plus.xml
+43
View File
@@ -0,0 +1,43 @@
<!--
FrSky Tarranis X9D plus over USB (mode 2)
http://www.frsky-rc.com/product/pro.php?pro_id=137
In this file we will use it as a 6CH joystick to control a UAS.
- The left stick vertical axis will be used for throttle
- The right stick horizontal axis will be used for roll
- The right stick vertical axis will be used for pitch
- The left stick horizontal axis will be used for yaw
- The VRA axis will be used for mode switching (right top switch)
- The arm axis will be used for arming (left top switch)
- The pusher can be controlled using the slider on the right
If you want to fly your UAS via the joystick add this to your session:
/home/username/paparazzi/sw/ground_segment/joystick/input2ivy -d 0 -ac yourarfamename tudelft/Rotwing_Taranis_X9D_plus.xml
Where -d 0 must be -d 1 if you have a laptop with accelometer installed
The basis of steering is the standard signs of aerospace convention
-->
<joystick>
<input>
<axis index="3" name="LeftStickHorizontal"/>
<axis index="0" name="LeftStickVertical"/>
<axis index="1" name="RightStickHorizontal"/>
<axis index="2" name="RightStickVertical"/>
<axis index="4" name="VRA"/>
<axis index="5" name="arm"/>
<axis index="6" name="yellow"/>
<button index="0" name="thrust_x"/> <!-- pusher doesn't work well due to it being treated as a button, only zero or full throttle work -->
<button index="1" name="filler"/> <!-- necessary to fill the channels array such that pusher can be in the righ -->
</input>
<!-- Follow the order of rc_datalink.h -->
<messages period="0.0333333">
<message class="datalink" name="RC_UP" send_always="true">
<field name="channels" value="RightStickHorizontal;RightStickVertical;LeftStickHorizontal;Fit(LeftStickVertical,-127,127,0,127);VRA;arm;yellow;filler;Fit(thrust_x,0,1,-127,127)"/>
</message>
</messages>
</joystick>
conf/modules/math.xml
+2 -1
View File
@@ -24,6 +24,7 @@
<file name="pprz_trig_int.h" dir="math"/>
<file name="pprz_orientation_conversion.h" dir="math"/>
<file name="pprz_stat.h"/>
<file name="pprz_random.h" dir="math"/>
</header>
<init fun="pprz_trig_int_init()"/>
<makefile>
@@ -36,7 +37,7 @@
<file name="pprz_trig_int.c" dir="math"/>
<file name="pprz_orientation_conversion.c" dir="math"/>
<file name="pprz_stat.c" dir="math"/>
<file name="pprz_random.c" dir="math"/>
<test/>
</makefile>
</module>
conf/modules/rotwing_state.xml
+1
View File
@@ -12,6 +12,7 @@
<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.fw_min_airspeed" min="0" max="50" step="0.1" shortname="fw_min_airspeed"/>
<dl_setting var="rotwing_state.cruise_airspeed" min="0" max="50" step="0.1" shortname="cruise_airspeed"/>
<dl_setting var="nav.radius" min="50" step="1" max="1000" unit="m" shortname="circle_radius"></dl_setting>
<dl_setting var="rotwing_state.force_skew" min="0" max="1" step="1" values="FALSE|TRUE" shortname="force_skew"/>
<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"/>
conf/simulator/jsbsim/aircraft/rotwing3.xml
+14 -14
View File
@@ -121,47 +121,47 @@
<!--First order filter represents actuator dynamics-->
<lag_filter name="front_motor_lag">
<input> fcs/front_motor </input>
<c1> 18 </c1>
<c1> 22.0 </c1>
<output> fcs/front_motor_lag</output>
</lag_filter>
<lag_filter name="right_motor_lag">
<input> fcs/right_motor </input>
<c1> 18 </c1>
<c1> 22.0 </c1>
<output> fcs/right_motor_lag</output>
</lag_filter>
<lag_filter name="back_motor_lag">
<input> fcs/back_motor </input>
<c1> 18 </c1>
<c1> 22.0 </c1>
<output> fcs/back_motor_lag</output>
</lag_filter>
<lag_filter name="left_motor_lag">
<input> fcs/left_motor </input>
<c1> 18 </c1>
<c1> 22.0 </c1>
<output> fcs/left_motor_lag</output>
</lag_filter>
<lag_filter name="pusher_lag">
<input> fcs/pusher </input>
<c1> 18 </c1>
<c1> 30.0 </c1>
<output> fcs/pusher_lag</output>
</lag_filter>
<lag_filter name="elevator_lag">
<input> fcs/elevator </input>
<c1> 54 </c1>
<c1> 50.0 </c1>
<output> fcs/elevator_lag</output>
</lag_filter>
<lag_filter name="rudder_lag">
<input> fcs/rudder </input>
<c1> 54 </c1>
<c1> 50.0 </c1>
<output> fcs/rudder_lag</output>
</lag_filter>
<lag_filter name="aileron_lag">
<input> fcs/aileron </input>
<c1> 54 </c1>
<c1> 50.0 </c1>
<output> fcs/aileron_lag</output>
</lag_filter>
<lag_filter name="flap_lag">
<input> fcs/flap </input>
<c1> 54 </c1>
<c1> 50.0 </c1>
<output> fcs/flap_lag</output>
</lag_filter>
@@ -321,7 +321,7 @@
<function>
<product>
<property>fcs/front_motor_lag</property>
<value>7.643508703</value>
<value>6.36958</value>
</product>
</function>
<location unit="IN">
@@ -340,7 +340,7 @@
<function>
<product>
<property>fcs/right_motor_lag</property>
<value>7.643508703</value>
<value>6.36958</value>
</product>
</function>
<location unit="IN">
@@ -359,7 +359,7 @@
<function>
<product>
<property>fcs/back_motor_lag</property>
<value>7.643508703</value>
<value>6.36958</value>
</product>
</function>
<location unit="IN">
@@ -378,7 +378,7 @@
<function>
<product>
<property>fcs/left_motor_lag</property>
<value>7.643508703</value>
<value>6.36958</value>
</product>
</function>
<location unit="IN">
@@ -825,7 +825,7 @@
<product>
<property>aero/qbar-psf</property>
<value>47.9</value> <!-- Conversion to pascals -->
<value>0.0453</value> <!-- CD x Area (m^2) -->
<value>0.108</value> <!-- CD x Area (m^2) -->
<value>0.224808943</value> <!-- N to LBS -->
</product>
</function>
conf/userconf/tudelft/conf.xml
+1 -1
View File
@@ -544,7 +544,7 @@
airframe="airframes/tudelft/rotwing_v3b.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotwing_EHVB.xml"
flight_plan="flight_plans/tudelft/rotwing_generic.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/lidar_tfmini.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"