Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 11:37:06 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Oneloop Controller Pull Request (#3150)

Browse Source
This commit is contained in:
Tomaso Maria Luigi De Ponti
2023-11-07 14:16:37 +01:00
committed by GitHub
parent 41451d5422
commit cc071e1ca7
31 changed files with 3407 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml
+212
View File
@@ -0,0 +1,212 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop_one_indi_ekf2">
<description> bebop with a oneloop ANDI and backup INDI controller and EKF2
</description>
<firmware name="rotorcraft">
<autopilot name="rotorcraft_oneloop_with_backup.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="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="stabilization" type="indi" >
<configure name="INDI_NUM_ACT" value="4"/>
</module>
<module name="ins" type="ekf2">
<define name="INS_EKF2_OPTITRACK" value="true"/>
</module>
<module name="nav_hybrid"/>
<module name="guidance" type="indi">
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
</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-->
</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 name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="400" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<define name="G1_ROLL" value="{20 , -20, -20 , 20 }"/>
<define name="G1_PITCH" value="{14 , 14, -14 , -14 }"/>
<define name="G1_YAW" value="{-1, 1, -1, 1}"/>
<define name="G1_THRUST" value="{-.4, -.4, -.4, -.4}"/>
<define name="G2" value="{-61.2093, 65.3670, -65.7419, 65.4516}"/>
<define name="REF_ERR_P" value="600.0"/>
<define name="REF_ERR_Q" value="600.0"/>
<define name="REF_ERR_R" value="600.0"/>
<define name="REF_RATE_P" value="28.0"/>
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
<define name="FILT_CUTOFF" value="5.0"/>
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
<define name="USE_ADAPTIVE" value="TRUE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="REF_MAX_SPEED" value="2" unit="m/s"/>
<define name="PGAIN" value="79"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</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>
conf/airframes/tudelft/bebop_oneloop_ekf2.xml
+167
View File
@@ -0,0 +1,167 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop_oneloop_ekf2">
<description> bebop with a oneloop ANDI controller and EKF2
</description>
<firmware name="rotorcraft">
<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_ATTITUDE_Z_HOLD"/>
<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>
conf/airframes/tudelft/bebop_oneloop_generated_ekf2.xml
+168
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>
conf/autopilot/rotorcraft_oneloop.xml
+177
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>
conf/autopilot/rotorcraft_oneloop_with_backup.xml
+198
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>
conf/flight_plans/tudelft/oneloop_cyberzoo.xml
+135
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>
conf/flight_plans/tudelft/oneloop_valkenburg.xml
+161
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>
conf/modules/guidance_oneloop.xml
+19
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>
conf/modules/oneloop_andi.xml
+40
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>
conf/modules/stabilization_oneloop.xml
+21
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>
conf/telemetry/AGGIEAIR/aggieair_iris.xml
+1 -1
View File
@@ -99,7 +99,7 @@
<message name="STAB_ATTITUDE_REF_INT" period=".03"/> <message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/> <message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_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>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
conf/telemetry/AGGIEAIR/aggieair_rotorcraft.xml
+1 -1
View File
@@ -104,7 +104,7 @@
<message name="STAB_ATTITUDE_REF_INT" period=".03"/> <message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/> <message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_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>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
conf/telemetry/DB/db_default_rotorcraft.xml
+1 -1
View File
@@ -97,7 +97,7 @@
<message name="STAB_ATTITUDE_REF_INT" period=".03"/> <message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/> <message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_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>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
conf/telemetry/DB/db_quadshot.xml
+2 -2
View File
@@ -14,7 +14,7 @@
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/> <message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CMD" period=".25"/> <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="INS" period=".5"/>
<message name="ENERGY" period="2.5"/> <message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/> <message name="DATALINK_REPORT" period="5.1"/>
@@ -111,7 +111,7 @@
<mode name="indi"> <mode name="indi">
<message name="ALIVE" period="0.9"/> <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_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
conf/telemetry/default_quadshot.xml
+2 -2
View File
@@ -14,7 +14,7 @@
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/> <message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CMD" period=".25"/> <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="INS" period=".5"/>
<message name="ENERGY" period="2.5"/> <message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/> <message name="DATALINK_REPORT" period="5.1"/>
@@ -109,7 +109,7 @@
<mode name="indi"> <mode name="indi">
<message name="ALIVE" period="0.9"/> <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_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
conf/telemetry/default_rotorcraft.xml
+4 -2
View File
@@ -37,6 +37,8 @@
<message name="INS_EKF2" period=".25"/> <message name="INS_EKF2" period=".25"/>
<message name="WIND_INFO_RET" period="1."/> <message name="WIND_INFO_RET" period="1."/>
<message name="AHRS_REF_QUAT" period="0.5"/> <message name="AHRS_REF_QUAT" period="0.5"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode> </mode>
<mode name="ppm"> <mode name="ppm">
@@ -109,8 +111,8 @@
<message name="STAB_ATTITUDE_REF_INT" period=".03"/> <message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/> <message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/> <message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/> <message name="STAB_ATTITUDE" period=".25"/>
<message name="INDI_G" period="2.0"/> <message name="EFF_MAT_G" period="2.0"/>
</mode> </mode>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
conf/telemetry/default_rotorcraft_mavlink.xml
+1 -1
View File
@@ -129,7 +129,7 @@
<message name="STAB_ATTITUDE_REF_INT" period=".03"/> <message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/> <message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_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>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
conf/telemetry/default_rotorcraft_slow.xml
+1 -1
View File
@@ -98,7 +98,7 @@
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/> <message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_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>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
conf/telemetry/highspeed_rotorcraft.xml
+51 -52
View File
@@ -10,6 +10,7 @@
<message name="DL_VALUE" period="0.7"/> <message name="DL_VALUE" period="0.7"/>
<message name="ROTORCRAFT_STATUS" period="0.4"/> <message name="ROTORCRAFT_STATUS" period="0.4"/>
<message name="ROTORCRAFT_FP" period="0.20"/> <message name="ROTORCRAFT_FP" period="0.20"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.20"/>
<message name="ALIVE" period="2.1"/> <message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/> <message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
@@ -45,10 +46,14 @@
<message name="HYBRID_GUIDANCE" period="0.4"/> <message name="HYBRID_GUIDANCE" period="0.4"/>
<message name="ESC" period="0.5"/> <message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.1"/> <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="GPS_RTK" period="0.1"/>
<message name="AIRSPEED_RAW" 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>
<mode name="calibration"> <mode name="calibration">
@@ -107,7 +112,6 @@
<message name="AHRS_QUAT_INT" period=".25"/> <message name="AHRS_QUAT_INT" period=".25"/>
<message name="AHRS_EULER_INT" period=".1"/> <message name="AHRS_EULER_INT" period=".1"/>
<message name="AHRS_EULER" period=".1"/> <message name="AHRS_EULER" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode> </mode>
<mode name="rate_loop"> <mode name="rate_loop">
@@ -118,23 +122,23 @@
</mode> </mode>
<mode name="attitude_setpoint_viz" key_press="v"> <mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/> <message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/> <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>
<mode name="attitude_loop" key_press="a"> <mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/> <message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/> <message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/> <message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/> <message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/> <message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/> <message name="STAB_ATTITUDE" period=".25"/>
<message name="INDI_G" period="2.0"/> <message name="EFF_MAT_G" period="2.0"/>
</mode> </mode>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
@@ -150,57 +154,52 @@
</mode> </mode>
<mode name="vel_guidance" key_press="q"> <mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/> <message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/> <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_FP" period="0.8"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="INS_REF" period="5.1"/>
<message name="INS_REF" period="5.1"/> <message name="WP_MOVED" period="1.3"/>
<message name="WP_MOVED" period="1.3"/> <message name="GPS_INT" period="1.0"/>
<message name="GPS_INT" period="1.0"/> <message name="INS" period="1.0"/>
<message name="INS" period="1.0"/>
</mode> </mode>
<mode name="h_loop" key_press="h"> <mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/> <message name="HOVER_LOOP" period="0.062"/>
<message name="GUIDANCE_H_REF_INT" period="0.062"/> <message name="GUIDANCE_H_REF_INT" period="0.062"/>
<message name="STAB_ATTITUDE_INT" period="0.4"/> <message name="STAB_ATTITUDE_INT" period="0.4"/>
<message name="STAB_ATTITUDE_FLOAT" 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_FP" period="0.8"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="INS_REF" period="5.1"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF --> <!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/> <message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/> <message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/> <message name="HFF_DBG" period=".2"/>
</mode> </mode>
<mode name="aligner"> <mode name="aligner">
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/> <message name="FILTER_ALIGNER" period="0.02"/>
</mode> </mode>
<mode name="tune_hover"> <mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/> <message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/> <message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/> <message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/> <message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> --> <message name="INS_REF" period="5.1"/>
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode> </mode>
<mode name="RTCM3" > <mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/> <message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/> <message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/> <message name="GPS_RTK" period="1"/>
</mode> </mode>
</process> </process>
@@ -240,13 +239,14 @@
<message name="GUIDANCE_INDI_HYBRID" period="0.002"/> <message name="GUIDANCE_INDI_HYBRID" period="0.002"/>
<message name="HYBRID_GUIDANCE" period="0.02"/> <message name="HYBRID_GUIDANCE" period="0.02"/>
<message name="ESC" 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="PPM" period="0.05"/>
<message name="INDI_G" period="0.1"/>
<message name="ACTUATORS" period="0.002"/> <message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/> <message name="GPS_RTK" period="0.1"/>
<message name="IMU_HEATER" period="1.0"/> <message name="IMU_HEATER" period="1.0"/>
<message name="AIRSPEED_RAW" period="0.01"/> <message name="AIRSPEED_RAW" period="0.01"/>
<message name="EFF_MAT_G" period="0.002"/>
<message name="GUIDANCE" period="0.002"/>
</mode> </mode>
</process> </process>
@@ -269,4 +269,3 @@
</process> </process>
</telemetry> </telemetry>
conf/telemetry/rotorcraft_with_logger.xml
+1 -1
View File
@@ -97,7 +97,7 @@
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/> <message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_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>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
conf/telemetry/tudelft/outback.xml
+1 -1
View File
@@ -98,7 +98,7 @@
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/> <message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_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>
<mode name="vert_loop" key_press="v"> <mode name="vert_loop" key_press="v">
sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c
+123
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;
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.h
+47
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 */
sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
+1538
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h
+134
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
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+31 -31
View File
@@ -281,13 +281,18 @@ void sum_g1_g2(void);
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h" #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], float zero = 0.0;
INDI_NUM_ACT, g1g2[1], pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
INDI_NUM_ACT, g1g2[2], 1, &zero,
INDI_NUM_ACT, g1g2[3], 1, &zero,
INDI_NUM_ACT, g2_est); 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) static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
@@ -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) 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 FloatRates *body_rates = stateGetBodyRates_f();
struct Int32Vect3 *body_accel_i = stateGetAccelBody_i(); pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
struct FloatVect3 body_accel_f_telem; &zero, &zero, &zero, // att
ACCELS_FLOAT_OF_BFP(body_accel_f_telem, *body_accel_i); &zero, &zero, &zero, // att.ref
float zero = 0; &body_rates->p, // rate
pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID, &body_rates->q,
&body_accel_f_telem.x, // input lin.acc &body_rates->r,
&body_accel_f_telem.y, &angular_rate_ref.p, // rate.ref
&body_accel_f_telem.z, &angular_rate_ref.q,
&body_rates->p, // rate &angular_rate_ref.r,
&body_rates->q, &angular_acceleration[0], // ang.acc
&body_rates->r, &angular_acceleration[1],
&angular_rate_ref.p, // rate.sp &angular_acceleration[2],
&angular_rate_ref.q, &angular_accel_ref.p, // ang.acc.ref
&angular_rate_ref.r, &angular_accel_ref.q,
&angular_acceleration[0], // ang.acc &angular_accel_ref.r,
&angular_acceleration[1], 1, &zero, // inputs
&angular_acceleration[2], INDI_NUM_ACT, indi_u); // out
&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
} }
#endif #endif
@@ -387,9 +387,9 @@ void stabilization_indi_init(void)
} }
#if PERIODIC_TELEMETRY #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_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 #endif
} }
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+31 -20
View File
@@ -144,30 +144,40 @@ struct IndiVariables indi = {
#include "modules/datalink/telemetry.h" #include "modules/datalink/telemetry.h"
static void send_att_indi(struct transport_tx *trans, struct link_device *dev) 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 //The estimated G values are scaled, so scale them back before sending
struct FloatRates g1_disp; struct FloatRates g1_disp;
RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE); RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE);
float g2_disp = indi.est.g2 * INDI_EST_SCALE; float g2_disp = indi.est.g2 * INDI_EST_SCALE;
float zero = 0; float zero = 0.0;
pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID, 1, &zero,
&zero, &zero, &zero, // input lin.acc 1, &zero,
&indi.rate[0].o[0], // rate 1, &zero,
&indi.rate[1].o[0], 1, &g1_disp.p,
&indi.rate[2].o[0], 1, &g1_disp.q,
&zero, &zero, &zero, // rate.ref 1, &g1_disp.r,
&indi.rate_d[0], // ang.acc = rate.diff 1, &g2_disp,
&indi.rate_d[1], 1, &zero);
&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
} }
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
@@ -191,7 +201,8 @@ void stabilization_indi_init(void)
indi_init_filters(); indi_init_filters();
#if PERIODIC_TELEMETRY #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); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat);
#endif #endif
} }
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c
+94
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)
{
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.h
+43
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 */
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: d4bfd949d7...bd349ff1ee
sw/ground_segment/python/rot_wing_mon/rot_wing_viewer.py
+1 -1
View File
@@ -140,7 +140,7 @@ class RotWingFrame(wx.Frame):
self.motors.fill_from_esc_msg(self.esc) self.motors.fill_from_esc_msg(self.esc)
wx.CallAfter(self.update) wx.CallAfter(self.update)
if msg.name == "STAB_ATTITUDE_INDI": if msg.name == "STAB_ATTITUDE":
self.indi = INDIMessage(msg) self.indi = INDIMessage(msg)
wx.CallAfter(self.update) wx.CallAfter(self.update)