Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Oneloop Controller: some updates #3261

Open
wants to merge 15 commits into
base: master
Choose a base branch
from
Open

Oneloop Controller: some updates #3261

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
8c5d566
initial commit
tmldeponti Apr 8, 2024
46572ed
merge airframes
tmldeponti Apr 8, 2024
cee2326
Delete unused airframe
tmldeponti Apr 8, 2024
206702f
code clean up
tmldeponti Apr 8, 2024
fefb226
removed unused functions and some extra clean up
tmldeponti Apr 9, 2024
9ef5660
Removed other unused functions
tmldeponti Apr 9, 2024
4bb59da
Leave out rotwing state fr now, will be addressed in the next PR
tmldeponti Apr 11, 2024
b5a4d99
Removed ins ext pose quaternion messages from oneloop controller
tmldeponti Apr 11, 2024
7b626f0
Conf for RW3C
tmldeponti Apr 11, 2024
d7df024
Added NAV_HYBRID_GOTO_MODE as suggested
tmldeponti Apr 11, 2024
fe4bfaf
Use aux 5 to set preferred pitch for the rotating wing drone
tmldeponti Apr 11, 2024
d35ee27
Merge remote-tracking branch 'upstream/master' into oneloop_update_PR
tmldeponti Apr 11, 2024
01a5a02
adjusted name
tmldeponti Apr 11, 2024
f93fb8b
removed printf
tmldeponti Apr 11, 2024
2ab3add
Use moment defnition in JSBSIM as suggested
tmldeponti Apr 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
314 changes: 314 additions & 0 deletions conf/airframes/tudelft/rot_wing_v3c_oneloop.xml
View file
Open in desktop

Large diffs are not rendered by default.

315 changes: 315 additions & 0 deletions conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
View file
Open in desktop

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion conf/autopilot/rotorcraft_oneloop.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@
<mode name="MODULE" shortname="ONE">
<select cond="RCMode1() && DLModeModule()" exception="HOME"/>
<on_enter>
<call fun="oneloop_andi_enter(false)"/>
<call fun="oneloop_andi_enter(false, CTRL_ANDI)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
Expand Down
191 changes: 191 additions & 0 deletions conf/autopilot/rotorcraft_oneloop_andi_indi.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,191 @@
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">

<autopilot name="Oneloop Autopilot Rotorcraft">

<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">

<includes>
<include name="generated/airframe.h"/>
<include name="autopilot.h"/>
<include name="autopilot_rc_helpers.h"/>
<include name="navigation.h"/>
<include name="guidance.h"/>
<include name="oneloop/oneloop_andi.h"/>
<include name="stabilization/stabilization_attitude.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT" cond="ifndef MODE_MANUAL"/>
<define name="MODE_ONELOOP" value="AP_MODE_MODULE" cond="ifndef MODE_AUTO1"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
<define name="DLModeNav()" value="(autopilot_mode_auto2 == AP_MODE_NAV)"/>
<define name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
<define name="DLModeModule()" value="(MODE_AUTO1 == AP_MODE_MODULE)"/>
<define name="DLModeAZH()" value="(MODE_AUTO1 == AP_MODE_ATTITUDE_Z_HOLD)"/>
<define name="ONELOOP_CONTROLLER" value="TRUE"/>
</includes>

<settings>
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
<dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
</settings>

<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</control_block>

<control_block name="attitude_loop">
<call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/>
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
</control_block>

<control_block name="throttle_direct">
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_set_z(stateGetPositionNed_i()->z)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
</control_block>

<control_block name="altitude_loop">
<call fun="gv_update_ref_from_z_sp(guidance_v.z_sp)"/>
<call fun="guidance_v.delta_t = guidance_v_run_pos(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
</control_block>

<exceptions>
<exception cond="nav.too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions>

<mode name="ATTITUDE_DIRECT" shortname="ATT">
<select cond="RCMode0()"/>
<on_enter>
<call fun="stabilization_attitude_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="attitude_loop"/>
<call_block name="throttle_direct"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>

<mode name="NAV">
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
<on_enter>
<call fun="oneloop_andi_enter(false, CTRL_INDI)"/>
</on_enter>
<control>
<call fun="nav_periodic_task()"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>

<mode name="GUIDED">
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>

<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
<select cond="RCMode1() && DLModeAZH()"/>
<on_enter>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>

<mode name="MODULE" shortname="ONE">
<select cond="RCMode1() && DLModeModule()" exception="HOME"/>
<on_enter>
<call fun="oneloop_andi_enter(false, CTRL_ANDI)"/>
</on_enter>
<control>
<call fun="nav_periodic_task()"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>

<mode name="HOME">
<on_enter>
<call fun="guidance_h_nav_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>

<!-- Safe landing -->
<mode name="FAILSAFE" shortname="FAIL">
<!-- Failsafe does not work needs to be fixed-->
<on_enter>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
</on_enter>
<control>
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="guidance_v_update_ref()"/>
<call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
</mode>

<!-- Kill throttle mode -->
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>

</state_machine>

</autopilot>
14 changes: 9 additions & 5 deletions conf/autopilot/rotorcraft_oneloop_with_backup.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
<define name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
<define name="DLModeModule()" value="(MODE_AUTO1 == AP_MODE_MODULE)"/>
<define name="DLModeAZH()" value="(MODE_AUTO1 == AP_MODE_ATTITUDE_Z_HOLD)"/>
<define name="ONELOOP_CONTROLLER" value="TRUE"/>
</includes>

<settings>
Expand Down Expand Up @@ -79,10 +80,11 @@
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v_z_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<!-- <control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
</control> -->
<control>
<call fun="nav_periodic_task()"/>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
Expand Down Expand Up @@ -137,12 +139,14 @@
<mode name="MODULE" shortname="ONE">
<select cond="RCMode1() && DLModeModule()" exception="HOME"/>
<on_enter>
<call fun="oneloop_andi_enter(false)"/>
<call fun="oneloop_andi_enter(false, CTRL_ANDI)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<!-- <control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
</control> -->
<control>
<call fun="nav_periodic_task()"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
Expand Down
10 changes: 5 additions & 5 deletions conf/flight_plans/tudelft/oneloop_valkenburg.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="8.0" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="5000" name="Oneloop Valkenburg" security_height="2">
<flight_plan alt="70.0" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="1070" name="Oneloop Valkenburg" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
Expand All @@ -9,15 +9,15 @@
#include "modules/ahrs/ahrs.h"
</header>
<waypoints>
<waypoint name="HOME" lat="52.1682196" lon="4.4134865"/>
<waypoint name="HOME" x="12.6" y="-48.7"/>
<waypoint name="CLIMB" x="62.6" y="-80.4"/>
<waypoint name="STDBY" x="106.0" y="-55.1"/>
<waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/>
<waypoint name="p1" x="322.0" y="-254.3"/>
<waypoint name="p2" x="696.0" y="-148.1"/>
<waypoint name="p3" x="600.4" y="73.4"/>
<waypoint name="p4" x="238.6" y="-48.5"/>
<waypoint name="TD" x="-4.1" y="-23.7"/>
<waypoint name="FOLLOW" x="123.2" y="-87.8"/>
<waypoint name="TD" lat="52.1681602" lon="4.4127708"/>
<waypoint name="FOLLOW" lat="52.16850964562752" lon="4.413635008734417"/>
<waypoint name="S1" lat="52.1669450" lon="4.4174372"/>
<waypoint name="S2" lat="52.1689871" lon="4.4208804"/>
<waypoint lat="52.169189" lon="4.410820" name="C1"/>
Expand Down
1 change: 1 addition & 0 deletions conf/modules/ins_ext_pose.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@

<makefile target="ap">
<define name="INS_TYPE_H" value="modules/ins/ins_ext_pose.h" type="string"/>
<define name="INS_EXT_POSE" value="TRUE"/>
<file name="ins_ext_pose.c"/>
<file name="ins.c"/>
</makefile>
Expand Down
37 changes: 28 additions & 9 deletions conf/modules/oneloop_andi.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,34 @@
<settings>
<dl_settings>
<dl_settings NAME="oneloop">
<dl_setting var="p_att_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_att_e_omega_n"/>
<dl_setting var="p_att_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_att_e_zeta"/>
<dl_setting var="p_head_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_head_e_omega_n"/>
<dl_setting var="p_head_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_head_e_zeta"/>
<dl_setting var="p_pos_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_pos_e_omega_n"/>
<dl_setting var="p_pos_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_pos_e_zeta"/>
<dl_setting var="p_alt_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_alt_e_omega_n"/>
<dl_setting var="p_alt_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_alt_e_zeta"/>
<dl_setting var="psi_des_deg" min="-180.0" step="0.1" max="180.0" shortname="psi_des"/>
<dl_setting var="chirp_on" min="0" step="1" max="1" values="OFF|ON" shortname="chirp_on"/>
<dl_setting var="chirp_axis" min="0" step="1" max="3" shortname="chirp_axis"/>
<dl_setting var="f0_chirp" min="0.01" step="0.001" max="10" shortname="f0_chirp"/>
<dl_setting var="f1_chirp" min="0.01" step="0.001" max="10" shortname="f1_chirp"/>
<dl_setting var="t_chirp" min="0.01" step="0.001" max="60" shortname="t_chirp"/>
<dl_setting var="A_chirp" min="0.01" step="0.001" max="10" shortname="A_chirp"/>
<dl_setting var="heading_manual" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/>
<dl_setting var="yaw_stick_in_auto" min="0" step="1" max="1" values="OFF|ON" shortname="yaw_stick_on"/>
<dl_setting var="fwd_sideslip_gain" min="0.1" step="0.001" max="20.0" shortname="fwd_sideslip_gain"/>
<dl_setting var="psi_des_deg" min="-180.0" step="0.1" max="180.0" shortname="psi_des"/>
<dl_setting var="p_att_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_att_e_omega_n"/>
<dl_setting var="p_att_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_att_e_zeta"/>
<dl_setting var="p_att_rm.omega_n" min="0.1" step="0.001" max="70.0" shortname="p_att_rm_omega_n"/>
<dl_setting var="p_att_rm.zeta" min="0.1" step="0.001" max="1.0" shortname="p_att_rm_zeta"/>
<dl_setting var="p_att_rm.p3" min="0.1" step="0.001" max="70.0" shortname="p_att_rm_p3"/>
<dl_setting var="p_head_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_head_e_omega_n"/>
<dl_setting var="p_head_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_head_e_zeta"/>
<dl_setting var="p_head_rm.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_head_rm_omega_n"/>
<dl_setting var="p_head_rm.zeta" min="0.1" step="0.001" max="1.0" shortname="p_head_rm_zeta"/>
<dl_setting var="p_pos_e.omega_n" min="0.1" step="0.001" max="70.0" shortname="p_pos_e_omega_n"/>
<dl_setting var="p_pos_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_pos_e_zeta"/>
<dl_setting var="p_pos_e.p3" min="0.1" step="0.001" max="70.0" shortname="p_pos_e_p3"/>
<dl_setting var="p_pos_rm.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_pos_rm_omega_n"/>
<dl_setting var="p_pos_rm.zeta" min="0.1" step="0.001" max="1.0" shortname="p_pos_rm_zeta"/>
<dl_setting var="p_alt_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_alt_e_omega_n"/>
<dl_setting var="p_alt_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_alt_e_zeta"/>
<dl_setting var="p_alt_rm.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_alt_rm_omega_n"/>
<dl_setting var="p_alt_rm.zeta" min="0.1" step="0.001" max="1.0" shortname="p_alt_rm_zeta"/>
</dl_settings>
</dl_settings>
</settings>
Expand Down