[airframes] RW25kg (#3133)

* [airframe] RW25kg
paparazzi · Oct 9, 2023 · cb97031 · cb97031
1 parent 6505098
commit cb97031
Show file tree

Hide file tree

Showing 8 changed files with 621 additions and 4 deletions.
diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml
diff --git a/conf/flight_plans/tudelft/rotating_wing25kg_EHVB.xml b/conf/flight_plans/tudelft/rotating_wing25kg_EHVB.xml
@@ -0,0 +1,164 @@
+<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
+
+<flight_plan alt="60" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="1100" name="Rotating wing Valkenburg" security_height="2">
+  <header/>
+  <waypoints>
+    <waypoint name="HOME" x="12.6" y="-48.7"/>
+    <waypoint name="CLIMB" x="62.6" y="-80.4"/>
+    <waypoint name="trans" x="100." y="100."/>
+    <waypoint name="decel" x="100." y="100."/>
+    <waypoint name="end_trans" x="100." y="100."/>
+    <waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/>
+    <waypoint name="p1" lat="52.1684116" lon="4.4149282"/>
+    <waypoint name="p2" lat="52.1675165" lon="4.4147158"/>
+    <waypoint name="p3" lat="52.1688983" lon="4.4139655"/>
+    <waypoint name="p4" lat="52.1687661" lon="4.4133220"/>
+    <waypoint name="circ" lat="52.1684116" lon="4.4149282"/>
+    <waypoint name="TD" lat="52.1681602" lon="4.4127708"/>
+    <waypoint name="APP" lat="52.1679567" lon="4.4136344"/>
+    <waypoint name="FOLLOW" lat="52.16850964562752" lon="4.413635008734417"/>
+    <waypoint lat="52.169189" lon="4.410820" name="C1"/>
+    <waypoint lat="52.168049" lon="4.406923" name="C2"/>
+    <waypoint lat="52.166515" lon="4.408235" name="C3"/>
+    <waypoint lat="52.163255" lon="4.407668" name="C4"/>
+    <waypoint lat="52.161908" lon="4.410082" name="C5"/>
+    <waypoint lat="52.162641" lon="4.416992" name="C6"/>
+    <waypoint lat="52.164861" lon="4.427268" name="C7"/>
+    <waypoint lat="52.170422" lon="4.427511" name="C8"/>
+    <waypoint lat="52.172276" lon="4.424011" name="C9"/>
+  </waypoints>
+  <sectors>
+    <sector color="red" name="Flyzone">
+      <corner name="C1"/>
+      <corner name="C2"/>
+      <corner name="C3"/>
+      <corner name="C4"/>
+      <corner name="C5"/>
+      <corner name="C6"/>
+      <corner name="C7"/>
+      <corner name="C8"/>
+      <corner name="C9"/>
+    </sector>
+  </sectors>
+  <modules>
+    <!--<module name="follow_me"/>-->
+  </modules>
+  <exceptions>
+    <exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
+    <exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
+  </exceptions>
+  <blocks>
+    <block name="Wait GPS">
+      <set value="false" var="force_forward"/>
+      <call_once fun="NavKillThrottle()"/>
+      <while cond="!GpsFixValid() || !state.ned_initialized_i"/>
+    </block>
+    <block name="Geo init">
+      <set value="false" var="force_forward"/>
+      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <call_once fun="NavSetAltitudeReferenceHere()"/>
+    </block>
+    <block name="Holding point">
+      <set value="false" var="force_forward"/>
+      <call_once fun="NavKillThrottle()"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+    <block name="Start Engine">
+        <set value="false" var="force_forward"/>
+        <call_once fun="NavResurrect()"/>
+        <attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
+    </block>
+    <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
+        <set value="false" var="force_forward"/>
+        <exception cond="GetPosAlt()  @LT 4.0" deroute="Standby"/>
+        <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
+        <attitude pitch="0" roll="0" throttle="0.75" until="stage_time>2" vmode="throttle"/>
+    </block>
+    <block name="Standby" strip_button="Standby" strip_icon="home.png">
+        <set value="false" var="force_forward"/>
+        <stay wp="STDBY"/>
+    </block>
+    <block name="stay_p1">
+      <set value="false" var="force_forward"/>
+      <stay wp="p1"/>
+    </block>
+    <block name="go_p2">
+      <set value="false" var="force_forward"/>
+      <call_once fun="nav_set_heading_deg(90)"/>
+      <go wp="p2"/>
+      <deroute block="stay_p1"/>
+    </block>
+    <block name="line_p1_p2">
+      <set value="false" var="force_forward"/>
+      <go from="p1" hmode="route" wp="p2"/>
+      <stay wp="p2"/>
+    </block>
+    <block name="line_trans_fwd">
+      <set value="false" var="force_forward"/>
+      <call_once fun="NavSetWaypointHere(WP_p1)"/>
+      <stay wp="end_trans"/>
+    </block>
+    <block name="line_trans_quad">
+      <set value="false" var="force_forward"/>
+      <call_once fun="NavSetWaypointHere(WP_p1)"/>
+      <stay wp="end_trans"/>
+    </block>
+    <block name="route">
+      <set value="false" var="force_forward"/>
+      <go from="p1" hmode="route" wp="p2"/>
+      <go from="p2" hmode="route" wp="p3"/>
+      <go from="p3" hmode="route" wp="p4"/>
+      <go from="p4" hmode="route" wp="p1"/>
+      <deroute block="route"/>
+    </block>
+    <block name="Oval">
+      <set value="false" var="force_forward"/>
+      <oval p1="p1" p2="p2" radius="100"/>
+    </block>
+    <block name="Circle_CW">
+      <set value="false" var="force_forward"/>
+      <circle radius="100" wp="circ"/>
+    </block>
+    <block name="Circle_CCW">
+      <set value="false" var="force_forward"/>
+      <circle radius="-100" wp="circ"/>
+    </block>
+    <block name="Circle_CW_fwd">
+      <set value="true" var="force_forward"/>
+      <circle radius="100" wp="circ"/>
+    </block>
+    <block name="Circle_CCW_fwd">
+      <set value="true" var="force_forward"/>
+      <circle radius="-100" wp="circ"/>
+    </block>
+    <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
+        <set value="false" var="force_forward"/>
+        <call_once fun="NavSetWaypointHere(WP_TD)"/>
+    </block>
+    <block name="land">
+        <set value="false" var="force_forward"/>
+        <go wp="TD"/>
+    </block>
+    <block name="descend">
+        <set value="false" var="force_forward"/>
+        <exception cond="!(GetPosAlt()  @LT 12.0)" deroute="flare"/>
+        <stay climb="-1.0" vmode="climb" wp="TD"/>
+    </block>
+    <block name="flare">
+        <set value="false" var="force_forward"/>
+        <stay climb="-0.5" vmode="climb" wp="TD"/>
+        <exception cond="!(GetPosAlt()  @LT 2.0)" deroute="flare_low"/>
+    </block>
+    <block name="flare_low">
+        <set value="false" var="force_forward"/>
+        <!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
+        <exception cond="!nav_is_in_flight()" deroute="landed"/>
+        <!-- <call_once fun="NavStartDetectGround()"/> -->
+        <!-- <stay climb="-0.5" vmode="climb" wp="TD"/> -->
+    </block>
+    <block name="landed">
+        <set value="false" var="force_forward"/>
+        <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+  </blocks>
+</flight_plan>
diff --git a/conf/modules/ctrl_eff_sched_rot_wing.xml b/conf/modules/ctrl_eff_sched_rot_wing.xml
@@ -1,7 +1,9 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 <module name="ctrl_eff_sched_rot_wing" dir="ctrl">
   <doc>
-    <description>The control effectiveness scheduler for the rotating wing drone type</description>
+    <description>The control effectiveness scheduler for the rotating wing quadplane drone type
+     - it requires a servo called ROTATION_MECH
+    </description>
     <section name="ROT_WING" prefix="ROT_WING_EFF_SCHED_">
       <define name="IXX_BODY" value="0"  description=""/>
       <define name="IYY_BODY" value="0"  description=""/>
@@ -30,6 +32,7 @@
   <makefile>
     <file name="ctrl_eff_sched_rot_wing.c"/>
     <test>
+      <define name="SERVO_ROTATION_MECH" value="0"/>
       <define name="INDI_NUM_ACT" value="4"/>
       <define name="INDI_OUTPUTS" value="3"/>
       <define name="ROT_WING_EFF_SCHED_IXX_BODY" value="1"/>

diff --git a/conf/simulator/jsbsim/aircraft/rotwing25.xml b/conf/simulator/jsbsim/aircraft/rotwing25.xml
@@ -41,7 +41,7 @@
     <ixy unit="SLUG*FT2"> 0. </ixy>
     <ixz unit="SLUG*FT2"> 0. </ixz>
     <iyz unit="SLUG*FT2"> 0. </iyz>
-        <emptywt unit="LBS"> 28.67 </emptywt>
+        <emptywt unit="LBS"> 56 </emptywt>
     <location name="CG" unit="M">
       <x> 0 </x>
       <y> 0 </y>

diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
@@ -571,4 +571,15 @@
    settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml"
    gui_color="red"
   />
+  <aircraft
+   name="rot_wing_25kg"
+   ac_id="29"
+   airframe="airframes/tudelft/rot_wing_25kg.xml"
+   radio="radios/crossfire_sbus.xml"
+   telemetry="telemetry/highspeed_rotorcraft.xml"
+   flight_plan="flight_plans/tudelft/rotating_wing25kg_EHVB.xml"
+   settings="settings/rotorcraft_basic.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/ctrl_eff_sched_rot_wing.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml"
+   gui_color="red"
+  />
 </conf>
diff --git a/sw/airborne/filters/notch_filter_float.h b/sw/airborne/filters/notch_filter_float.h
@@ -101,7 +101,7 @@ static inline void notch_filter_update(struct SecondOrderNotchFilter *filter, fl
  */
 
 
-static inline float notch_filter_get_output(struct notch_filter_float *filter)
+static inline float notch_filter_get_output(struct SecondOrderNotchFilter *filter)
 {
   return filter->yn1;
 }

diff --git a/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c b/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c
@@ -28,6 +28,10 @@
 #include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
 #include "modules/core/abi.h"
 
+#ifndef SERVO_ROTATION_MECH
+#error ctrl_eff_sched_rot_wing requires a servo named ROTATION_MECH
+#endif
+
 
 #ifndef ROT_WING_EFF_SCHED_IXX_BODY
 #error "NO ROT_WING_EFF_SCHED_IXX_BODY defined"
@@ -117,7 +121,7 @@ static abi_event wing_position_ev;
 static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
 {
   for (int i=0; i<num_act; i++){
-    if (pos_msg[i].set.position && (pos_msg[i].idx =  7))
+    if (pos_msg[i].set.position && (pos_msg[i].idx =  SERVO_ROTATION_MECH))
     {
       // Get wing rotation angle from sensor
       eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;

diff --git a/sw/airborne/modules/radio_control/rc_datalink.h b/sw/airborne/modules/radio_control/rc_datalink.h
@@ -40,6 +40,10 @@
 #define RADIO_YAW       2
 #define RADIO_THROTTLE  3
 #define RADIO_MODE      4
+#define RADIO_AUX1      5
+#define RADIO_AUX2      6
+#define RADIO_AUX4      7
+#define RADIO_AUX6      8
 
 extern int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
 extern volatile bool rc_dl_frame_available;