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

<airframe name="bebop_oneloop_ekf2">
<description> bebop with a oneloop ANDI controller and EKF2
</description>

<firmware name="rotorcraft">
<autopilot name="rotorcraft_oneloop.xml"/>
<target name="ap" board="bebop">
<define name="RADIO_TH_HOLD" value="0"/>
</target>

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

<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>

<!-- <module name="gps" type="furuno"/> -->
<module name="gps" type="datalink"/>

<module name="stabilization" type="oneloop" >
<configure name="INDI_NUM_ACT" value="4"/>
</module>

<module name="ins" type="ekf2">
<define name="INS_EKF2_OPTITRACK" value="true"/>
</module>

<module name="guidance" type="oneloop"/>
<module name="nav_hybrid"/>

<module name="oneloop" type="andi">
<configure name="ANDI_NUM_ACT" value="4"/>
<configure name="ANDI_NUM_VIRTUAL_ACT" value="2"/>
<configure name="ANDI_OUTPUTS" value="6"/>
</module>

<module name="geo_mag"/>
<module name="air_data"/>

<module name="wls">
<define name="WLS_N_U" value = "6"/>
<define name="WLS_N_V" value = "6"/>
</module>

<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/"/>
</module>
</firmware>

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

<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="3000" neutral="3000" max="12000"/>
<servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="12000"/>
<servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="12000"/>
<servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="12000"/>
</servos>

<command_laws>
<let VAR="th_hold" value="!autopilot_get_motors_on()"/>
<set value="($th_hold? -9600 : actuators_pprz[0])" SERVO="TOP_LEFT"/>
<set value="($th_hold? -9600 : actuators_pprz[1])" SERVO="TOP_RIGHT"/>
<set value="($th_hold? -9600 : actuators_pprz[2])" SERVO="BOTTOM_RIGHT"/>
<set value="($th_hold? -9600 : actuators_pprz[3])" SERVO="BOTTOM_LEFT"/>
</command_laws>

<section name="MISC">
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value = "300"/>
<define name="THRESHOLD_GROUND_DETECT" value = "40"/>
<define name="AUTOPILOT_ARMING_DELAY" value = "2"/>
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value = "5.0f"/> <!--remove once hybrid nav is made not dependent on guidance indi hybrid-->
<define name="GUIDANCE_INDI_NAV_SPEED_MARGIN" value = "0.0"/> <!--remove once hybrid nav is made not dependent on guidance indi hybrid-->
<define NAME="STABILIZATION_ATTITUDE_SP_MAX_PHI" value = "45." UNIT="deg" />
<define NAME="STABILIZATION_ATTITUDE_SP_MAX_THETA" value = "45." UNIT="deg" />
<define NAME="STABILIZATION_ATTITUDE_SP_MAX_R" value = "90." UNIT="deg/s" />
<define NAME="STABILIZATION_ATTITUDE_DEADBAND_R" value = "200" />
</section>

<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>

<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<define name="ACCEL_CALIB" value="{{.abi_id=1, .calibrated={.neutral=true, .scale=true},.neutral={171,25,-326}, .scale={{22001,54059,59579},{8998,22092,24933}}}}"/>
<define name="IMU_MAG_CALIB" value="{{.abi_id=1, .calibrated={.neutral=true, .scale=true},.neutral={-9,8,121}, .scale={{1143,47395,48383},{152,5978,5811}}}}"/>
</section>

<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
</section>

<section PREFIX="ONELOOP_ANDI_" name="ONELOOP_ANDI">
<define name = "G1_ROLL" value = "{20.0f , -20.0f, -20.0f, 20.0f , 0.0f, 0.0f }"/>
<define name = "G1_PITCH" value = "{10.0f , 10.0f , -10.0f, -10.0f, 0.0f, 0.0f }"/>
<define name = "G1_YAW" value = "{-1.0f , 1.0f , -1.0f , 1.0f , 0.0f, 0.0f }"/>
<define name = "G1_THRUST" value = "{-.4f , -.4f , -.4f , -.4f , 0.0f, 0.0f }"/>
<define name = "G1_ZERO" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }"/>
<define name = "G2" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
<define name = "MAX_R" value = "120.0" unit="deg/s"/>
<define name = "FILT_CUTOFF" value = "1.0" />
<define name = "FILT_CUTOFF_ACC" value = "1.0" />
<define name = "FILT_CUTOFF_VEL" value = "10.0"/>
<define name = "FILT_CUTOFF_POS" value = "10.0"/>
<define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" />
<define name = "FILT_CUTOFF_P" value = "5.0" />
<define name = "FILT_CUTOFF_Q" value = "5.0" />
<define name = "FILT_CUTOFF_R" value = "5.0" />
<define name = "FILT_CUTOFF_RDOT" value = "0.5" />
<define name = "FILTER_YAW_RATE" value = "TRUE"/>
<define name = "ACT_DYN" value = "{50.0f, 50.0f, 50.0f, 50.0f, 0.0f,0.0f}" />
<define name = "ACT_IS_SERVO" value = "{0, 0, 0, 0, 0, 0}" />
<define name = "ACT_MAX" value = "{9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/>
<define name = "ACT_MIN" value = "{0.0f , 0.0f, 0.0f, 0.0f, -M_PI_4, -M_PI_4}"/>
<define name = "ACT_MAX_NORM" value = "{1.0f , 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/>
<define name = "ACT_MIN_NORM" value = "{0.0f , 0.0f, 0.0f, 0.0f, -1.0f, -1.0f}"/>
<define name = "DEBUG_MODE" value = "FALSE"/>
<define name = "AC_HAS_PUSHER" value = "FALSE"/>
<define name = "WV" value = "{1.0f, 1.0f, 1.0f, 1000.0f, 1000.0f, 100.0f}"/>
<define name = "WU" value = "{3.0f, 3.0f, 3.0f, 3.0f, 2.0f, 2.0f}"/>
<define name = "U_PREF" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
<define name = "MAX_AIRSPEED" value = "5.0f"/>
</section>

<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="2.5"/>
<define name="DESCEND_VSPEED" value="-1.0"/>
</section>

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="bebop" type="string"/>
<define name="COMMANDS_NB" value="4"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
</section>

<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>

<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="11.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>

</airframe>
177 changes: 177 additions & 0 deletions conf/autopilot/rotorcraft_oneloop.xml
View file
@@ -0,0 +1,177 @@
<!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)"/>
</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>

<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()"/>
<select cond="$DEFAULT_MODE"/>
<on_enter>
<call fun="stabilization_attitude_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>

<mode name="NAV">
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_nav_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_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" 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)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<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>
198 changes: 198 additions & 0 deletions conf/autopilot/rotorcraft_oneloop_with_backup.xml
View file
@@ -0,0 +1,198 @@
<!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)"/>
</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="guidance_h_nav_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v_z_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_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" 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)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<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>
135 changes: 135 additions & 0 deletions conf/flight_plans/tudelft/oneloop_cyberzoo.xml
View file
@@ -0,0 +1,135 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Safe TU Delft Cyberzoo" security_height="0.4">
<header>
#include "modules/datalink/datalink.h"
#include "modules/energy/electrical.h"
#include "modules/radio_control/radio_control.h"
#include "modules/ahrs/ahrs.h"
<!-- #include "sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h" -->
</header>
<waypoints>
<waypoint lat="51.990631" lon="4.376796" name="HOME"/>
<waypoint name="CLIMB" x="1.9" y="1.0"/>
<waypoint name="STDBY" x="0.5" y="0.3"/>
<waypoint name="p5" x="0.2" y="0.3"/>
<waypoint name="TD" x="0.8" y="-1.7"/>
<waypoint lat="51.9905979" lon="4.3767279" name="CZ1"/>
<waypoint lat="51.9906685" lon="4.3767327" name="CZ2"/>
<waypoint lat="51.9906652" lon="4.3768481" name="CZ3"/>
<waypoint lat="51.9905941" lon="4.3768441" name="CZ4"/>
<waypoint lat="51.9906028" lon="4.3767375" name="p1"/>
<waypoint lat="51.9906633" lon="4.3767411" name="p2"/>
<waypoint lat="51.9906601" lon="4.3768415" name="p3"/>
<waypoint lat="51.9905979" lon="4.3768386" name="p4"/>
</waypoints>
<sectors>
<sector color="blue" name="CyberZoo">
<corner name="CZ1"/>
<corner name="CZ2"/>
<corner name="CZ3"/>
<corner name="CZ4"/>
</sector>
<sector color="red" name="Flyzone">
<corner name="p1"/>
<corner name="p2"/>
<corner name="p3"/>
<corner name="p4"/>
</sector>
</sectors>
<exceptions>
<!--Soft Geofencing (go back to Standby)-->
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT 20.0) @AND
!(nav_block == IndexOfBlock('Wait GPS')) @AND
!(nav_block == IndexOfBlock('Geo init'))" deroute="safe"/>
<!-- Hard Geofencing (Kill) -->
<exception cond="(Or(!InsideCyberZoo(GetPosX(), GetPosY()), GetPosAlt() @GT 80.0) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- RC lost -->
<exception cond="((radio_control.status == RC_REALLY_LOST) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- Datalink lost -->
<exception cond="((datalink_time @GT 5) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- Bat low (constant RPM descent)-->
<exception cond="(electrical.bat_low @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Bat critical (constant RPM no stabilization)-->
<exception cond="(electrical.bat_critical @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 2)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block key="r" name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<call_once fun="NavResurrect()"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosAlt() @GT 0.8" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
</block>
<block name="go_p5">
<stay wp="p5"/>
</block>
<block name="safe">
<stay wp="p5"/>
<exception cond="NavBlockTime() @GT 5" deroute="go_p5"/>
</block>
<block key="l" name="land here" strip_button="land here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
<go wp="TD"/>
<deroute block="Flare"/>
</block>
<block name="Land">
<go wp="TD"/>
<deroute block="Flare"/>
</block>
<block name="Flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Landed"/>
<exception cond="0.10 @GT GetPosAlt()" deroute="Landed"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Safe landing">
<exception cond="!nav_is_in_flight()" deroute="Manual"/>
<exception cond="0.10 @GT GetPosAlt()" deroute="Manual"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Manual">
<set value="AP_MODE_ATTITUDE_DIRECT" var="autopilot_mode_auto2"/>
<stay wp="STDBY"/>
</block>
<block name="Landed">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
161 changes: 161 additions & 0 deletions conf/flight_plans/tudelft/oneloop_valkenburg.xml
View file
@@ -0,0 +1,161 @@
<!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">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
#include "modules/energy/electrical.h"
#include "modules/radio_control/radio_control.h"
#include "modules/ahrs/ahrs.h"
</header>
<waypoints>
<waypoint name="HOME" lat="52.1682196" lon="4.4134865"/>
<waypoint name="CLIMB" x="62.6" y="-80.4"/>
<waypoint name="STDBY" x="106.0" y="-55.1"/>
<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="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"/>
<waypoint lat="52.168049" lon="4.406923" name="C2"/>
<waypoint lat="52.166515" lon="4.408235" name="C3"/>
<waypoint lat="52.163255" lon="4.407668" name="C4"/>
<waypoint lat="52.161908" lon="4.410082" name="C5"/>
<waypoint lat="52.162641" lon="4.416992" name="C6"/>
<waypoint lat="52.164861" lon="4.427268" name="C7"/>
<waypoint lat="52.170422" lon="4.427511" name="C8"/>
<waypoint lat="52.172276" lon="4.424011" name="C9"/>
<waypoint lat="52.1673931" lon="4.4127541" name="p5"/>
<waypoint lat="52.1678569" lon="4.4150577" name="p6"/>
<waypoint lat="52.1689993" lon="4.4144979" name="p7"/>
<waypoint lat="52.1686045" lon="4.4121999" name="p8"/>
</waypoints>
<sectors>
<sector color="red" name="Hard_Geofence">
<corner name="C1"/>
<corner name="C2"/>
<corner name="C3"/>
<corner name="C4"/>
<corner name="C5"/>
<corner name="C6"/>
<corner name="C7"/>
<corner name="C8"/>
<corner name="C9"/>
</sector>
<sector color="blue" name="Soft_Geofence">
<corner name="p5"/>
<corner name="p6"/>
<corner name="p7"/>
<corner name="p8"/>
</sector>
</sectors>
<modules>
<!--module name="follow_me"/-->
<module name="nav" type="survey_rectangle_rotorcraft">
<define name="RECTANGLE_SURVEY_DEFAULT_SWEEP" value="100"/>
</module>
</modules>
<exceptions>
<!--Soft Geofencing (go back to Standby)-->
<exception cond="Or(!InsideSoft_Geofence(GetPosX(), GetPosY()), GetPosAlt() @GT 80.0) @AND
!(nav_block == IndexOfBlock('Wait GPS')) @AND
!(nav_block == IndexOfBlock('Geo init'))" deroute="safe"/>
<!-- Hard Geofencing (Kill) -->
<exception cond="(Or(!InsideHard_Geofence(GetPosX(), GetPosY()), GetPosAlt() @GT 80.0) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- RC lost -->
<exception cond="((radio_control.status == RC_REALLY_LOST) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- Datalink lost -->
<exception cond="((datalink_time @GT 5) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- Bat low (constant RPM descent)-->
<exception cond="(electrical.bat_low @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Bat critical (constant RPM no stabilization)-->
<exception cond="(electrical.bat_critical @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block key="r" name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<call_once fun="NavResurrect()"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosAlt() @GT 0.8" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Pre-Standby">
<while cond="oneloop_andi.half_loop"/>
<!-- <call_once fun="NavSetWaypointPosAndAltHere(WP_STDBY)"/> -->
</block>
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
</block>
<block name="safe">
<stay wp="HOME"/>
</block>
<block name="Circle_CW">
<set value="false" var="force_forward"/>
<circle radius="100" wp="FOLLOW"/>
</block>
<block key="l" name="land here" strip_button="land here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
<go wp="TD"/>
<deroute block="Flare"/>
</block>
<block name="Land">
<go wp="TD"/>
<deroute block="Flare"/>
</block>
<block name="Flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Landed"/>
<exception cond="0.10 @GT GetPosAlt()" deroute="Landed"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Safe landing">
<exception cond="!nav_is_in_flight()" deroute="Manual"/>
<exception cond="0.10 @GT GetPosAlt()" deroute="Manual"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Manual">
<set value="AP_MODE_ATTITUDE_DIRECT" var="autopilot_mode_auto2"/>
<stay wp="STDBY"/>
</block>
<block name="Landed">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
19 changes: 19 additions & 0 deletions conf/modules/guidance_oneloop.xml
View file
@@ -0,0 +1,19 @@
<!DOCTYPE module SYSTEM "module.dtd">

<module name="guidance_oneloop" dir="guidance" task="control">
<doc>
<description>
Dummy file for oneloop controller
</description>
</doc>
<dep>
<depends>@navigation,guidance_rotorcraft</depends>
<provides>guidance,attitude_command</provides>
</dep>
<header>
<file name="guidance_oneloop.h"/>
</header>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="guidance_oneloop.c" dir="$(SRC_FIRMWARE)/guidance"/>
</makefile>
</module>
40 changes: 40 additions & 0 deletions conf/modules/oneloop_andi.xml
View file
@@ -0,0 +1,40 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="oneloop_andi" dir="oneloop" task="control">
<doc>
<description>One loop (Guidance + Stabilization) ANDI controller for the rotating wing drone RW3C</description>
</doc>
<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_settings>
</dl_settings>
</settings>
<dep>
<depends>stabilization_rotorcraft,@attitude_command,wls</depends>
<provides>commands</provides>
</dep>
<header>
<file name="oneloop_andi.h"/>
</header>
<init fun="oneloop_andi_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="oneloop_andi.c" dir="$(SRC_FIRMWARE)/oneloop"/>
<configure name="ANDI_OUTPUTS" default="4"/>
<configure name="ANDI_NUM_ACT" default="4"/>
<configure name="ANDI_NUM_VIRTUAL_ACT" default="2"/>
<configure name="ANDI_NUM_ACT_TOT" default="6"/>
<define name="ANDI_OUTPUTS" value="$(ANDI_OUTPUTS)"/>
<define name="ANDI_NUM_ACT" value="$(ANDI_NUM_ACT)"/>
<define name="ANDI_NUM_VIRTUAL_ACT" value="$(ANDI_NUM_VIRTUAL_ACT)"/>
<define name="ANDI_NUM_ACT_TOT" value="$(ANDI_NUM_ACT_TOT)"/>
</makefile>
</module>
21 changes: 21 additions & 0 deletions conf/modules/stabilization_oneloop.xml
View file
@@ -0,0 +1,21 @@
<!DOCTYPE module SYSTEM "module.dtd">

<module name="stabilization_oneloop" dir="stabilization" task="control">
<doc>
<description>
Dummy stabilization for oneloop control
</description>
</doc>
<dep>
<depends>stabilization_rotorcraft,@attitude_command,wls</depends>
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_oneloop.h"/>
</header>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_oneloop.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<!-- <define name="STABILIZATION_ATTITUDE_TYPE_INT"/> -->
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_oneloop.h" type="string"/>
</makefile>
</module>
2 changes: 1 addition & 1 deletion conf/telemetry/AGGIEAIR/aggieair_iris.xml
View file
Expand Up @@ -99,7 +99,7 @@
<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="STAB_ATTITUDE" period=".25"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand Down
2 changes: 1 addition & 1 deletion conf/telemetry/AGGIEAIR/aggieair_rotorcraft.xml
View file
Expand Up @@ -104,7 +104,7 @@
<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="STAB_ATTITUDE" period=".25"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand Down
2 changes: 1 addition & 1 deletion conf/telemetry/DB/db_default_rotorcraft.xml
View file
Expand Up @@ -97,7 +97,7 @@
<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="STAB_ATTITUDE" period=".25"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand Down
4 changes: 2 additions & 2 deletions conf/telemetry/DB/db_quadshot.xml
View file
Expand Up @@ -14,7 +14,7 @@
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CMD" period=".25"/>
<message name="STAB_ATTITUDE_INDI" period=".5"/>
<message name="STAB_ATTITUDE" period=".5"/>
<message name="INS" period=".5"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
Expand Down Expand Up @@ -111,7 +111,7 @@

<mode name="indi">
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INDI" period="0.062"/>
<message name="STAB_ATTITUDE" period="0.062"/>
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
Expand Down
4 changes: 2 additions & 2 deletions conf/telemetry/default_quadshot.xml
View file
Expand Up @@ -14,7 +14,7 @@
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CMD" period=".25"/>
<message name="STAB_ATTITUDE_INDI" period=".5"/>
<message name="STAB_ATTITUDE" period=".5"/>
<message name="INS" period=".5"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
Expand Down Expand Up @@ -109,7 +109,7 @@

<mode name="indi">
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INDI" period="0.062"/>
<message name="STAB_ATTITUDE" period="0.062"/>
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
Expand Down
6 changes: 4 additions & 2 deletions conf/telemetry/default_rotorcraft.xml
View file
Expand Up @@ -37,6 +37,8 @@
<message name="INS_EKF2" period=".25"/>
<message name="WIND_INFO_RET" period="1."/>
<message name="AHRS_REF_QUAT" period="0.5"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>

<mode name="ppm">
Expand Down Expand Up @@ -109,8 +111,8 @@
<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"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand Down
2 changes: 1 addition & 1 deletion conf/telemetry/default_rotorcraft_mavlink.xml
View file
Expand Up @@ -129,7 +129,7 @@
<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="STAB_ATTITUDE" period=".25"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand Down
2 changes: 1 addition & 1 deletion conf/telemetry/default_rotorcraft_slow.xml
View file
Expand Up @@ -98,7 +98,7 @@
<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_INDI" period=".25"/>
<message name="STAB_ATTITUDE" period=".25"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand Down
103 changes: 51 additions & 52 deletions conf/telemetry/highspeed_rotorcraft.xml
View file
Expand Up @@ -10,6 +10,7 @@
<message name="DL_VALUE" period="0.7"/>
<message name="ROTORCRAFT_STATUS" period="0.4"/>
<message name="ROTORCRAFT_FP" period="0.20"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.20"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
Expand Down Expand Up @@ -45,10 +46,14 @@
<message name="HYBRID_GUIDANCE" period="0.4"/>
<message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.1"/>
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="GPS_RTK" period="0.1"/>
<message name="AIRSPEED_RAW" period="0.1"/>
<message name="ROTATING_WING_STATE" period="0.2"/>
<message name="ROTATING_WING_STATE" period="0.2"/>
<message name="ACTUATORS" period="0.02"/>
<message name="EFF_MAT_G" period="0.02"/>
<message name="GUIDANCE" period="0.02"/>
<message name="STAB_ATTITUDE" period="0.02"/>
</mode>

<mode name="calibration">
Expand Down Expand Up @@ -107,7 +112,6 @@
<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">
Expand All @@ -118,23 +122,23 @@
</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_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"/>
<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="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"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand All @@ -150,57 +154,52 @@
</mode>

<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<!--<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"/>
<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"/>
<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="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"/>
<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"/>
<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"/>
<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="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"/>
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
</mode>

</process>
Expand Down Expand Up @@ -240,13 +239,14 @@
<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="STAB_ATTITUDE" 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="IMU_HEATER" period="1.0"/>
<message name="AIRSPEED_RAW" period="0.01"/>
<message name="EFF_MAT_G" period="0.002"/>
<message name="GUIDANCE" period="0.002"/>
</mode>
</process>

Expand All @@ -269,4 +269,3 @@
</process>

</telemetry>

2 changes: 1 addition & 1 deletion conf/telemetry/rotorcraft_with_logger.xml
View file
Expand Up @@ -97,7 +97,7 @@
<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_INDI" period=".25"/>
<message name="STAB_ATTITUDE" period=".25"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand Down
2 changes: 1 addition & 1 deletion conf/telemetry/tudelft/outback.xml
View file
Expand Up @@ -98,7 +98,7 @@
<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_INDI" period=".25"/>
<message name="STAB_ATTITUDE" period=".25"/>
</mode>

<mode name="vert_loop" key_press="v">
Expand Down
123 changes: 123 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c
View file
@@ -0,0 +1,123 @@
/*
* Copyright (C) 2015 Tomaso De Ponti <t.m.l.deponti@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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/

/**
* @file firmwares/rotorcraft/guidance/guidance_oneloop.c
*
* A dummy guidance module to run the oneloop_andi controller
*
*/

#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_oneloop.h"
#include "firmwares/rotorcraft/oneloop/oneloop_andi.h"

void guidance_h_run_enter(void)
{
oneloop_andi_enter(false);
}

void guidance_v_run_enter(void)
{
// nothing to do
}

static struct VerticalGuidance *_gv = &guidance_v;
static enum GuidanceOneloop_VMode _v_mode = GUIDANCE_ONELOOP_V_POS;

struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
{
return guidance_oneloop_run_mode(in_flight, gh, _gv, GUIDANCE_ONELOOP_H_POS, _v_mode);
}

struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
{
return guidance_oneloop_run_mode(in_flight, gh, _gv, GUIDANCE_ONELOOP_H_SPEED, _v_mode);
}

struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
{
return guidance_oneloop_run_mode(in_flight, gh, _gv, GUIDANCE_ONELOOP_H_ACCEL, _v_mode);
}

int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_POS;
return 0; // nothing to do
}

int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_SPEED;
return 0; // nothing to do
}

int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_ACCEL;
return 0; // nothing to do
}

struct StabilizationSetpoint guidance_oneloop_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceOneloop_HMode h_mode, enum GuidanceOneloop_VMode v_mode)
{
struct FloatVect3 PSA_des = { 0 };
int rm_order_h = 3;
int rm_order_v = 3;
// Oneloop controller wants desired targets and handles reference generation internally
if (h_mode == GUIDANCE_ONELOOP_H_POS) {
PSA_des.x = POS_FLOAT_OF_BFP(gh->sp.pos.x);
PSA_des.y = POS_FLOAT_OF_BFP(gh->sp.pos.y);
rm_order_h = 3;
}
else if (h_mode == GUIDANCE_ONELOOP_H_SPEED) {
PSA_des.x = SPEED_FLOAT_OF_BFP(gh->sp.speed.x);
PSA_des.y = SPEED_FLOAT_OF_BFP(gh->sp.speed.y);
PSA_des.z = SPEED_FLOAT_OF_BFP(gv->zd_sp);
rm_order_h = 2;
}
else { // H_ACCEL
PSA_des.x = ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
PSA_des.y = ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
PSA_des.z = ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
rm_order_h = 1;
}

if (v_mode == GUIDANCE_ONELOOP_V_POS){
PSA_des.z = POS_FLOAT_OF_BFP(gv->z_sp);
rm_order_v = 3;
}
else if (v_mode == GUIDANCE_ONELOOP_V_SPEED) {
PSA_des.z = SPEED_FLOAT_OF_BFP(gv->zd_sp);
rm_order_v = 2;
}
else { // H_ACCEL
PSA_des.z = ACCEL_FLOAT_OF_BFP(gv->zdd_ref); //why is there not acceleration SP and only REF?
rm_order_v = 1;
}
oneloop_andi.half_loop = false;
oneloop_andi_run(in_flight, oneloop_andi.half_loop, PSA_des, rm_order_h, rm_order_v);
struct StabilizationSetpoint sp = { 0 };
return sp;
}
47 changes: 47 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.h
View file
@@ -0,0 +1,47 @@
/*
* Copyright (C) 2015 Tomaso De Ponti <t.m.l.deponti@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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/

/**
* @file firmwares/rotorcraft/guidance/guidance_oneloop.h
*
* A dummy guidance mode to run the oneloop_andi controller
*/

#ifndef GUIDANCE_ONELOOP_H
#define GUIDANCE_ONELOOP_H

#include "firmwares/rotorcraft/oneloop/oneloop_andi.h"

enum GuidanceOneloop_HMode {
GUIDANCE_ONELOOP_H_POS,
GUIDANCE_ONELOOP_H_SPEED,
GUIDANCE_ONELOOP_H_ACCEL
};

enum GuidanceOneloop_VMode {
GUIDANCE_ONELOOP_V_POS,
GUIDANCE_ONELOOP_V_SPEED,
GUIDANCE_ONELOOP_V_ACCEL
};

extern struct StabilizationSetpoint guidance_oneloop_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceOneloop_HMode h_mode, enum GuidanceOneloop_VMode v_mode);

#endif /* GUIDANCE_INDI_H */
1,538 changes: 1,538 additions & 0 deletions sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
View file

Large diffs are not rendered by default.

134 changes: 134 additions & 0 deletions sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h
View file
@@ -0,0 +1,134 @@
/*
* Copyright (C) 2023 Tomaso De Ponti <tmldeponti@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 "firmwares/rotorcraft/oneloop/oneloop_andi.h"
* @author Tomaso De Ponti <tmldeponti@tudelft.nl>
* One loop (Guidance + Stabilization) ANDI controller for the rotating wing drone RW3C
*/

#ifndef ONELOOP_ANDI_H
#define ONELOOP_ANDI_H

#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"

#define ANDI_G_SCALING 1000.0f

extern float act_state_filt_vect_1l[ANDI_NUM_ACT];
extern float actuator_state_1l[ANDI_NUM_ACT];
extern float nu[6];
extern float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT];
extern float andi_u[ANDI_NUM_ACT_TOT];
extern float andi_du[ANDI_NUM_ACT_TOT];
extern float psi_des_deg;

// Delete once hybrid nav is fixed //////////////////////////////////////////////////////////////////////////////////
struct guidance_indi_hybrid_params {
float pos_gain;
float pos_gainz;
float speed_gain;
float speed_gainz;
float heading_bank_gain;
float liftd_asq;
float liftd_p80;
float liftd_p50;
};
extern struct guidance_indi_hybrid_params gih_params;
extern bool force_forward;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
struct OneloopGuidanceRef {
float pos[3];
float vel[3];
float acc[3];
float jer[3];
};

struct OneloopGuidanceState {
float pos[3];
float vel[3];
float acc[3];
};

struct OneloopStabilizationRef {
float att[3];
float att_d[3];
float att_2d[3];
float att_3d[3];
};

struct OneloopStabilizationState {
float att[3];
float att_d[3];
float att_2d[3];
};
struct OneloopGeneral {
bool half_loop;
struct OneloopGuidanceRef gui_ref; // Guidance References
struct OneloopGuidanceState gui_state; // Guidance State
struct OneloopStabilizationRef sta_ref; // Stabilization References
struct OneloopStabilizationState sta_state; // Stabilization State

};

extern struct OneloopGeneral oneloop_andi;

struct PolePlacement{
float omega_n;
float zeta;
float p3;
};
struct Gains3rdOrder{
float k1[3];
float k2[3];
float k3[3];
};
struct Gains2ndOrder{
float k2;
float k3;
};

/*Declaration of Reference Model and Error Controller Gains*/
extern struct PolePlacement p_att_e;
extern struct PolePlacement p_att_rm;
/*Position Loop*/
extern struct PolePlacement p_pos_e;
extern struct PolePlacement p_pos_rm;
/*Altitude Loop*/
extern struct PolePlacement p_alt_e;
extern struct PolePlacement p_alt_rm;
/*Heading Loop*/
extern struct PolePlacement p_head_e;
extern struct PolePlacement p_head_rm;
/*Gains of EC and RM*/
extern struct Gains3rdOrder k_att_e;
extern struct Gains3rdOrder k_att_rm;
extern struct Gains2ndOrder k_head_e;
extern struct Gains2ndOrder k_head_rm;
extern struct Gains3rdOrder k_pos_e;
extern struct Gains3rdOrder k_pos_rm;
extern void oneloop_andi_init(void);
extern void oneloop_andi_enter(bool half_loop_sp);
extern void oneloop_andi_set_failsafe_setpoint(void);
extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
extern void oneloop_from_nav(bool in_flight);
#endif // ONELOOP_ANDI_H
62 changes: 31 additions & 31 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
View file
Expand Up @@ -281,13 +281,18 @@ void sum_g1_g2(void);

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1g2[0],
INDI_NUM_ACT, g1g2[1],
INDI_NUM_ACT, g1g2[2],
INDI_NUM_ACT, g1g2[3],
INDI_NUM_ACT, g2_est);
float zero = 0.0;
pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
1, &zero,
1, &zero,
1, &zero,
INDI_NUM_ACT, g1g2[0],
INDI_NUM_ACT, g1g2[1],
INDI_NUM_ACT, g1g2[2],
INDI_NUM_ACT, g1g2[3],
INDI_NUM_ACT, g2_est);
}

static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
Expand All @@ -306,30 +311,25 @@ static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *d

static void send_att_full_indi(struct transport_tx *trans, struct link_device *dev)
{
float zero = 0.0;
struct FloatRates *body_rates = stateGetBodyRates_f();
struct Int32Vect3 *body_accel_i = stateGetAccelBody_i();
struct FloatVect3 body_accel_f_telem;
ACCELS_FLOAT_OF_BFP(body_accel_f_telem, *body_accel_i);
float zero = 0;
pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID,
&body_accel_f_telem.x, // input lin.acc
&body_accel_f_telem.y,
&body_accel_f_telem.z,
&body_rates->p, // rate
&body_rates->q,
&body_rates->r,
&angular_rate_ref.p, // rate.sp
&angular_rate_ref.q,
&angular_rate_ref.r,
&angular_acceleration[0], // ang.acc
&angular_acceleration[1],
&angular_acceleration[2],
&angular_accel_ref.p, // ang.acc.sp
&angular_accel_ref.q,
&angular_accel_ref.r,
&zero, &zero, // eff.mat
&zero, &zero,
INDI_NUM_ACT, indi_u); // out
pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
&zero, &zero, &zero, // att
&zero, &zero, &zero, // att.ref
&body_rates->p, // rate
&body_rates->q,
&body_rates->r,
&angular_rate_ref.p, // rate.ref
&angular_rate_ref.q,
&angular_rate_ref.r,
&angular_acceleration[0], // ang.acc
&angular_acceleration[1],
&angular_acceleration[2],
&angular_accel_ref.p, // ang.acc.ref
&angular_accel_ref.q,
&angular_accel_ref.r,
1, &zero, // inputs
INDI_NUM_ACT, indi_u); // out
}
#endif

Expand Down Expand Up @@ -387,9 +387,9 @@ void stabilization_indi_init(void)
}

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INDI_G, send_indi_g);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_G, send_eff_mat_g_indi);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_full_indi);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_att_full_indi);
#endif
}

Expand Down
51 changes: 31 additions & 20 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
View file
Expand Up @@ -144,30 +144,40 @@ struct IndiVariables indi = {
#include "modules/datalink/telemetry.h"

static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
{
float zero = 0.0;
pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
&zero, &zero, &zero, // att
&zero, &zero, &zero, // att.ref
&indi.rate[0].o[0], // rate
&indi.rate[1].o[0],
&indi.rate[2].o[0],
&zero, &zero, &zero, // rate.ref
&indi.rate_d[0], // ang.acc = rate.diff
&indi.rate_d[1],
&indi.rate_d[2],
&indi.angular_accel_ref.p, // ang.acc.ref
&indi.angular_accel_ref.q,
&indi.angular_accel_ref.r,
1, &zero, // inputs
1, &zero); // outputs
}
static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_device *dev)
{
//The estimated G values are scaled, so scale them back before sending
struct FloatRates g1_disp;
RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE);
float g2_disp = indi.est.g2 * INDI_EST_SCALE;
float zero = 0;

pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID,
&zero, &zero, &zero, // input lin.acc
&indi.rate[0].o[0], // rate
&indi.rate[1].o[0],
&indi.rate[2].o[0],
&zero, &zero, &zero, // rate.ref
&indi.rate_d[0], // ang.acc = rate.diff
&indi.rate_d[1],
&indi.rate_d[2],
&indi.angular_accel_ref.p, // ang.acc.ref
&indi.angular_accel_ref.q,
&indi.angular_accel_ref.r,
&g1_disp.p, // matrix
&g1_disp.q,
&g1_disp.r,
&g2_disp,
0, &zero); // outputs
float zero = 0.0;
pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
1, &zero,
1, &zero,
1, &zero,
1, &g1_disp.p,
1, &g1_disp.q,
1, &g1_disp.r,
1, &g2_disp,
1, &zero);
}

static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
Expand All @@ -191,7 +201,8 @@ void stabilization_indi_init(void)
indi_init_filters();

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_indi);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_att_indi);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_G, send_eff_mat_g_indi_simple);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat);
#endif
}
Expand Down
94 changes: 94 additions & 0 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c
View file
@@ -0,0 +1,94 @@
/*
* Copyright (C) 2011-2012 The Paparazzi Team
*
* 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 stabilization_none.c
* Dummy stabilization for rotorcrafts.
*
* Doesn't actually do any stabilization,
* just directly passes the RC commands along.
*/

//#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_oneloop.h"
#include "firmwares/rotorcraft/oneloop/oneloop_andi.h"

#include "modules/radio_control/radio_control.h"
#include "generated/airframe.h"
#include "generated/modules.h"


struct FloatEulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat;
struct FloatRates stab_att_ff_rates;



void stabilization_attitude_init(void)
{
// oneloop init is already done through module init
}

void stabilization_attitude_enter(void)
{
oneloop_andi_enter(true);
}

void stabilization_attitude_set_failsafe_setpoint(void)
{

}

void stabilization_attitude_set_rpy_setpoint_i(UNUSED struct Int32Eulers *rpy)
{

}

void stabilization_attitude_set_quat_setpoint_i(UNUSED struct Int32Quat *quat)
{

}

void stabilization_attitude_set_earth_cmd_i(UNUSED struct Int32Vect2 *cmd, UNUSED int32_t heading)
{

}

void stabilization_attitude_set_stab_sp(UNUSED struct StabilizationSetpoint *sp)
{

}

void stabilization_attitude_run(bool in_flight)
{
struct FloatVect3 PSA_des = { 0 };
int rm_order_h = 3;
int rm_order_v = 3;
// Run the oneloop controller in half-loop mode
if (oneloop_andi.half_loop){
oneloop_andi_run(in_flight, oneloop_andi.half_loop, PSA_des, rm_order_h, rm_order_v);
}
}


void stabilization_attitude_read_rc(UNUSED bool in_flight, UNUSED bool in_carefree, UNUSED bool coordinated_turn)
{

}
43 changes: 43 additions & 0 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.h
View file
@@ -0,0 +1,43 @@
/*
* Copyright (C) 2011-2012 The Paparazzi Team
*
* 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 stabilization_none.h
* Dummy stabilization for rotorcrafts.
*
* Doesn't actually do any stabilization,
* just directly passes the RC commands along.
*/

#ifndef STABILIZATION_ONELOOP
#define STABILIZATION_ONELOOP

#include "math/pprz_algebra_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"

extern struct FloatEulers stab_att_sp_euler;
extern struct Int32Quat stab_att_sp_quat;
extern struct FloatRates stab_att_ff_rates;


extern struct Int32Rates stabilization_oneloop_rc_cmd;


#endif /* STABILIZATION_NONE */
2 changes: 1 addition & 1 deletion sw/ext/pprzlink
Submodule pprzlink updated 1 files
+52 −29 message_definitions/v1.0/messages.xml
2 changes: 1 addition & 1 deletion sw/ground_segment/python/rot_wing_mon/rot_wing_viewer.py
View file
Expand Up @@ -140,7 +140,7 @@ def message_recv(self, ac_id, msg):
self.motors.fill_from_esc_msg(self.esc)
wx.CallAfter(self.update)

if msg.name == "STAB_ATTITUDE_INDI":
if msg.name == "STAB_ATTITUDE":
self.indi = INDIMessage(msg)
wx.CallAfter(self.update)

Expand Down