22 changes: 22 additions & 0 deletions conf/modules/rot_wing_auto_doublet.xml
View file
@@ -0,0 +1,22 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="rot_wing_auto_doublet" dir="system_identification">
<doc>
<description>Module that automatically runs a doublet program for the rotating wing drone</description>
</doc>
<settings>
<dl_settings NAME="AutoDoublet">
<dl_settings name="auto_doublet">
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="activated" var="rot_wing_auto_doublet_activated" type="uint8_t" module="system_identification/rot_wing_auto_doublet" handler="on_activation"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="rot_wing_auto_doublet.h"/>
</header>
<init fun="init_rot_wing_auto_doublet()"/>
<periodic fun="periodic_rot_wing_auto_doublet()" freq="20.0"/>
<makefile>
<file name="rot_wing_auto_doublet.c"/>
<test/>
</makefile>
</module>
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>
8 changes: 6 additions & 2 deletions conf/modules/stabilization_indi.xml
View file
Expand Up @@ -44,6 +44,10 @@
<settings>
<dl_settings>
<dl_settings NAME="indi">
<dl_setting var="indi_Wu_motor" min="1" step="1." max="200" shortname="Wu_motor" param="STABILIZATION_INDI_WLS_WU_MOTOR"/>
<dl_setting var="indi_Wu_elevator" min="1" step="1." max="200" shortname="Wu_elevator" param="STABILIZATION_INDI_WLS_WU_ELEVATOR"/>
<dl_setting var="indi_Wu_rudder" min="1" step="1." max="200" shortname="Wu_rudder" param="STABILIZATION_INDI_WLS_WU_RUDDER"/>
<dl_setting var="indi_Wu_aileron" min="1" step="1." max="200" shortname="Wu_aileron" param="STABILIZATION_INDI_WLS_WU_AILERON"/>
<dl_setting var="indi_gains.att.p" min="0" step="1" max="2500" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P"/>
<dl_setting var="indi_gains.rate.p" min="0.1" step="0.1" max="100" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="indi_gains.att.q" min="0" step="1" max="2500" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q"/>
Expand Down Expand Up @@ -72,8 +76,8 @@
<configure name="INDI_NUM_ACT" default="4"/>
<define name="INDI_OUTPUTS" value="$(INDI_OUTPUTS)"/>
<define name="INDI_NUM_ACT" value="$(INDI_NUM_ACT)"/>
<define name="CA_N_V" value="$(INDI_OUTPUTS)"/>
<define name="CA_N_U" value="$(INDI_NUM_ACT)"/>
<define name="CA_N_V_INNER" value="$(INDI_OUTPUTS)"/>
<define name="CA_N_U_INNER" value="$(INDI_NUM_ACT)"/>
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_quat_indi.h" type="string"/>
<define name="STABILIZATION_ATTITUDE_INDI_FULL" value="true"/>
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/sys_id_chirp.xml
View file
Expand Up @@ -43,7 +43,7 @@
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Chirp" var="chirp_active" type="uint8_t" module="system_identification/sys_id_chirp" handler="activate_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Fade in" var="chirp_fade_in" type="uint8_t" module="system_identification/sys_id_chirp" handler="fade_in_activate_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Exponential" var="chirp_exponential" type="uint8_t" module="system_identification/sys_id_chirp" handler="exponential_activate_handler"/>
<dl_setting min="0" max="5" step="1" shortname="Chirp axis" var="chirp_axis" type="uint8_t" module="system_identification/sys_id_chirp" handler="axis_handler"/>
<dl_setting min="0" max="7" step="1" shortname="Chirp axis" var="chirp_axis" type="uint8_t" module="system_identification/sys_id_chirp" handler="axis_handler"/>
<dl_setting min="0" max="9600" step="100" shortname="Amplitude" var="chirp_amplitude" type="int32_t" module="system_identification/sys_id_chirp"/>
<dl_setting min="0" max="0.5" step="0.01" shortname="on-axis noise" var="chirp_noise_stdv_onaxis_ratio" type="float" module="system_identification/sys_id_chirp"/>
<dl_setting min="0" max="9600" step="50" shortname="off-axis noise" var="chirp_noise_stdv_offaxis" type="float" module="system_identification/sys_id_chirp"/>
Expand Down
8 changes: 4 additions & 4 deletions conf/modules/sys_id_doublet.xml
View file
Expand Up @@ -41,16 +41,16 @@
</description>
<define name="DOUBLET_AXES" value="{COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW}" description="Which axes the doublet is applied to (specify as array with {})"/>
<define name="DOUBLET_ENABLED" value="TRUE|FALSE" description="If false, the doublet does not run and values are not added"/>
<define name="DOUBLET_MOD3211" value="TRUE|FALSE" description="If true, the 3-2-1-1 doublet will be executed instead of the normal doublet"/>
<define name="DOUBLET_MOD" value="TRUE|FALSE" description="If true, the 3-2-1-1 doublet will be executed instead of the normal doublet"/>
</doc>

<settings>
<dl_settings name="System identification">
<dl_settings name="Doublet input">
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Doublet" var="doublet_active" type="uint8_t" module="system_identification/sys_id_doublet" handler="activate_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="3-2-1-1 mode" var="doublet_mode_3211" type="uint8_t" module="system_identification/sys_id_doublet" handler="mod3211_handler"/>
<dl_setting min="0" max="9600" step="100" shortname="Amplitude" var="doublet_amplitude" type="int32_t" module="system_identification/sys_id_doublet"/>
<dl_setting min="0" max="5" step="1" shortname="Doublet axis" var="doublet_axis" type="uint8_t" module="system_identification/sys_id_doublet" handler="axis_handler"/>
<dl_setting min="0" max="2" step="1" shortname="mode" var="doublet_mode" type="uint8_t" module="system_identification/sys_id_doublet" handler="mod_handler"/>
<dl_setting min="-9600" max="9600" step="100" shortname="Amplitude" var="doublet_amplitude" type="int32_t" module="system_identification/sys_id_doublet"/>
<dl_setting min="0" max="7" step="1" shortname="Doublet axis" var="doublet_axis" type="uint8_t" module="system_identification/sys_id_doublet" handler="axis_handler"/>
<dl_setting min="0" max="100" step="0.5" shortname="Length_s" var="doublet_length_s" type="float" module="system_identification/sys_id_doublet"/>
<dl_setting min="0" max="100" step="0.5" shortname="Extra_waiting_s" var="doublet_extra_waiting_time_s" type="float" module="system_identification/sys_id_doublet"/>
</dl_settings>
Expand Down
51 changes: 51 additions & 0 deletions conf/modules/wind_tunnel_rot_wing.xml
View file
@@ -0,0 +1,51 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="wind_tunnel_rot_wing" dir="wind_tunnel">
<doc>
<description>This module allows the user to control seperate actuators for example during wind tunnel experiments.</description>
</doc>
<settings>
<dl_settings name="Wind tunnel">
<dl_settings name="Wt act control">
<dl_setting min="0" max="1" step="1" values="Off|On" shortname="motors_on" var="motors_on_wt" type="bool"/>
<dl_setting min="0" max="1" step="1" values="Off|On" shortname="front_on" var="motor_on_wt[0]" type="bool"/>
<dl_setting min="0" max="1" step="1" values="Off|On" shortname="right_on" var="motor_on_wt[1]" type="bool"/>
<dl_setting min="0" max="1" step="1" values="Off|On" shortname="back_on" var="motor_on_wt[2]" type="bool"/>
<dl_setting min="0" max="1" step="1" values="Off|On" shortname="left_on" var="motor_on_wt[3]" type="bool"/>
<dl_setting min="0" max="1" step="1" values="Off|On" shortname="push_on" var="motor_on_wt[4]" type="bool"/>
<dl_setting min="0" max="9600" step="1" shortname="front_cmd" var="actuators_slider_wt[0]" type="int16_t"/>
<dl_setting min="0" max="9600" step="1" shortname="right_cmd" var="actuators_slider_wt[1]" type="int16_t"/>
<dl_setting min="0" max="9600" step="1" shortname="back_cmd" var="actuators_slider_wt[2]" type="int16_t"/>
<dl_setting min="0" max="9600" step="1" shortname="left_cmd" var="actuators_slider_wt[3]" type="int16_t"/>
<dl_setting min="0" max="9600" step="1" shortname="push_cmd" var="actuators_slider_wt[4]" type="int16_t"/>
<dl_setting min="-9600" max="9600" step="1" shortname="elevator_cmd" var="actuators_slider_wt[9]" type="int16_t"/>
<dl_setting min="-9600" max="9600" step="1" shortname="rudder_cmd" var="actuators_slider_wt[10]" type="int16_t"/>
<dl_setting min="0" max="9600" step="1" shortname="motors_cmd" var="motors_slider_wt" type="int16_t"/>
<dl_setting min="0" max="11" step="1" shortname="sweep_idx" var="wt_actuator_sweep_index" type="uint8_t"/>
<dl_setting min="-9600" max="9600" step="1" shortname="min_sweep" var="wt_input_min_cmd" type="int16_t"/>
<dl_setting min="-9600" max="9600" step="1" shortname="max_sweep" var="wt_input_max_cmd" type="int16_t"/>
<dl_setting min="0" max="20" step="0.1" shortname="steptime" var="wt_input_steptime" type="float"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" var="wt_sweep_running" shortname="Sweep" type="bool" module="wind_tunnel/wind_tunnel_rot_wing" handler="sweep_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" var="wt_sweep_motors_running" shortname="Motors_sweep" type="bool" module="wind_tunnel/wind_tunnel_rot_wing" handler="sweep_motors_handler"/>
<!--<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Chirp" var="chirp_active" type="uint8_t" module="system_identification/sys_id_chirp" handler="activate_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Fade in" var="chirp_fade_in" type="uint8_t" module="system_identification/sys_id_chirp" handler="fade_in_activate_handler"/>
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Exponential" var="chirp_exponential" type="uint8_t" module="system_identification/sys_id_chirp" handler="exponential_activate_handler"/>
<dl_setting min="0" max="7" step="1" shortname="Chirp axis" var="chirp_axis" type="uint8_t" module="system_identification/sys_id_chirp" handler="axis_handler"/>
<dl_setting min="0" max="9600" step="100" shortname="Amplitude" var="chirp_amplitude" type="int32_t" module="system_identification/sys_id_chirp"/>
<dl_setting min="0" max="0.5" step="0.01" shortname="on-axis noise" var="chirp_noise_stdv_onaxis_ratio" type="float" module="system_identification/sys_id_chirp"/>
<dl_setting min="0" max="9600" step="50" shortname="off-axis noise" var="chirp_noise_stdv_offaxis" type="float" module="system_identification/sys_id_chirp"/>
<dl_setting min="0.05" max="20" step="0.05" shortname="Fstart_hz" var="chirp_fstart_hz" type="float" module="system_identification/sys_id_chirp" handler="fstart_handler"/>
<dl_setting min="0.1" max="20" step="0.1" shortname="Fend_hz" var="chirp_fstop_hz" type="float" module="system_identification/sys_id_chirp" handler="fstop_handler"/>
<dl_setting min="0" max="100" step="0.5" shortname="Length_s" var="chirp_length_s" type="float" module="system_identification/sys_id_chirp"/>-->
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="wind_tunnel_rot_wing.h"/>
</header>
<init fun="init_wt_rot_wing()"/>
<event fun="event_wt_rot_wing()"/>
<makefile>
<file name="wind_tunnel_rot_wing.c"/>
<test/>
</makefile>
</module>
27 changes: 27 additions & 0 deletions conf/modules/wing_rotation_controller_v3b.xml
View file
@@ -0,0 +1,27 @@
<!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="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>
<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
448 changes: 448 additions & 0 deletions conf/simulator/jsbsim/aircraft/rotwing25.xml
View file

Large diffs are not rendered by default.

259 changes: 259 additions & 0 deletions conf/simulator/jsbsim/aircraft/rotwingv3.xml
View file
@@ -0,0 +1,259 @@
<?xml version="1.0"?>
<?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?>
<fdm_config name="QUAD COMPLETE EXT" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd">

<fileheader>
<author>Dennis van Wijngaarden </author>
<filecreationdate>08-04-2022</filecreationdate>
<version>Version 0.9 - beta</version>
<description>Rotating wing drone v3 with actuator dynamics (NE/SW turning CCW, NW/SE CW)</description>
</fileheader>

<metrics>
<wingarea unit="IN2"> 78.53 </wingarea>
<wingspan unit="IN"> 10 </wingspan>
<chord unit="IN"> 6.89 </chord>
<htailarea unit="FT2"> 0 </htailarea>
<htailarm unit="FT"> 0 </htailarm>
<vtailarea unit="FT2"> 0 </vtailarea>
<vtailarm unit="FT"> 0 </vtailarm>
<location name="AERORP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="EYEPOINT" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="VRP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</metrics>

<mass_balance>
<ixx unit="SLUG*FT2"> 0.047772 </ixx>
<iyy unit="SLUG*FT2"> 0.054670 </iyy>
<izz unit="SLUG*FT2"> 1.203701 </izz>
<ixy unit="SLUG*FT2"> 0. </ixy>
<ixz unit="SLUG*FT2"> 0. </ixz>
<iyz unit="SLUG*FT2"> 0. </iyz>
<emptywt unit="LBS"> 14.52846 </emptywt>
<location name="CG" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</mass_balance>

<ground_reactions>
<contact type="STRUCTURE" name="CONTACT_FRONT">
<location unit="M">
<x>-0.15 </x>
<y> 0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_BACK">
<location unit="M">
<x> 0.15</x>
<y> 0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_RIGHT">
<location unit="M">
<x> 0. </x>
<y> 0.15</y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_LEFT">
<location unit="M">
<x> 0. </x>
<y>-0.15</y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
</ground_reactions>

<flight_control name="actuator_dynamics">
<channel name="filtering">

<!--First order filter represents actuator dynamics-->
<lag_filter name="front_motor_lag">
<input> fcs/front_motor </input>
<c1> 50 </c1>
<output> fcs/front_motor_lag</output>
</lag_filter>
<lag_filter name="right_motor_lag">
<input> fcs/right_motor </input>
<c1> 50 </c1>
<output> fcs/right_motor_lag</output>
</lag_filter>
<lag_filter name="back_motor_lag">
<input> fcs/back_motor </input>
<c1> 50 </c1>
<output> fcs/back_motor_lag</output>
</lag_filter>
<lag_filter name="left_motor_lag">
<input> fcs/left_motor </input>
<c1> 50 </c1>
<output> fcs/left_motor_lag</output>
</lag_filter>
</channel>

</flight_control>

<external_reactions>

<property>fcs/front_motor</property>
<property>fcs/front_motor_lag</property>
<property>fcs/right_motor</property>
<property>fcs/right_motor_lag</property>
<property>fcs/back_motor</property>
<property>fcs/back_motor_lag</property>
<property>fcs/left_motor</property>
<property>fcs/left_motor_lag</property>


<!-- First the lift forces produced by each propeller -->

<force name="front_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/front_motor_lag</property>
<value> 7.0 </value>
</product>
</function>
<location unit="IN">
<x>-18.504</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>

<force name="right_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/right_motor_lag</property>
<value> 7.0 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>14.567</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>

<force name="back_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/back_motor_lag</property>
<value> 7.0 </value>
</product>
</function>
<location unit="IN">
<x>18.504</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>

<force name="left_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/left_motor_lag</property>
<value> 7.0 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>-14.567</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>

<!-- Then the Moment Couples -->


</external_reactions>

<propulsion/>

<flight_control name="FGFCS"/>

<aerodynamics>
<axis name="DRAG">
<function name="aero/coefficient/CD">
<description>Drag</description>
<product>
<property>aero/qbar-psf</property>
<value>47.9</value> <!-- Conversion to pascals -->
<value>0.0151</value> <!-- CD x Area (m^2) -->
<value>0.224808943</value> <!-- N to LBS -->
</product>
</function>
</axis>
</aerodynamics>

</fdm_config>
448 changes: 448 additions & 0 deletions conf/simulator/jsbsim/aircraft/rotwingv3b.xml
View file

Large diffs are not rendered by default.

939 changes: 939 additions & 0 deletions conf/simulator/jsbsim/aircraft/rotwingv3b_fwd.xml
View file

Large diffs are not rendered by default.

22 changes: 22 additions & 0 deletions conf/telemetry/highspeed_rotorcraft.xml
View file
Expand Up @@ -47,6 +47,16 @@
<message name="AHRS_REF_QUAT" period="0.1"/>
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="GPS_RTK" period="0.1"/>
<message name="STAB_ATTITUDE_FULL_INDI" period="0.05"/>
<message name="ROT_WING_CONTROLLER" period="0.2"/>
<message name="AOA" period="0.2"/>
<message name="ACTUATORS" period="0.02"/>
<message name="INDI_G" period="0.2"/>
<message name="APPROACH_MOVING_TARGET" period="0.1"/>
<message name="AIRSPEED_UAVCAN" period="0.1"/>
<message name="TARGET_POS_INFO" period="0.1"/>
<message name="THRUST_BX_EFF" period="0.1"/>
<message name="ROTATING_WING_STATE" period="0.2"/>
</mode>

<mode name="calibration">
Expand Down Expand Up @@ -242,6 +252,18 @@
<message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/>
<message name="IMU_HEATER" period="1.0"/>
<message name="STAB_ATTITUDE_FULL_INDI" period="0.002"/>
<message name="ROT_WING_CONTROLLER" period="0.2"/>
<message name="AOA" period="0.02"/>
<message name="DOUBLET" period="0.002"/>
<message name="APPROACH_MOVING_TARGET" period="0.1"/>
<message name="TARGET_POS_INFO" period="0.1"/>
<message name="AIRSPEED_MS45XX" period="0.002"/>
<message name="AIRSPEED_UAVCAN" period="0.002"/>
<message name="AIRSPEED_WIND_ESTIMATOR_EKF" period="0.04"/>
<message name="AIRSPEED_WIND_ESTIMATOR_EKF_COV" period="0.04"/>
<message name="AIRSPEED_WIND_ESTIMATOR_EKF_FORCES" period="0.04"/>
<message name="ROTATING_WING_STATE" period="0.1"/>
</mode>
</process>

Expand Down
254 changes: 254 additions & 0 deletions conf/telemetry/highspeed_rotorcraft_windtunnel.xml
View file
@@ -0,0 +1,254 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>


<process name="Main">

<mode name="default" key_press="d">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="0.7"/>
<message name="ROTORCRAFT_STATUS" period="0.4"/>
<message name="ROTORCRAFT_FP" period="0.20"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CAM" period="1."/>
<message name="GPS_INT" period=".2"/>
<message name="INS" period=".2"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="SUPERBITRF" period="3"/>
<message name="ENERGY" period="0.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="0.6"/>
<message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.05"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="OPTICAL_FLOW_HOVER" period="0.05"/>
<message name="VISUALTARGET" period="0.10"/>
<message name="VISION_POSITION_ESTIMATE" period="0.1"/>
<message name="DIVERGENCE" period="0.05"/>
<message name="DRAGSPEED" period="0.02"/>
<message name="LOGGER_STATUS" period="5.1"/>
<message name="LIDAR" period="1.2"/>
<message name="INS_EKF2" period=".25"/>
<message name="INS_EKF2_EXT" period="4.25"/>
<message name="FBW_STATUS" period="2.1"/>
<message name="WIND_INFO_RET" period="9.2"/>
<message name="AHRS_BIAS" period="7.5"/>
<message name="HOVER_LOOP" period="0.3"/>
<message name="GUIDANCE_H_REF_INT" period="0.31"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.4"/>
<message name="HYBRID_GUIDANCE" period="0.4"/>
<message name="ESC" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.1"/>
<message name="GPS_RTK" period="0.1"/>
<message name="INDI_G" period="0.1"/>
<message name="ACTUATORS" period="0.1"/>
<message name="STAB_ATTITUDE_FULL_INDI" period="0.05"/>
<message name="ROT_WING_CONTROLLER" period="0.2"/>
</mode>

<mode name="calibration">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="IMU_MAG_CURRENT_CALIBRATION" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="GPS_INT" period=".2"/>
</mode>

<mode name="ppm">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1"/>
<message name="BEBOP_ACTUATORS" period="0.2"/>
</mode>

<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
</mode>

<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO" period=".075"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>

<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<message name="AHRS_QUAT_INT" period=".25"/>
<message name="AHRS_EULER_INT" period=".1"/>
<message name="AHRS_EULER" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>

<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>

<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>

<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/>
<message name="INDI_G" period="2.0"/>
</mode>

<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VFF_EXTENDED" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS_Z" period=".05"/>
<message name="INS" period=".11"/>
<message name="INS_REF" period="5.1"/>
</mode>

<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>

<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
<message name="GUIDANCE_H_REF_INT" period="0.062"/>
<message name="STAB_ATTITUDE_INT" period="0.4"/>
<message name="STAB_ATTITUDE_FLOAT" period="0.4"/>
<!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>

<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>

<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>

<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
</mode>

</process>

<process name="FlightRecorder">
<mode name="default">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="GPS" period="11.1"/>
<message name="DL_VALUE" period="2.5"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.002"/>
<message name="ROTORCRAFT_CMD" period=".002"/>
<message name="COMMANDS" period=".02"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period=".1"/>
<message name="INS" period=".002"/>
<message name="INS_Z" period=".1"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="ENERGY" period="0.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="0.02"/>
<message name="SURVEY" period="2.5"/>
<message name="IMU_GYRO_SCALED" period=".0006667"/>
<message name="IMU_ACCEL_SCALED" period=".0006667"/>
<message name="IMU_MAG_SCALED" period=".0125"/>
<message name="LIDAR" period="0.05"/>
<message name="INS_EKF2" period="0.05"/>
<message name="INS_EKF2_EXT" period="0.1"/>
<message name="WIND_INFO_RET" period="0.2"/>
<message name="AHRS_BIAS" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.01"/>
<message name="GUIDANCE_H_REF_INT" period="0.02"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.002"/>
<message name="HYBRID_GUIDANCE" period="0.02"/>
<message name="ESC" period="0.02"/>
<!-- <message name="STAB_ATTITUDE_INDI" period="0.002"/> -->
<message name="PPM" period="0.05"/>
<!-- <message name="INDI_G" period="0.1"/> -->
<message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/>
<!-- <message name="STAB_ATTITUDE_FULL_INDI" period="0.002"/> -->
<message name="ROT_WING_CONTROLLER" period="0.2"/>
<!--<message name="WINDTUNNEL_MEAS" period="0.005"/>-->
</mode>
</process>

</telemetry>

33 changes: 33 additions & 0 deletions conf/userconf/tudelft/conf.xml
View file
Expand Up @@ -560,4 +560,37 @@
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/guidance_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
/>
<aircraft
name="RotatingWingV3b"
ac_id="19"
airframe="airframes/tudelft/rot_wing_v3b.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ctrl_eff_sched_rot_wing_v3b.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_rot_wing.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/rot_wing_auto_doublet.xml modules/rot_wing_automation.xml modules/stabilization_indi.xml modules/sys_id_doublet.xml modules/target_pos.xml modules/wing_rotation_controller_v3b.xml"
gui_color="red"
/>
<aircraft
name="RotatingWingV3a"
ac_id="23"
airframe="airframes/tudelft/rot_wing_v3a.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ctrl_eff_sched_rot_wing_v3a.xml modules/ekf_aw.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_rot_wing.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/rot_wing_automation.xml modules/stabilization_indi.xml modules/sys_id_doublet.xml modules/target_pos.xml modules/wing_rotation_controller_v3a.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/actuators_faulhaber.xml modules/air_data.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_rot_wing.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/rot_wing_auto_doublet.xml modules/stabilization_indi.xml modules/sys_id_doublet.xml"
gui_color="red"
/>
</conf>
4 changes: 2 additions & 2 deletions sw/airborne/boards/cube/orange/board.cfg
View file
Expand Up @@ -140,8 +140,8 @@ PD09 UART3_RX UART AF:USART3_RX # TELEM2
PD10 SPI_SLAVE7 OUTPUT PUSHPULL SPEED_HIGH FLOATING LEVEL_HIGH # INV FRAM_CS
PD11 UART3_CTS PASSIVE # TELEM2
PD12 UART3_RTS PASSIVE # TELEM2
PD13 SERVO5 PWM AF:TIM4_CH2 ()
PD14 SERVO6 PWM AF:TIM4_CH3 ()
PD13 PWM_INPUT1 ICU AF:TIM4_CH2 () # SERVO5
PD14 PWM_INPUT2 ICU AF:TIM4_CH3 () # SERVO6
PD15 MPU_DRDY PASSIVE

PE00 UART8_RX UART AF:UART8_RX # GPS2
Expand Down
60 changes: 29 additions & 31 deletions sw/airborne/boards/cube/orange/board.h
View file
Expand Up @@ -125,8 +125,8 @@
#define PD10_SPI_SLAVE7 10U
#define PD11_UART3_CTS 11U
#define PD12_UART3_RTS 12U
#define PD13_SERVO5 13U
#define PD14_SERVO6 14U
#define PD13_PWM_INPUT1 13U
#define PD14_PWM_INPUT2 14U
#define PD15_MPU_DRDY 15U

#define PE00_UART8_RX 0U
Expand Down Expand Up @@ -313,8 +313,8 @@
#define LINE_SPI_SLAVE7 PAL_LINE(GPIOD, 10U)
#define LINE_UART3_CTS PAL_LINE(GPIOD, 11U)
#define LINE_UART3_RTS PAL_LINE(GPIOD, 12U)
#define LINE_SERVO5 PAL_LINE(GPIOD, 13U)
#define LINE_SERVO6 PAL_LINE(GPIOD, 14U)
#define LINE_PWM_INPUT1 PAL_LINE(GPIOD, 13U)
#define LINE_PWM_INPUT2 PAL_LINE(GPIOD, 14U)
#define LINE_MPU_DRDY PAL_LINE(GPIOD, 15U)

#define LINE_UART8_RX PAL_LINE(GPIOE, 0U)
Expand Down Expand Up @@ -682,8 +682,8 @@
PIN_MODE_OUTPUT(PD10_SPI_SLAVE7) | \
PIN_MODE_INPUT(PD11_UART3_CTS) | \
PIN_MODE_INPUT(PD12_UART3_RTS) | \
PIN_MODE_ALTERNATE(PD13_SERVO5) | \
PIN_MODE_ALTERNATE(PD14_SERVO6) | \
PIN_MODE_ALTERNATE(PD13_PWM_INPUT1) | \
PIN_MODE_ALTERNATE(PD14_PWM_INPUT2) | \
PIN_MODE_INPUT(PD15_MPU_DRDY))

#define VAL_GPIOD_OTYPER (PIN_OTYPE_PUSHPULL(PD00_CAN1_RX) | \
Expand All @@ -699,8 +699,8 @@
PIN_OTYPE_PUSHPULL(PD10_SPI_SLAVE7) | \
PIN_OTYPE_OPENDRAIN(PD11_UART3_CTS) | \
PIN_OTYPE_OPENDRAIN(PD12_UART3_RTS) | \
PIN_OTYPE_PUSHPULL(PD13_SERVO5) | \
PIN_OTYPE_PUSHPULL(PD14_SERVO6) | \
PIN_OTYPE_PUSHPULL(PD13_PWM_INPUT1) | \
PIN_OTYPE_PUSHPULL(PD14_PWM_INPUT2) | \
PIN_OTYPE_OPENDRAIN(PD15_MPU_DRDY))

#define VAL_GPIOD_OSPEEDR (PIN_OSPEED_SPEED_HIGH(PD00_CAN1_RX) | \
Expand All @@ -716,8 +716,8 @@
PIN_OSPEED_SPEED_HIGH(PD10_SPI_SLAVE7) | \
PIN_OSPEED_SPEED_VERYLOW(PD11_UART3_CTS) | \
PIN_OSPEED_SPEED_VERYLOW(PD12_UART3_RTS) | \
PIN_OSPEED_SPEED_HIGH(PD13_SERVO5) | \
PIN_OSPEED_SPEED_HIGH(PD14_SERVO6) | \
PIN_OSPEED_SPEED_HIGH(PD13_PWM_INPUT1) | \
PIN_OSPEED_SPEED_HIGH(PD14_PWM_INPUT2) | \
PIN_OSPEED_SPEED_VERYLOW(PD15_MPU_DRDY))

#define VAL_GPIOD_PUPDR (PIN_PUPDR_FLOATING(PD00_CAN1_RX) | \
Expand All @@ -733,8 +733,8 @@
PIN_PUPDR_FLOATING(PD10_SPI_SLAVE7) | \
PIN_PUPDR_PULLDOWN(PD11_UART3_CTS) | \
PIN_PUPDR_PULLDOWN(PD12_UART3_RTS) | \
PIN_PUPDR_FLOATING(PD13_SERVO5) | \
PIN_PUPDR_FLOATING(PD14_SERVO6) | \
PIN_PUPDR_FLOATING(PD13_PWM_INPUT1) | \
PIN_PUPDR_FLOATING(PD14_PWM_INPUT2) | \
PIN_PUPDR_PULLDOWN(PD15_MPU_DRDY))

#define VAL_GPIOD_ODR (PIN_ODR_LEVEL_HIGH(PD00_CAN1_RX) | \
Expand All @@ -750,8 +750,8 @@
PIN_ODR_LEVEL_HIGH(PD10_SPI_SLAVE7) | \
PIN_ODR_LEVEL_HIGH(PD11_UART3_CTS) | \
PIN_ODR_LEVEL_HIGH(PD12_UART3_RTS) | \
PIN_ODR_LEVEL_LOW(PD13_SERVO5) | \
PIN_ODR_LEVEL_LOW(PD14_SERVO6) | \
PIN_ODR_LEVEL_HIGH(PD13_PWM_INPUT1) | \
PIN_ODR_LEVEL_HIGH(PD14_PWM_INPUT2) | \
PIN_ODR_LEVEL_HIGH(PD15_MPU_DRDY))

#define VAL_GPIOD_AFRL (PIN_AFIO_AF(PD00_CAN1_RX, 9) | \
Expand All @@ -768,8 +768,8 @@
PIN_AFIO_AF(PD10_SPI_SLAVE7, 0) | \
PIN_AFIO_AF(PD11_UART3_CTS, 0) | \
PIN_AFIO_AF(PD12_UART3_RTS, 0) | \
PIN_AFIO_AF(PD13_SERVO5, 2) | \
PIN_AFIO_AF(PD14_SERVO6, 2) | \
PIN_AFIO_AF(PD13_PWM_INPUT1, 2) | \
PIN_AFIO_AF(PD14_PWM_INPUT2, 2) | \
PIN_AFIO_AF(PD15_MPU_DRDY, 0))

#define VAL_GPIOE_MODER (PIN_MODE_ALTERNATE(PE00_UART8_RX) | \
Expand Down Expand Up @@ -1559,10 +1559,10 @@
#define AF_LINE_UART3_TX 7U
#define AF_PD09_UART3_RX 7U
#define AF_LINE_UART3_RX 7U
#define AF_PD13_SERVO5 2U
#define AF_LINE_SERVO5 2U
#define AF_PD14_SERVO6 2U
#define AF_LINE_SERVO6 2U
#define AF_PD13_PWM_INPUT1 2U
#define AF_LINE_PWM_INPUT1 2U
#define AF_PD14_PWM_INPUT2 2U
#define AF_LINE_PWM_INPUT2 2U
#define AF_PE00_UART8_RX 8U
#define AF_LINE_UART8_RX 8U
#define AF_PE01_UART8_TX 8U
Expand Down Expand Up @@ -1609,14 +1609,14 @@
#define ADC5_ADC 1
#define ADC5_ADC_FN INP
#define ADC5_ADC_INP 8
#define SERVO5_TIM 4
#define SERVO5_TIM_FN CH
#define SERVO5_TIM_CH 2
#define SERVO5_TIM_AF 2
#define SERVO6_TIM 4
#define SERVO6_TIM_FN CH
#define SERVO6_TIM_CH 3
#define SERVO6_TIM_AF 2
#define PWM_INPUT1_TIM 4
#define PWM_INPUT1_TIM_FN CH
#define PWM_INPUT1_TIM_CH 2
#define PWM_INPUT1_TIM_AF 2
#define PWM_INPUT2_TIM 4
#define PWM_INPUT2_TIM_FN CH
#define PWM_INPUT2_TIM_CH 3
#define PWM_INPUT2_TIM_AF 2
#define SERVO4_TIM 1
#define SERVO4_TIM_FN CH
#define SERVO4_TIM_CH 1
Expand Down Expand Up @@ -1658,15 +1658,13 @@
LINE_SPI_SLAVE5, \
LINE_SPI_SLAVE6, \
LINE_SPI_SLAVE7, \
LINE_SERVO5, \
LINE_SERVO6, \
LINE_SPI_SLAVE8, \
LINE_SERVO4, \
LINE_SERVO3, \
LINE_LED1, \
LINE_SERVO2, \
LINE_SERVO1
#define ENERGY_SAVE_INPUTS_SIZE 16
#define ENERGY_SAVE_INPUTS_SIZE 14

#define ENERGY_SAVE_LOWS \
LINE_VDD_5V_PERIPH_EN, \
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/boards/cube/orange/mcuconf.h
View file
Expand Up @@ -339,7 +339,7 @@
#define STM32_ICU_USE_TIM1 FALSE
#define STM32_ICU_USE_TIM2 FALSE
#define STM32_ICU_USE_TIM3 FALSE
#define STM32_ICU_USE_TIM4 FALSE
#define STM32_ICU_USE_TIM4 TRUE
#define STM32_ICU_USE_TIM5 FALSE
#define STM32_ICU_USE_TIM8 FALSE
#define STM32_ICU_USE_TIM12 FALSE
Expand Down Expand Up @@ -367,7 +367,7 @@
#define STM32_PWM_USE_TIM1 TRUE
#define STM32_PWM_USE_TIM2 FALSE
#define STM32_PWM_USE_TIM3 FALSE
#define STM32_PWM_USE_TIM4 TRUE
#define STM32_PWM_USE_TIM4 FALSE
#define STM32_PWM_USE_TIM5 FALSE
#define STM32_PWM_USE_TIM8 FALSE
#define STM32_PWM_USE_TIM9 FALSE
Expand Down
3 changes: 2 additions & 1 deletion sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
View file
Expand Up @@ -259,7 +259,8 @@ void autopilot_check_in_flight(bool motors_on)
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL) &&
!motors_on) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot.in_flight = false;
Expand Down
2 changes: 0 additions & 2 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
View file
Expand Up @@ -313,7 +313,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
//Calculate roll,pitch and thrust command
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);

//printf("abi thrust %f\n", euler_cmd.z);
AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);

// Coordinated turn
Expand Down Expand Up @@ -511,7 +510,6 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
/*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
BoundAbs(accel_sp.z, 3.0);

//printf("accel_sp %f %f %f\n", accel_sp.x, accel_sp.y, accel_sp.z);
return accel_sp;
}

Expand Down
903 changes: 903 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_rot_wing.c
View file

Large diffs are not rendered by default.

106 changes: 106 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_rot_wing.h
View file
@@ -0,0 +1,106 @@
/*
* Copyright (C) 2015 Ewoud Smeur <ewoud.smeur@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/

/**
* @file firmwares/rotorcraft/guidance/guidance_indi_rot_wing.h
*
* A guidance mode based on Incremental Nonlinear Dynamic Inversion
* Come to ICRA2016 to learn more!
*
*/

#ifndef GUIDANCE_INDI_ROT_WING_H
#define GUIDANCE_INDI_ROT_WING_H

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "filters/high_pass_filter.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"

// TODO change names for _indi_hybrid_

extern void guidance_indi_init(void);
extern void guidance_indi_enter(void);

enum GuidanceIndiHybrid_HMode {
GUIDANCE_INDI_HYBRID_H_POS,
GUIDANCE_INDI_HYBRID_H_SPEED,
GUIDANCE_INDI_HYBRID_H_ACCEL
};

enum GuidanceIndiHybrid_VMode {
GUIDANCE_INDI_HYBRID_V_POS,
GUIDANCE_INDI_HYBRID_V_SPEED,
GUIDANCE_INDI_HYBRID_V_ACCEL
};

extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp);
extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode);
extern void guidance_indi_propagate_filters(void);

struct guidance_indi_hybrid_params {
float pos_gain;
float pos_gainz;
float speed_gain;
float speed_gainz;
float heading_bank_gain;
};

extern struct guidance_indi_hybrid_params gih_params;
extern float guidance_indi_specific_force_gain;
extern float guidance_indi_max_airspeed;
extern float nav_max_speed;
extern bool take_heading_control;
extern bool force_forward; ///< forward flight for hybrid nav

extern struct FloatRates ff_rates;
extern bool ff_rates_set;

extern float guidance_indi_max_bank;
extern float push_first_order_constant;
extern float hover_motor_slowdown;
extern float a_diff_limit;
extern float a_diff_limit_z;
extern float rot_wing_max_pitch_limit_deg;
extern float rot_wing_min_pitch_limit_deg;
extern float pitch_pref_deg;
extern float airspeed_turn_lower_bound;

extern bool hover_motors_active;
extern bool bool_disable_hover_motors;

extern bool weather_vane_on;
extern float weather_vane_gain;

extern float pitch_priority_factor;
extern float roll_priority_factor;
extern float thrust_priority_factor;
extern float pusher_priority_factor;

extern float horizontal_accel_weight;
extern float vertical_accel_weight;

extern float climb_vspeed_fwd;
extern float descend_vspeed_fwd;

#endif /* GUIDANCE_INDI_ROT_WING_H */
265 changes: 246 additions & 19 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
View file

Large diffs are not rendered by default.

10 changes: 10 additions & 0 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
View file
Expand Up @@ -34,10 +34,20 @@ extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
extern float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
extern float actuator_state_filt_vect[INDI_NUM_ACT];
extern bool act_is_servo[INDI_NUM_ACT];

extern bool indi_use_adaptive;

extern float *Bwls[INDI_OUTPUTS];
extern float indi_Wu_motor;
extern float indi_Wu_elevator;
extern float indi_Wu_rudder;
extern float indi_Wu_aileron;

extern float thrust_bx_eff;
extern float thrust_bx_act_dyn;
extern float actuator_thrust_bx_pprz;
extern float thrust_bx_state_filt;

extern float act_pref[INDI_NUM_ACT];

Expand Down
35 changes: 27 additions & 8 deletions sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c
View file
Expand Up @@ -52,12 +52,33 @@ void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, flo
#define WLS_VERBOSE FALSE

// Problem size needs to be predefined to avoid having to use VLAs
#ifndef CA_N_V
#error CA_N_V needs to be defined!
#ifdef CA_N_V_INNER
#ifdef CA_N_V_OUTER

#if CA_N_V_INNER > CA_N_V_OUTER
#define CA_N_V CA_N_V_INNER
#else
#define CA_N_V CA_N_V_OUTER
#endif

#if CA_N_U_INNER > CA_N_U_OUTER
#define CA_N_U CA_N_U_INNER
#else
#define CA_N_U CA_N_U_OUTER
#endif

#ifndef CA_N_U
#error CA_N_U needs to be defined!
#else
#define CA_N_V CA_N_V_INNER
#define CA_N_U CA_N_U_INNER
#endif

#elif CA_N_U_OUTER
#define CA_N_V CA_N_V_OUTER
#define CA_N_U CA_N_U_OUTER
#else

#error CA_N_V_INNER or CA_N_V_OUTER needs to be defined!

#endif

#define CA_N_C (CA_N_U+CA_N_V)
Expand Down Expand Up @@ -111,14 +132,12 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
*/
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
float* u_guess, float* W_init, float* Wv, float* Wu, float* up,
float gamma_sq, int imax) {
float gamma_sq, int imax, int n_u, int n_v) {
// allocate variables, use defaults where parameters are set to 0
if(!gamma_sq) gamma_sq = 100000;
if(!imax) imax = 100;

int n_c = CA_N_C;
int n_u = CA_N_U;
int n_v = CA_N_V;
int n_c = n_u + n_v;

float A[CA_N_C][CA_N_U];
float A_free[CA_N_C][CA_N_U];
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h
View file
Expand Up @@ -58,4 +58,4 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
*/
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
float* u_guess, float* W_init, float* Wv, float* Wu,
float* ud, float gamma, int imax);
float* ud, float gamma, int imax, int n_u, int n_v);
6 changes: 6 additions & 0 deletions sw/airborne/modules/actuators/actuators.h
View file
Expand Up @@ -40,6 +40,12 @@
extern void actuators_init(void);
extern void actuators_periodic(void);

// New actuator structure for ABI Message
struct rpm_act_t {
uint8_t actuator_idx;
int32_t rpm;
};

#if ACTUATORS_NB

extern uint32_t actuators_delay_time;
Expand Down
57 changes: 56 additions & 1 deletion sw/airborne/modules/actuators/actuators_uavcan.c
View file
Expand Up @@ -43,6 +43,7 @@ struct actuators_uavcan_telem_t {
float voltage;
float current;
float temperature;
float temperature_dev;
int32_t rpm;
uint32_t energy;
};
Expand Down Expand Up @@ -85,9 +86,15 @@ int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB];
#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE (0xD8A7486238EC3AF3ULL)
#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE ((484 + 7)/8)

/* uavcan EQUIMPENT_DEVICE_TEMPERATURE message definition */
#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID 1110
#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE (0x70261C28A94144C6ULL)
#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE ((40 + 7)/8)

/* private variables */
static bool actuators_uavcan_initialized = false;
static uavcan_event esc_status_ev;
static uavcan_event device_temperature_ev;

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
Expand Down Expand Up @@ -150,7 +157,7 @@ static void actuators_uavcan_send_esc(struct transport_tx *trans, struct link_de
float rpm = telem->rpm;
float energy = telem->energy;
pprz_msg_send_ESC(trans, dev, AC_ID, &telem->current, &electrical.vsupply, &power,
&rpm, &telem->voltage, &energy, &telem->temperature, &telem->node_id, &old_idx);
&rpm, &telem->voltage, &energy, &telem->temperature, &telem->temperature_dev, &telem->node_id, &old_idx);
}
#endif

Expand Down Expand Up @@ -208,8 +215,53 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR
}
#endif
#endif

// Create struct for ABI
#ifdef SERVOS_UAVCAN1_NB
struct rpm_act_t rpm_message;
rpm_message.actuator_idx = esc_idx;
rpm_message.rpm = telem[esc_idx].rpm;

// Send ABI message
AbiSendMsgRPM(RPM_SENSOR_ID, &rpm_message, SERVOS_UAVCAN1_NB);
#endif
}

/**
* Whevener an DEVICE_TEMPERATURE message from the EQUIPMENT group is received
*/
static void actuators_uavcan_device_temperature_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
{
uint16_t device_id;
uint16_t tmp_float;

struct actuators_uavcan_telem_t *telem = NULL;
uint8_t max_id = 0;
#ifdef SERVOS_UAVCAN1_NB
if (iface == &uavcan1) {
telem = uavcan1_telem;
max_id = SERVOS_UAVCAN1_NB;
}
#endif
#ifdef SERVOS_UAVCAN2_NB
if (iface == &uavcan2) {
telem = uavcan2_telem;
max_id = SERVOS_UAVCAN2_NB;
}
#endif


canardDecodeScalar(transfer, 0, 16, false, (void*)&device_id);
//Could not find the right interface
if (device_id >= max_id || telem == NULL || max_id == 0) {
return;
}

canardDecodeScalar(transfer, 16, 16, false, (void*)&tmp_float);
telem[device_id].temperature_dev = canardConvertFloat16ToNativeFloat(tmp_float);
}


/**
* Initialize an uavcan interface
*/
Expand All @@ -221,6 +273,9 @@ void actuators_uavcan_init(struct uavcan_iface_t *iface __attribute__((unused)))
// Bind uavcan ESC_STATUS message from EQUIPMENT
uavcan_bind(UAVCAN_EQUIPMENT_ESC_STATUS_ID, UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, &esc_status_ev,
&actuators_uavcan_esc_status_cb);
// Bind uavcan DEVICE_TEMPERATURE message from EQUIPMENT
uavcan_bind(UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID, UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE, &device_temperature_ev,
&actuators_uavcan_device_temperature_cb);

// Configure telemetry
#if PERIODIC_TELEMETRY
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/core/abi_common.h
View file
Expand Up @@ -34,6 +34,7 @@
#include "math/pprz_algebra_float.h"
#include "modules/gps/gps.h"
#include "modules/radio_control/radio_control.h"
#include "modules/actuators/actuators.h"
/* Include here headers with structure definition you may want to use with ABI
* Ex: '#include "modules/gps/gps.h"' in order to use the GpsState structure
*/
Expand Down
12 changes: 12 additions & 0 deletions sw/airborne/modules/core/abi_sender_ids.h
View file
Expand Up @@ -482,6 +482,10 @@
#define THRUST_INCREMENT_ID 1
#endif

#ifndef THRUST_BX_INCREMENT_ID
#define THRUST_BX_INCREMENT_ID 2
#endif

#ifndef MAG_CALIB_UKF_ID
#define MAG_CALIB_UKF_ID 20
#endif
Expand Down Expand Up @@ -624,4 +628,12 @@
#define VEL_SP_FCR_ID 1 // Approach Moving Target
#endif

/*
* IDs of LIFT_D senders
*/

#ifndef LIFT_D_SCHED_ID
#define LIFT_D_SCHED_ID 1
#endif

#endif /* ABI_SENDER_IDS_H */
6 changes: 3 additions & 3 deletions sw/airborne/modules/ctrl/approach_moving_target.c
View file
Expand Up @@ -54,11 +54,11 @@ Butterworth2LowPass target_heading_filt;
#include <stdio.h>

struct Amt amt = {
.distance = 60,
.speed = -1.0,
.distance = 70,
.speed = 0.0,
.pos_gain = 0.3,
.psi_ref = 180.0,
.slope_ref = 35.0,
.slope_ref = 60.0,
.speed_gain = 1.0,
.relvel_gain = 1.0,
.enabled_time = 0,
Expand Down
406 changes: 406 additions & 0 deletions sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing_v3b.c
View file

Large diffs are not rendered by default.

51 changes: 51 additions & 0 deletions sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing_v3b.h
View file
@@ -0,0 +1,51 @@
/*
* Copyright (C) 2022 D.C. van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/ctrl/ctrl_eff_sched_rot_wing_v3b.h"
* @author D.C. van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Crtl effectiveness scheduler for thr rotating wing drone V3
*/

#ifndef CTRL_EFF_SCHED_ROT_WING_V3B_H
#define CTRL_EFF_SCHED_ROT_WING_V3B_H

#include "std.h"

// Define settings
extern float lift_d_multiplier;
extern float g1_p_multiplier;
extern float g1_q_multiplier;
extern float g1_r_multiplier;
extern float g1_t_multiplier;

extern float pitch_angle_set;
extern float pitch_angle_range;

extern float rot_wing_aerodynamic_eff_const_g1_p[1];
extern float rot_wing_aerodynamic_eff_const_g1_q[1];
extern float rot_wing_aerodynamic_eff_const_g1_r[1];

extern bool wing_rotation_sched_activated;
extern bool pusher_sched_activated;

extern void init_eff_scheduling(void);
extern void event_eff_scheduling(void);

#endif // CTRL_EFF_SCHED_ROT_WING_V3_H
12 changes: 6 additions & 6 deletions sw/airborne/modules/ctrl/follow_me.c
View file
Expand Up @@ -179,18 +179,18 @@ void follow_me_set_wp(uint8_t wp_id, float speed)
static uint32_t last_relpos_tow = 0;
// Make sure to only use the current state from the receive of the GPS message (FIXME overflow at sunday)
if(last_relpos_tow < gps.relpos_tow) {
if(last_relpos_tow < gps_relposned.iTOW) {
cur_pos = *stateGetPositionNed_f();
last_relpos_tow = gps.relpos_tow;
last_relpos_tow = gps_relposned.iTOW;
}
// Set the target position
target_pos.x = cur_pos.x - gps.relpos_ned.x / 100.0f;
target_pos.y = cur_pos.y - gps.relpos_ned.y / 100.0f;
target_pos.z = cur_pos.z - gps.relpos_ned.z / 100.0f;
target_pos.x = cur_pos.x - gps_relposned.relPosN / 100.0f;
target_pos.y = cur_pos.y - gps_relposned.relPosE / 100.0f;
target_pos.z = cur_pos.z - gps_relposned.relPosD / 100.0f;
// Calculate the difference in time from measurement
diff_time_ms = gps_tow_from_sys_ticks(sys_time.nb_tick) - gps.relpos_tow + follow_me_gps_delay;
diff_time_ms = gps_tow_from_sys_ticks(sys_time.nb_tick) - gps_relposned.iTOW + follow_me_gps_delay;
if(diff_time_ms < 0) diff_time_ms += (1000*60*60*24*7); //msec of a week
}
// Check if we got a position from the ground which didn't timeout and local NED is initialized
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/ctrl/target_pos.c
View file
Expand Up @@ -143,7 +143,7 @@ bool target_get_pos(struct NedCoor_f *pos, float *heading) {
float time_diff = 0;

/* When we have a valid target_pos message, state ned is initialized and no timeout */
if(target.pos.valid && state.ned_initialized_i && (target.pos.recv_time+target.target_pos_timeout) > get_sys_time_msec()) {
if(target.pos.valid && state.ned_initialized_i && (target.pos.recv_time+target.target_pos_timeout) > get_sys_time_msec() && (target.pos.heading <= 360)) {
struct NedCoor_i target_pos_cm, drone_pos_cm;

// Convert from LLA to NED using origin from the UAV
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/datalink/mavlink.c
View file
Expand Up @@ -500,7 +500,7 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
}
mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
mav_type,
MAV_AUTOPILOT_PPZ,
MAV_AUTOPILOT_ARDUPILOTMEGA,
mav_mode,
0, // custom_mode
mav_state);
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/gps/gps.h
View file
Expand Up @@ -160,6 +160,7 @@ struct RtcmMan {

/** global GPS state */
extern struct GpsState gps;
extern struct GpsRelposNED gps_relposned;

#ifdef GPS_TYPE_H
#include GPS_TYPE_H
Expand Down
12 changes: 12 additions & 0 deletions sw/airborne/modules/imu/imu.c
View file
Expand Up @@ -164,6 +164,8 @@ static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
static uint8_t id = 0;
pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].temperature,
&imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
if(imu.accels[id].abi_id == IMU_CUBE1_ID)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
id = 0;
Expand All @@ -174,6 +176,8 @@ static void send_accel_scaled(struct transport_tx *trans, struct link_device *de
static uint8_t id = 0;
pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
&imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
if(imu.accels[id].abi_id == IMU_CUBE1_ID)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
id = 0;
Expand All @@ -186,6 +190,8 @@ static void send_accel(struct transport_tx *trans, struct link_device *dev)
ACCELS_FLOAT_OF_BFP(accel_float, imu.accels[id].scaled);
pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
&accel_float.x, &accel_float.y, &accel_float.z);
if(imu.accels[id].abi_id == IMU_CUBE1_ID)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
id = 0;
Expand All @@ -196,6 +202,8 @@ static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
static uint8_t id = 0;
pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id, &imu.gyros[id].temperature,
&imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
if(imu.gyros[id].abi_id == IMU_CUBE1_ID)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
id = 0;
Expand All @@ -206,6 +214,8 @@ static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev
static uint8_t id = 0;
pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
&imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
if(imu.gyros[id].abi_id == IMU_CUBE1_ID)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
id = 0;
Expand All @@ -218,6 +228,8 @@ static void send_gyro(struct transport_tx *trans, struct link_device *dev)
RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
&gyro_float.p, &gyro_float.q, &gyro_float.r);
if(imu.gyros[id].abi_id == IMU_CUBE1_ID)
return;
id++;
if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
id = 0;
Expand Down
1,559 changes: 1,559 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.cpp
View file

Large diffs are not rendered by default.

145 changes: 145 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.h
View file
@@ -0,0 +1,145 @@
#ifndef EKF_AW_H
#define EKF_AW_H

#ifdef __cplusplus
extern "C" {
#endif

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"

// Settings
struct ekfAwParameters {
// Q
float Q_accel; ///< accel process noise
float Q_gyro; ///< gyro process noise
float Q_mu; ///< wind process noise
float Q_k; ///< offset process noise

// R
float R_V_gnd; ///< speed measurement noise
float R_accel_filt[3]; ///< filtered accel measurement noise
float R_V_pitot; ///< airspeed measurement noise

// Model Based Parameters
float vehicle_mass;

// X Axis
float k_fx_drag[2]; // Temporary setting for fuselage + hover prop
float k_fx_fuselage[4];
float k_fx_hover[3];
float k_fx_wing[5];
float k_fx_push[3];
float k_fx_elev[3];

// Y Axis
float k_fy_beta;
float k_fy_v;
float k_fy_wing[5];

// Z Axis
float k_fz_fuselage[4];
float k_fz_wing[4];
float k_fz_hover[5];
float k_fz_elev[2];

// Other options
bool use_model[3];
bool use_pitot;
bool propagate_offset;
bool quick_convergence;
};

struct ekfHealth{
bool healthy;
uint16_t crashes_n;
};

extern struct ekfAwParameters ekf_aw_params;

// Init functions
extern void ekf_aw_init(void);
extern void ekf_aw_reset(void);

// Filtering functions
extern void ekf_aw_propagate(struct FloatVect3 *acc,struct FloatRates *gyro, struct FloatEulers *euler, float *pusher_RPM,float *hover_RPM_array, float *skew, float *elevator_angle, struct FloatVect3 * V_gnd, struct FloatVect3 *acc_filt, float *V_pitot,float dt);

// Getter/Setter functions
extern struct NedCoor_f ekf_aw_get_speed_body(void);
extern struct NedCoor_f ekf_aw_get_wind_ned(void);
extern struct NedCoor_f ekf_aw_get_offset(void);
extern struct FloatVect3 ekf_aw_get_innov_V_gnd(void);
extern struct FloatVect3 ekf_aw_get_innov_accel_filt(void);
extern float ekf_aw_get_innov_V_pitot(void);
extern void ekf_aw_get_meas_cov(float meas_cov[7]);
extern void ekf_aw_get_state_cov(float state_cov[9]);
extern void ekf_aw_get_process_cov(float process_cov[9]);
extern void ekf_aw_get_fuselage_force(float force[3]);
extern void ekf_aw_get_wing_force(float force[3]);
extern void ekf_aw_get_elevator_force(float force[3]);
extern void ekf_aw_get_hover_force(float force[3]);
extern void ekf_aw_get_pusher_force(float force[3]);
extern struct ekfAwParameters *ekf_aw_get_param_handle(void);

extern void ekf_aw_set_speed_body(struct NedCoor_f *s);
extern void ekf_aw_set_wind(struct NedCoor_f *s);
extern void ekf_aw_set_offset(struct NedCoor_f *s);
extern struct ekfHealth ekf_aw_get_health(void);

// Settings handlers
extern void ekf_aw_update_params(void);
extern void ekf_aw_reset_health(void);


// Handlers
#define ekf_aw_update_Q_accel(_v) { \
ekf_aw_params.Q_accel = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_gyro(_v) { \
ekf_aw_params.Q_gyro = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_mu(_v) { \
ekf_aw_params.Q_mu = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_k(_v) { \
ekf_aw_params.Q_k = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_gnd(_v) { \
ekf_aw_params.R_V_gnd = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_x(_v) { \
ekf_aw_params.R_accel_filt[0] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_y(_v) { \
ekf_aw_params.R_accel_filt[1] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_z(_v) { \
ekf_aw_params.R_accel_filt[2] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_pitot(_v) { \
ekf_aw_params.R_V_pitot = _v; \
ekf_aw_update_params(); \
}

#ifdef __cplusplus
}
#endif

#endif /* EKF_AW_H */
477 changes: 477 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.c
View file

Large diffs are not rendered by default.

117 changes: 117 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.h
View file
@@ -0,0 +1,117 @@
#ifndef EKF_AW_WRAPPER_H
#define EKF_AW_WRAPPER_H

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "modules/meteo/ekf_aw.h"


// EKF structure
struct ekfAw {

// States
struct NedCoor_f wind;
struct NedCoor_f V_body;
struct NedCoor_f offset;

// Inputs
struct FloatVect3 acc; ///< Last accelerometer measurements
struct FloatRates gyro; ///< Last gyroscope measurements
struct FloatEulers euler; /// Euler angles

int32_t last_RPM_hover[4]; // Value obtained from ABI Callback
int32_t last_RPM_pusher; // Value obtained from ABI Callback
float RPM_hover[4]; /// Hover motor RPM
float RPM_pusher; /// Pusher motor RPM
float skew; /// Skew
float elevator_angle;


// Measurements
struct FloatVect3 Vg_NED; /// Ground Speed
struct FloatVect3 acc_filt; ///< Last accelerometer measurements
float V_pitot; /// Pitot tube airspeed

// Innovation
struct FloatVect3 innov_V_gnd;
struct FloatVect3 innov_acc_filt;
float innov_V_pitot;

// Covariance
float meas_cov[7];
float state_cov[9];
float process_cov[12];

// Forces
float fuselage_force[3];
float wing_force[3];
float elevator_force[3];
float hover_force[3];
float pusher_force[3];

// Other
bool reset;
bool in_air;
struct NedCoor_f wind_guess;
struct NedCoor_f offset_guess;
struct ekfHealth health;
uint64_t internal_clock;
uint64_t time_last_on_gnd;
uint64_t time_last_in_air;
bool override_start;
bool override_quick_convergence;

};

extern void ekf_aw_wrapper_init(void);
extern void ekf_aw_wrapper_periodic(void);
extern void ekf_aw_wrapper_fetch(void);

extern void set_in_air_status(bool);


extern float tau_filter_high;
extern float tau_filter_low;

extern struct ekfAw ekf_aw;

// Handlers
#define ekf_aw_wrapper_reset(_v) { \
ekf_aw.reset = false; \
ekf_aw_reset(); \
ekf_aw_reset_health(); \
}

#define ekf_aw_wrapper_set_wind_N(_v) { \
ekf_aw.wind_guess.x = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_E(_v) { \
ekf_aw.wind_guess.y = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_D(_v) { \
ekf_aw.wind_guess.z = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_offset_x(_v) { \
ekf_aw.offset_guess.x = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_y(_v) { \
ekf_aw.offset_guess.y = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_z(_v) { \
ekf_aw.offset_guess.z = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#endif /* EKF_AW_WRAPPER_H */
92 changes: 92 additions & 0 deletions sw/airborne/modules/nav/ground_detect_ship.c
View file
@@ -0,0 +1,92 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/nav/ground_detect_ship.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Ground detection module relying on lidar to detect ground on a ship
*/

#include "modules/nav/ground_detect_ship.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "filters/low_pass_filter.h"
#include "modules/sonar/agl_dist.h"
#include "state.h"

#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"

int32_t counter = 0;
bool ground_detection_ship = false;
bool ground_detected = false;
float agl_vert_speed = 0.0;
bool agl_vert_speed_valid = false;
float agl_distance = 0.0;

#define GROUND_DETECT_SHIP_AGL_MIN_VALUE 0.1
#define GROUND_DETECT_SHIP_AGL_MAX_VALUE 0.3

#define GROUND_DETECT_SHIP_COUNTER_TRIGGER 10

// Cutoff frequency of the vertical speed calculated from the lidar
#define GROUND_DETECT_SHIP_VERT_SPEED_FILT_FREQ 5.0

Butterworth2LowPass vert_speed_filter;

void ground_detect_ship_init(void)
{
// init filters
float tau_vert_speed = 1.0 / (2.0 * M_PI * GROUND_DETECT_SHIP_VERT_SPEED_FILT_FREQ);
float sample_time = 1.0 / PERIODIC_FREQUENCY;
init_butterworth_2_low_pass(&vert_speed_filter, tau_vert_speed, sample_time, 0.0);
}

bool ground_detect(void) {
return ground_detected;
}

void ground_detect_periodic(void)
{
// Evaluate thrust given (less than hover thrust)
// Use the control effectiveness in thrust in order to estimate the thrust delivered (only works for multicopters)
float specific_thrust = 0.0; // m/s2
uint8_t i;
for (i = 0; i < INDI_NUM_ACT; i++) {
specific_thrust += actuator_state_filt_vect[i] * g1g2[3][i] * -((int32_t) act_is_servo[i] - 1);
}

if (specific_thrust > -5.) {
counter += 1;
if (counter > GROUND_DETECT_SHIP_COUNTER_TRIGGER) {
ground_detected = true;
}
} else {
ground_detected = false;
counter = 0;
}

#ifdef DEBUG_GROUND_DETECT
uint8_t test_gd = ground_detected;
float payload[2];
payload[0] = specific_thrust;
payload[1] = stateGetAccelNed_f()->z;

RunOnceEvery(10, {DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, &test_gd); DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 4, payload);} );
#endif
}
32 changes: 32 additions & 0 deletions sw/airborne/modules/nav/ground_detect_ship.h
View file
@@ -0,0 +1,32 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/nav/ground_detect_ship.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Ground detection module relying on lidar to detect ground on a ship
*/

#ifndef GROUND_DETECT_SHIP_H
#define GROUND_DETECT_SHIP_H

extern void ground_detect_ship_init(void);
extern void ground_detect_periodic(void);

#endif // GROUND_DETECT_SHIP_H
34 changes: 30 additions & 4 deletions sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
View file
Expand Up @@ -25,7 +25,7 @@

#include "modules/nav/nav_rotorcraft_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" // strong dependency for now
#include "firmwares/rotorcraft/guidance/guidance_indi_rot_wing.h" // strong dependency for now
#include "math/pprz_isa.h"

// Max ground speed that will be commanded
Expand All @@ -34,6 +34,8 @@
#endif
#define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)
float nav_max_speed = NAV_MAX_SPEED;
float nav_max_acceleration_sp = 6.0;
float nav_max_deceleration_sp = 0.5;

#ifndef MAX_DECELERATION
#define MAX_DECELERATION 1.f
Expand Down Expand Up @@ -76,7 +78,7 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
// Calculate distance to waypoint
float dist_to_wp = float_vect2_norm(&pos_error);
// Calculate max speed to decelerate from
float max_speed_decel2 = fabsf(2.f * dist_to_wp * MAX_DECELERATION); // dist_to_wp can only be positive, but just in case
float max_speed_decel2 = fabsf(2.f * dist_to_wp * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
float max_speed_decel = sqrtf(max_speed_decel2);
// Bound the setpoint velocity vector
float max_h_speed = Min(nav_max_speed, max_speed_decel);
Expand All @@ -102,6 +104,14 @@ static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_en
} else {
desired_speed = dist_to_target * gih_params.pos_gain;
Bound(desired_speed, 0.0f, nav_max_speed);

// Calculate max speed to decelerate from
float max_speed_decel2 = fabsf(2.f * dist_to_target * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
float max_speed_decel = sqrtf(max_speed_decel2);
// Bound the setpoint velocity vector
if (desired_speed > max_speed_decel) {
Bound(desired_speed, 0.0f, max_speed_decel);
}
}

// Calculate length of line segment
Expand All @@ -126,7 +136,10 @@ static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_en
VECT2_ADD(direction, v_to_line);
float length_direction = Max(float_vect2_norm(&direction), 0.01f);
// Scale to have the desired speed
VECT2_SMUL(nav.speed, direction, desired_speed / length_direction);
struct FloatVect2 speed_sp;

VECT2_SMUL(speed_sp, direction, desired_speed / length_direction);

// final target position, should be on the line, for display
VECT2_SUM(nav.target, *stateGetPositionEnu_f(), direction);

Expand Down Expand Up @@ -227,7 +240,20 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
struct FloatVect2 speed_unit;
VECT2_DIFF(speed_unit, nav.target, *stateGetPositionEnu_f());
float_vect2_normalize(&speed_unit);
VECT2_SMUL(nav.speed, speed_unit, desired_speed);

struct FloatVect2 speed_sp;

VECT2_SMUL(speed_sp, speed_unit, desired_speed);

// Check for acceleration bound
float abs_nav_speed = sqrtf(nav.speed.x * nav.speed.x + nav.speed.y * nav.speed.y);
float abs_speed_sp = sqrtf(speed_sp.x * speed_sp.x + speed_sp.y * speed_sp.y);
if (abs_speed_sp > abs_nav_speed) {
float bound_speed = abs_nav_speed + nav_max_acceleration_sp / NAVIGATION_FREQUENCY;
float_vect2_bound_in_2d(&speed_sp, bound_speed);
}

VECT2_COPY(nav.speed, speed_sp);

nav_rotorcraft_base.circle.center = *wp_center;
nav_rotorcraft_base.circle.radius = radius;
Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/modules/nav/nav_rotorcraft_hybrid.h
View file
Expand Up @@ -33,6 +33,8 @@

// settings
extern float nav_max_speed;
extern float nav_max_acceleration_sp;
extern float nav_max_deceleration_sp;

extern void nav_rotorcraft_hybrid_init(void);

Expand Down
150 changes: 150 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rot_wing_automation.c
View file
@@ -0,0 +1,150 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/rot_wing_drone/rot_wing_automation.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Fucntions to automate the navigation and guidance of the rotating wing drone
*/

#include "modules/rot_wing_drone/rot_wing_automation.h"
#include "state.h"
#include "modules/datalink/telemetry.h"

#ifndef ROT_WING_AUTOMATION_TRANS_ACCEL
#define ROT_WING_AUTOMATION_TRANS_ACCEL 1.0
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_DECEL
#define ROT_WING_AUTOMATION_TRANS_DECEL 0.5
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_LENGTH
#define ROT_WING_AUTOMATION_TRANS_LENGTH 200.0
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_AIRSPEED
#define ROT_WING_AUTOMATION_TRANS_AIRSPEED 15.0
#endif

struct rot_wing_automation rot_wing_a;

// declare function
inline void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 * target_ned);

void init_rot_wing_automation(void)
{
rot_wing_a.trans_accel = ROT_WING_AUTOMATION_TRANS_ACCEL;
rot_wing_a.trans_decel = ROT_WING_AUTOMATION_TRANS_DECEL;
rot_wing_a.trans_length = ROT_WING_AUTOMATION_TRANS_LENGTH;
rot_wing_a.trans_airspeed = ROT_WING_AUTOMATION_TRANS_AIRSPEED;
rot_wing_a.transitioned = false;
}

// periodic function
void periodic_rot_wing_automation(void)
{
float airspeed = stateGetAirspeed_f();
if (airspeed > rot_wing_a.trans_airspeed)
{
rot_wing_a.transitioned = true;
} else {
rot_wing_a.transitioned = false;
}
}

// Update a waypoint such that you can see on the GCS where the drone wants to go
void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 * target_ned) {

// Update the waypoint
struct EnuCoor_f target_enu;
ENU_OF_TO_NED(target_enu, *target_ned);
waypoint_set_enu(wp_id, &target_enu);

// Send waypoint update every half second
RunOnceEvery(100/2, {
// Send to the GCS that the waypoint has been moved
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
} );
}

// Function to visualize from flightplan, call repeadely
void rot_wing_vis_transition(uint8_t wp_transition_id, uint8_t wp_decel_id, uint8_t wp_end_id)
{
//state eulers in zxy order
struct FloatEulers eulers_zxy;

// get heading and cos and sin of heading
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
float psi = eulers_zxy.psi;
float cpsi = cosf(psi);
float spsi = sinf(psi);

// get airspeed
float airspeed = stateGetAirspeed_f();
// get groundspeed
float ground_speed = stateGetHorizontalSpeedNorm_f();

// get drone position
struct NedCoor_f *drone_pos = stateGetPositionNed_f();

// Move reference waypoints
// Move end transition waypoint at the correct length
struct FloatVect3 end_transition_rel_pos;
VECT3_COPY(end_transition_rel_pos, *drone_pos);
end_transition_rel_pos.x = cpsi * rot_wing_a.trans_length;
end_transition_rel_pos.y = spsi * rot_wing_a.trans_length;
struct FloatVect3 end_transition_pos;
VECT3_SUM(end_transition_pos, end_transition_rel_pos, *drone_pos);
end_transition_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_end_id, &end_transition_pos);

// Move transition waypoint
float airspeed_error = rot_wing_a.trans_airspeed - airspeed;
float transition_time = airspeed_error / rot_wing_a.trans_accel;
float average_ground_speed = ground_speed + airspeed_error / 2.;
float transition_distance = average_ground_speed * transition_time;

struct FloatVect3 transition_rel_pos;
VECT3_COPY(transition_rel_pos, *drone_pos);
transition_rel_pos.x = cpsi * transition_distance;
transition_rel_pos.y = spsi * transition_distance;
struct FloatVect3 transition_pos;
VECT3_SUM(transition_pos, transition_rel_pos, *drone_pos);
transition_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_transition_id, &transition_pos);

// Move decel point
float final_groundspeed = ground_speed + airspeed_error;
float decel_time = final_groundspeed / rot_wing_a.trans_decel;
float decel_distance = (final_groundspeed / 2.) * decel_time;
float decel_distance_from_drone = rot_wing_a.trans_length - decel_distance;

struct FloatVect3 decel_rel_pos;
VECT3_COPY(decel_rel_pos, *drone_pos);
decel_rel_pos.x = cpsi * decel_distance_from_drone;
decel_rel_pos.y = spsi * decel_distance_from_drone;
struct FloatVect3 decel_pos;
VECT3_SUM(decel_pos, decel_rel_pos, *drone_pos);
decel_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_decel_id, &decel_pos);
}
46 changes: 46 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rot_wing_automation.h
View file
@@ -0,0 +1,46 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

/** @file "modules/rot_wing_drone/rot_wing_automation.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Fucntions to automate the navigation and guidance of the rotating wing drone
*/

#ifndef ROT_WING_AUTOMATION_H
#define ROT_WING_AUTOMATION_H

#include "std.h"
#include "math/pprz_algebra_float.h"

extern void init_rot_wing_automation(void);
extern void periodic_rot_wing_automation(void);
extern void rot_wing_vis_transition(uint8_t wp_transition_id, uint8_t wp_decel_id, uint8_t wp_end_id);

struct rot_wing_automation {
float trans_accel;
float trans_decel;
float trans_length;
float trans_airspeed;
bool transitioned;
};

extern struct rot_wing_automation rot_wing_a;

#endif // ROT_WING_AUTOMATION_H