Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Hybrid guidance code for hybrid drones (#1769)

Browse Source 
This pull request is for the guidance algorithms used to control hybrid drones like the Quadshot and MavShot. It supports autonomous waypoint navigation as well as "forward-flight". The code was flight tested in autonomous waypoint navigation mode, including autonomous landing (using sonar).
This commit is contained in:
Deon Blaauw
2016-06-30 21:51:52 +02:00
committed by Felix Ruess
parent e51f67b40c
commit f1f75e96b8
28 changed files with 2249 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/DB/db_conf.xml
+13 -2
View File
@@ -7,7 +7,7 @@
telemetry="telemetry/DB/db_default_rotorcraft.xml"
flight_plan="flight_plans/DB/db_outdoors.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/rotorcraft_speed.xml settings/control/stabilization_indi.xml [settings/control/stabilization_rate.xml] [settings/control/stabilization_att_int.xml] [settings/control/stabilization_att_int_quat.xml] [settings/control/stabilization_att_float_euler.xml]"
settings_modules="modules/geo_mag.xml modules/air_data.xml"
settings_modules="modules/gps.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="#0d890debf1c6"
/>
<aircraft
@@ -18,7 +18,18 @@
telemetry="telemetry/DB/db_default_rotorcraft.xml"
flight_plan="flight_plans/DB/db_outdoors.xml"
settings="settings/rotorcraft_basic.xml settings/modules/config_asctec_v2_new.xml"
settings_modules="modules/geo_mag.xml [modules/gps_ubx_ucenter.xml]"
settings_modules="modules/geo_mag.xml [modules/gps_ubx_ucenter.xml] modules/gps.xml"
gui_color="#ee1bf3780c03"
/>
<aircraft
name="quadshot"
ac_id="53"
airframe="airframes/DB/db_quadshot_asp21_spektrum.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/DB/db_quadshot.xml"
flight_plan="flight_plans/DB/db_quadshot_delft.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_indi.xml settings/control/hybrid_guidance.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/gps.xml"
gui_color="blue"
/>
</conf>
conf/airframes/DB/db_mavshot.xml
+2 -1
View File
@@ -278,12 +278,13 @@
<module name="actuators" type="asctec_v2_new"/>
<module name="telemetry" type="transparent"/>
<!-- <module name="telemetry" type="transparent"/> -->
<module name="imu" type="aspirin_v2.2"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="indi"/>
<module name="stabilization" type="rate_indi"/>
<module name="guidance" type="hybrid"/>
<module name="logger_spi_link"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<!--define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" value="TRUE"/-->
conf/airframes/DB/db_quadshot_asp21_spektrum.xml
+384
View File
@@ -0,0 +1,384 @@
<!-- this is a quadshot vehicle equiped with Lisa/m2.0 and an aspirin 2.1
More information on the Quadshot can be found at transition-robotics.com -->
<airframe name="quadshot aspirin 2.1 spektrum">
<servos driver="Pwm">
<servo name="A1" no="0" min="1000" neutral="1100" max="2000"/>
<servo name="A2" no="1" min="1000" neutral="1100" max="2000"/>
<servo name="B1" no="2" min="1000" neutral="1100" max="2000"/>
<servo name="B2" no="3" min="1000" neutral="1100" max="2000"/>
<!-- <servo name="A1" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="A2" no="1" min="1000" neutral="1000" max="2000"/>
<servo name="B1" no="2" min="1000" neutral="1000" max="2000"/>
<servo name="B2" no="3" min="1000" neutral="1000" max="2000"/>-->
<!-- <servo name="ELEVON_LEFT" no="4" min="1150" neutral="1500" max="1850"/>
<servo name="ELEVON_RIGHT" no="5" min="1150" neutral="1500" max="1850"/>-->
<servo name="ELEVON_LEFT" no="4" min="1300" neutral="1500" max="1700"/>
<servo name="ELEVON_RIGHT" no="5" min="1300" neutral="1500" max="1700"/>
</servos>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="A1" value="motor_mixing.commands[0]"/>
<set servo="A2" value="motor_mixing.commands[1]"/>
<set servo="B1" value="motor_mixing.commands[2]"/>
<set servo="B2" value="motor_mixing.commands[3]"/>
<!-- Mode dependent actuator laws for the elevons. The elevons act different in rc attitude flight mode-->
<!-- First the correct feedback is stored in variables -->
<let var="aileron_feedback_left" value="-@YAW"/>
<let var="aileron_feedback_right" value="-@YAW"/>
<let var="elevator_feedback_left" value="-@PITCH"/>
<let var="elevator_feedback_right" value="+@PITCH"/>
<!-- Here the gains are defined for the two feedback cases, hover and forward-->
<let var="hover_left" value="15*$aileron_feedback_left"/>
<let var="hover_right" value="15*$aileron_feedback_right"/>
<!-- if using INDI -->
<!-- <let var="forward_left" value="aileron_gain*$aileron_feedback_left+elevator_gain*$elevator_feedback_left"/>
<let var="forward_right" value="aileron_gain*$aileron_feedback_right+elevator_gain*$elevator_feedback_right"/>-->
<!-- if using PID with gain scheduling -->
<let var="forward_left" value="5*$aileron_feedback_left + 4*$elevator_feedback_left"/>
<let var="forward_right" value="5*$aileron_feedback_right + 4*$elevator_feedback_right"/>
<!-- This statement tells the autopilot to use the hover feedback if in mode attitude direct and to use the forward feedback in all other cases-->
<set servo="ELEVON_LEFT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_left : $forward_left" />
<set servo="ELEVON_RIGHT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_right : $forward_right" />
<!-- <set servo="ELEVON_RIGHT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? 9000 : -9000" /> -->
</command_laws>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<define name="PITCH_COEF" value="{ -256, -256, 256, 256 }"/>
<define name="ROLL_COEF" value="{ 256, -256, 256, -256 }"/>
<define name="YAW_COEF" value="{ -256, 256, 128, -128 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
<!-- <define name="PITCH_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="ROLL_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="YAW_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="THRUST_COEF" value="{ 0, 0, 0, 0 }"/>-->
</section>
<!-- If you got a caliberation XML document from your IMU supplier, import this in the IMU section -->
<!-- Note that is better to have *no* caliberation file at all, than a one with incorrect caliberation values.
The default factory values are most of the time perfectly acceptable, so by default we will not use the caliberation file -->
<section name="IMU" prefix="IMU_">
<!-- Use default driver values for gyro -->
<!-- IMU calibration, make sure to calibrate the IMU properly before flight, see the wiki for more info-->
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<define name="ACCEL_X_SENS" value="4.86487566223" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.89957269597" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.82616398266" integer="16"/>
<!-- <define name="MAG_X_NEUTRAL" value="104"/> -->
<!-- <define name="MAG_Y_NEUTRAL" value="9"/> -->
<!-- <define name="MAG_Z_NEUTRAL" value="15"/> -->
<!-- <define name="MAG_X_SENS" value="3.57880347067" integer="16"/> -->
<!-- <define name="MAG_Y_SENS" value="3.54049742843" integer="16"/> -->
<!-- <define name="MAG_Z_SENS" value="4.0620533034" integer="16"/> -->
<define name="MAG_X_NEUTRAL" value="-86"/>
<define name="MAG_Y_NEUTRAL" value="-36"/>
<define name="MAG_Z_NEUTRAL" value="35"/>
<define name="MAG_X_SENS" value="3.98639595061" integer="16"/>
<define name="MAG_Y_SENS" value="4.00632407649" integer="16"/>
<define name="MAG_Z_SENS" value="4.17946082997" integer="16"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<!-- <define name="USE_YAW_FOR_MOTOR_ARMING" value="TRUE"/> -->
</section>
<section name="BAT">
<!-- <define name="MIN_BAT_LEVEL" value="10.8" units="V"/> -->
<define name="CRITIC_BAT_LEVEL" value="10.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.8" unit="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="10.1" unit="V"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="350"/>
<define name="GAIN_Q" value="250"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="200"/>
<define name="IGAIN_Q" value="200"/>
<define name="IGAIN_R" value="200"/>
</section>
<!-- These gains are used when the gain scheduling module is enabled (by loading it in the modules section)-->
<section name ="GAIN_SETS">
<define name="NUMBER_OF_GAINSETS" value="2"/>
<define name="SCHEDULING_VARIABLE" value="guidance_hybrid_norm_ref_airspeed"/>
<define name="SCHEDULING_POINTS" value="{4, 10}"/>
<define name="SCHEDULING_VARIABLE_FRAC" value="8"/>
<define name="PHI_P" value="{150, 150}"/>
<define name="PHI_D" value="{150, 150}"/>
<define name="PHI_I" value="{30, 30}"/>
<define name="PHI_DD" value="{0, 0}"/>
<define name="THETA_P" value="{100, 80}"/>
<define name="THETA_D" value="{100, 110}"/>
<define name="THETA_I" value="{40, 40}"/>
<define name="THETA_DD" value="{0, 0}"/>
<define name="PSI_P" value="{300, 300}"/>
<define name="PSI_D" value="{150, 150}"/>
<define name="PSI_I" value="{0, 0}"/>
<define name="PSI_DD" value="{0, 0}"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="100." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="1500" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="1500" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="1500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="300." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(7000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="150"/>
<define name="PHI_DGAIN" value="150"/>
<define name="PHI_IGAIN" value="30"/>
<define name="THETA_PGAIN" value="100"/>
<define name="THETA_DGAIN" value="100"/>
<define name="THETA_IGAIN" value="40"/>
<define name="PSI_PGAIN" value="300"/>
<define name="PSI_DGAIN" value="150"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 0"/>
<define name="THETA_DDGAIN" value=" 0"/>
<define name="PSI_DDGAIN" value=" 0"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="180." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
<!-- control effectiveness -->
<define name="G1_P" value="0.025"/>
<define name="G1_Q" value="0.0833"/>
<define name="G1_R" value="0.00588"/>
<define name="G2_R" value="0.0"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="70.0"/>
<define name="REF_ERR_Q" value="45.0"/>
<define name="REF_ERR_R" value="70.0"/>
<define name="REF_RATE_P" value="9.0"/>
<define name="REF_RATE_Q" value="16.0"/>
<define name="REF_RATE_R" value="12.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<!-- Gains for vertical navigation -->
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="200"/>
<define name="HOVER_KD" value="175"/>
<define name="HOVER_KI" value="100"/>
<define name="NOMINAL_HOVER_THROTTLE" value ="0.2"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- Delft magnetic field -->
<define name="H_X" value="0.39049610"/>
<define name="H_Y" value="0.00278894"/>
<define name="H_Z" value="0.92060036"/>
</section>
<!-- Gains for horizontal navigation-->
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="65"/>
<define name="MAX_BANK" value="RadOfDeg(55)"/>
<define name="FORWARD_MAX_BANK" value="RadOfDeg(35)"/>
<define name="USE_REF" value="FALSE"/>
</section>
<section name="NAV">
<define name="ARRIVED_AT_WAYPOINT" value="5" unit="m"/>
</section>
<section name="MISC">
<define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value="{1,1,1,1,1,1,1,1,1,1,1,1}"/>
<!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-75.0" unit="deg"/>
<define name="RC_LOST_MODE" value="MODE_AUTO2"/>
<define name="SWITCH_STICKS_FOR_RATE_CONTROL" value="TRUE"/>
<define name="AIRSPEED_ETS_I2C_DEV" value="i2c2"/>
<define name="AHRS_MAG_UPDATE_ALL_AXES" value="FALSE"/>
<define name="THRESHOLD_GROUND_DETECT" value="25.0"/>
<define name="KILL_ON_GROUND_DETECT" value="TRUE"/>
<define name="FAILSAFE_GROUND_DETECT" value="TRUE"/>
<define name="HYBRID_NAVIGATION" value="TRUE"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="ADAPTIVE_INDI" value="FALSE"/>
</section>
<modules main_freq="512">
<load name="gps_ubx_ucenter.xml"/>
<!-- The the led_safety_status module will make the Quadshot LEDs blink in certain patterns when there are safety violations-->
<load name="led_safety_status.xml">
<define name="USE_LED_BODY" value="1"/>
<define name="SAFETY_WARNING_LED" value="BODY"/>
</load>
<load name="sonar_adc.xml">
<configure name="ADC_SONAR" value="ADC_2"/>
<!-- <define name="USE_SONAR"/> -->
<define name="SENSOR_SYNC_SEND_SONAR"/>
<define name="SONAR_SCALE" value="0.0032"/>
</load>
<!--Use an airspeed sensor and get the measured airspeed in the messages-->
<!-- <load name="airspeed_ets.xml">
<define name="AIRSPEED_ETS_SYNC_SEND"/>
</load>-->
<module name="logger_spi_link"/>
<!-- <load name="AOA_adc.xml">
<configure name="ADC_AOA" value="ADC_2"/>
<define name="AOA_OFFSET" value="0"/>
<define name="AOA_FILTER" value="0"/>
<define name="USE_AOA"/>
<define name="AOA_SENS" value="2.0*M_PI/1024/4"/>
</load>-->
<!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section. Does not work with INDI -->
<!-- <module name="gain_scheduling"/> -->
</modules>
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0">
<module name="radio_control" type="spektrum">
<define name="RADIO_KILL_SWITCH" value="5"/>
<!-- Put the mode on channel AUX1-->
<define name="RADIO_MODE" value="6"/>
<!-- <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/> -->
</module>
<!-- ONLY if flashing via JTAG cable -->
<configure name="FLASH_MODE" value="SWD"/>
<configure name="BMP_PORT" value="/dev/ttyACM0" />
<!-- To use an airspeed sensor on I2C, enable I2C2-->
<!-- <define name="USE_I2C2"/> -->
<!-- If using aspirin 2.2, make sure to uncomment the following barometer configuration: -->
<!-- <configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/> -->
<configure name="LISA_M_BARO" value="BARO_BOARD_BMP085"/>
</target>
<module name="motor_mixing"/>
<module name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
<define name="USE_SERVOS_7AND8"/>
</module>
<module name="telemetry" type="xbee_api"/>
<!-- <module name="telemetry" type="transparent"/> -->
<module name="imu" type="aspirin_v2.1"/>
<module name="gps" type="ublox"/>
<!-- <module name="stabilization" type="int_quat"/> -->
<module name="stabilization" type="indi"/>
<module name="guidance" type="hybrid"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="1"/>
<define name="AHRS_USE_GPS_HEADING" value="0"/>
</module>
<module name="ins"/>
</firmware>
</airframe>
conf/airframes/examples/quadshot_178_pylons.xml
+367
View File
@@ -0,0 +1,367 @@
<!-- this is a quadshot vehicle equiped with Lisa/m2.0 and an aspirin 2.1
More information on the Quadshot can be found at transition-robotics.com -->
<airframe name="quadshot 178 pylons">
<servos driver="Pwm">
<servo name="A1" no="0" min="1000" neutral="1100" max="2000"/>
<servo name="A2" no="1" min="1000" neutral="1100" max="2000"/>
<servo name="B1" no="2" min="1000" neutral="1100" max="2000"/>
<servo name="B2" no="3" min="1000" neutral="1100" max="2000"/>
<!-- <servo name="A1" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="A2" no="1" min="1000" neutral="1000" max="2000"/>
<servo name="B1" no="2" min="1000" neutral="1000" max="2000"/>
<servo name="B2" no="3" min="1000" neutral="1000" max="2000"/>-->
<!-- <servo name="ELEVON_LEFT" no="4" min="1150" neutral="1500" max="1850"/>
<servo name="ELEVON_RIGHT" no="5" min="1150" neutral="1500" max="1850"/>-->
<servo name="ELEVON_LEFT" no="4" min="1300" neutral="1500" max="1700"/>
<servo name="ELEVON_RIGHT" no="5" min="1300" neutral="1500" max="1700"/>
</servos>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="A1" value="motor_mixing.commands[0]"/>
<set servo="A2" value="motor_mixing.commands[1]"/>
<set servo="B1" value="motor_mixing.commands[2]"/>
<set servo="B2" value="motor_mixing.commands[3]"/>
<!-- Mode dependent actuator laws for the elevons. The elevons act different in rc attitude flight mode-->
<!-- First the correct feedback is stored in variables -->
<let var="aileron_feedback_left" value="-@YAW"/>
<let var="aileron_feedback_right" value="-@YAW"/>
<let var="elevator_feedback_left" value="-@PITCH"/>
<let var="elevator_feedback_right" value="+@PITCH"/>
<!-- Here the gains are defined for the two feedback cases, hover and forward-->
<let var="hover_left" value="15*$aileron_feedback_left"/>
<let var="hover_right" value="15*$aileron_feedback_right"/>
<!-- if using INDI -->
<!-- <let var="forward_left" value="aileron_gain*$aileron_feedback_left+elevator_gain*$elevator_feedback_left"/>
<let var="forward_right" value="aileron_gain*$aileron_feedback_right+elevator_gain*$elevator_feedback_right"/>-->
<!-- if using PID with gain scheduling -->
<let var="forward_left" value="15*$aileron_feedback_left + 0*$elevator_feedback_left"/>
<let var="forward_right" value="15*$aileron_feedback_right + 0*$elevator_feedback_right"/>
<!-- This statement tells the autopilot to use the hover feedback if in mode attitude direct and to use the forward feedback in all other cases-->
<set servo="ELEVON_LEFT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_left : $forward_left" />
<set servo="ELEVON_RIGHT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_right : $forward_right" />
<!-- <set servo="ELEVON_RIGHT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? 9000 : -9000" /> -->
</command_laws>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<define name="PITCH_COEF" value="{-256, -256, 256, 256 }"/>
<define name="ROLL_COEF" value="{ 256, -256, 109, -109 }"/>
<define name="YAW_COEF" value="{ 256, -256, -109, 109 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
<!-- <define name="PITCH_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="ROLL_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="YAW_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="THRUST_COEF" value="{ 0, 0, 0, 0 }"/>-->
</section>
<!-- If you got a caliberation XML document from your IMU supplier, import this in the IMU section -->
<!-- Note that is better to have *no* caliberation file at all, than a one with incorrect caliberation values.
The default factory values are most of the time perfectly acceptable, so by default we will not use the caliberation file -->
<section name="IMU" prefix="IMU_">
<!-- Use default driver values for gyro -->
<!-- IMU calibration, make sure to calibrate the IMU properly before flight, see the wiki for more info-->
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<define name="ACCEL_X_SENS" value="4.86487566223" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.89957269597" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.82616398266" integer="16"/>
<define name="MAG_X_NEUTRAL" value="104"/>
<define name="MAG_Y_NEUTRAL" value="9"/>
<define name="MAG_Z_NEUTRAL" value="15"/>
<define name="MAG_X_SENS" value="3.57880347067" integer="16"/>
<define name="MAG_Y_SENS" value="3.54049742843" integer="16"/>
<define name="MAG_Z_SENS" value="4.0620533034" integer="16"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<!-- <define name="USE_YAW_FOR_MOTOR_ARMING" value="TRUE"/> -->
</section>
<section name="BAT">
<define name="MIN_BAT_LEVEL" value="10.4" units="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="350"/>
<define name="GAIN_Q" value="250"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="200"/>
<define name="IGAIN_Q" value="200"/>
<define name="IGAIN_R" value="200"/>
</section>
<!-- These gains are used when the gain scheduling module is enabled (by loading it in the modules section)-->
<section name ="GAIN_SETS">
<define name="NUMBER_OF_GAINSETS" value="2"/>
<define name="SCHEDULING_VARIABLE" value="guidance_hybrid_norm_ref_airspeed"/>
<define name="SCHEDULING_POINTS" value="{4, 10}"/>
<define name="SCHEDULING_VARIABLE_FRAC" value="8"/>
<define name="PHI_P" value="{150, 150}"/>
<define name="PHI_D" value="{150, 150}"/>
<define name="PHI_I" value="{30, 30}"/>
<define name="PHI_DD" value="{0, 0}"/>
<define name="THETA_P" value="{100, 100}"/>
<define name="THETA_D" value="{100, 100}"/>
<define name="THETA_I" value="{40, 40}"/>
<define name="THETA_DD" value="{0, 0}"/>
<define name="PSI_P" value="{300, 300}"/>
<define name="PSI_D" value="{150, 150}"/>
<define name="PSI_I" value="{0, 0}"/>
<define name="PSI_DD" value="{0, 0}"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="180." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="1500" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="1500" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="1500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="300." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(7000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="150"/>
<define name="PHI_DGAIN" value="150"/>
<define name="PHI_IGAIN" value="30"/>
<define name="THETA_PGAIN" value="100"/>
<define name="THETA_DGAIN" value="100"/>
<define name="THETA_IGAIN" value="40"/>
<define name="PSI_PGAIN" value="300"/>
<define name="PSI_DGAIN" value="150"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 0"/>
<define name="THETA_DDGAIN" value=" 0"/>
<define name="PSI_DDGAIN" value=" 0"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="180." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
<!-- control effectiveness -->
<define name="CONTROL_EFFECTIVENESS_P" value="40.0"/>
<define name="CONTROL_EFFECTIVENESS_Q" value="9.0"/>
<define name="CONTROL_EFFECTIVENESS_R" value="170.0"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="70.0"/>
<define name="REF_ERR_Q" value="90.0"/>
<define name="REF_ERR_R" value="70.0"/>
<define name="REF_RATE_P" value="12.0"/>
<define name="REF_RATE_Q" value="12.0"/>
<define name="REF_RATE_R" value="12.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.37"/>
<define name="FILT_OMEGA_R" value="40.0"/>
<define name="FILT_ZETA_R" value="0.5"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
</section>
<!-- Gains for vertical navigation -->
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="200"/>
<define name="HOVER_KD" value="175"/>
<define name="HOVER_KI" value="100"/>
<define name="NOMINAL_HOVER_THROTTLE" value ="0.2"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- Delft magnetic field -->
<define name="H_X" value="0.39049610"/>
<define name="H_Y" value="0.00278894"/>
<define name="H_Z" value="0.92060036"/>
</section>
<!-- Gains for horizontal navigation-->
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="65"/>
<define name="MAX_BANK" value="RadOfDeg(55)"/>
<define name="FORWARD_MAX_BANK" value="RadOfDeg(35)"/>
<define name="USE_REF" value="FALSE"/>
</section>
<section name="NAV">
<define name="ARRIVED_AT_WAYPOINT" value="5" unit="m"/>
</section>
<section name="MISC">
<!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-85.0" unit="deg"/>
<define name="RC_LOST_MODE" value="MODE_AUTO2"/>
<define name="SWITCH_STICKS_FOR_RATE_CONTROL" value="TRUE"/>
<define name="AIRSPEED_ETS_I2C_DEV" value="i2c2"/>
<define name="AHRS_MAG_UPDATE_ALL_AXES" value="FALSE"/>
<define name="THRESHOLD_GROUND_DETECT" value="25.0"/>
<define name="KILL_ON_GROUND_DETECT" value="TRUE"/>
<define name="FAILSAFE_GROUND_DETECT" value="TRUE"/>
<define name="HYBRID_NAVIGATION" value="TRUE"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="ADAPTIVE_INDI" value="TRUE"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
</section>
<modules main_freq="512">
<load name="gps_ubx_ucenter.xml"/>
<!-- The the led_safety_status module will make the Quadshot LEDs blink in certain patterns when there are safety violations-->
<load name="led_safety_status.xml">
<define name="USE_LED_BODY" value="1"/>
<define name="SAFETY_WARNING_LED" value="BODY"/>
</load>
<!--Use an airspeed sensor and get the measured airspeed in the messages-->
<!-- <load name="airspeed_ets.xml">
<define name="AIRSPEED_ETS_SYNC_SEND"/>
</load>-->
<!-- <load name="high_speed_logger_spi_link.xml"/> -->
<!-- <load name="AOA_adc.xml">
<configure name="ADC_AOA" value="ADC_2"/>
<define name="AOA_OFFSET" value="0"/>
<define name="AOA_FILTER" value="0"/>
<define name="USE_AOA"/>
<define name="AOA_SENS" value="2.0*M_PI/1024/4"/>
</load>-->
<!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section -->
<!-- <load name="gain_scheduling.xml"/> -->
</modules>
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0">
<subsystem name="radio_control" type="spektrum">
<define name="RADIO_KILL_SWITCH" value="6"/>
<!-- Put the mode on channel AUX1-->
<!-- <define name="RADIO_MODE" value="RADIO_AUX1"/>-->
<!-- <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/> -->
</subsystem>
<!-- ONLY if flashing via JTAG cable -->
<configure name="FLASH_MODE" value="JTAG"/>
<configure name="BMP_PORT" value="/dev/ttyACM0" />
<!-- To use an airspeed sensor on I2C, enable I2C2-->
<!-- <define name="USE_I2C2"/> -->
<!-- If using aspirin 2.2, make sure to uncomment the following barometer configuration: -->
<!-- <configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/> -->
</target>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
<define name="USE_SERVOS_7AND8"/>
</subsystem>
<subsystem name="telemetry" type="xbee_api"/>
<!-- <subsystem name="telemetry" type="transparent"/> -->
<subsystem name="imu" type="aspirin_v2.1"/>
<subsystem name="gps" type="ublox"/>
<!-- <subsystem name="stabilization" type="int_quat"/> -->
<subsystem name="stabilization" type="indi"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="1"/>
<define name="AHRS_USE_GPS_HEADING" value="0"/>
</subsystem>
<subsystem name="ins"/>
</firmware>
</airframe>
conf/airframes/examples/quadshot_asp21_spektrum.xml
+203 -77
View File
@@ -4,12 +4,18 @@
<airframe name="quadshot aspirin 2.1 spektrum">
<servos driver="Pwm">
<servo name="A1" no="0" min="1000" neutral="1050" max="2000"/>
<servo name="A2" no="1" min="1000" neutral="1050" max="2000"/>
<servo name="B1" no="2" min="1000" neutral="1050" max="2000"/>
<servo name="B2" no="3" min="1000" neutral="1050" max="2000"/>
<servo name="ELEVON_LEFT" no="4" min="1000" neutral="1500" max="2000"/>
<servo name="ELEVON_RIGHT" no="5" min="1000" neutral="1500" max="2000"/>
<servo name="A1" no="0" min="1000" neutral="1100" max="2000"/>
<servo name="A2" no="1" min="1000" neutral="1100" max="2000"/>
<servo name="B1" no="2" min="1000" neutral="1100" max="2000"/>
<servo name="B2" no="3" min="1000" neutral="1100" max="2000"/>
<!-- <servo name="A1" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="A2" no="1" min="1000" neutral="1000" max="2000"/>
<servo name="B1" no="2" min="1000" neutral="1000" max="2000"/>
<servo name="B2" no="3" min="1000" neutral="1000" max="2000"/>-->
<!-- <servo name="ELEVON_LEFT" no="4" min="1150" neutral="1500" max="1850"/>
<servo name="ELEVON_RIGHT" no="5" min="1150" neutral="1500" max="1850"/>-->
<servo name="ELEVON_LEFT" no="4" min="1300" neutral="1500" max="1700"/>
<servo name="ELEVON_RIGHT" no="5" min="1300" neutral="1500" max="1700"/>
</servos>
<commands>
@@ -26,24 +32,30 @@
<set servo="B1" value="motor_mixing.commands[2]"/>
<set servo="B2" value="motor_mixing.commands[3]"/>
<!-- Mode dependent actuator laws for the elevons. The elevons act different in rc attitude flight mode-->
<!-- First the correct feedback is stored in variables -->
<let var="aileron_feedback_left" value="@YAW"/>
<let var="aileron_feedback_right" value="@YAW"/>
<!-- Mode dependent actuator laws for the elevons. The elevons act different in rc attitude flight mode-->
<!-- First the correct feedback is stored in variables -->
<let var="aileron_feedback_left" value="-@YAW"/>
<let var="aileron_feedback_right" value="-@YAW"/>
<let var="elevator_feedback_left" value="+@PITCH"/>
<let var="elevator_feedback_right" value="-@PITCH"/>
<let var="elevator_feedback_left" value="-@PITCH"/>
<let var="elevator_feedback_right" value="+@PITCH"/>
<!-- Here the gains are defined for the two feedback cases, hover and forward-->
<let var="hover_left" value="3*$aileron_feedback_left"/>
<let var="hover_right" value="3*$aileron_feedback_right"/>
<!-- Here the gains are defined for the two feedback cases, hover and forward-->
<let var="hover_left" value="15*$aileron_feedback_left"/>
<let var="hover_right" value="15*$aileron_feedback_right"/>
<let var="forward_left" value="6*$aileron_feedback_left+4*$elevator_feedback_left"/>
<let var="forward_right" value="6*$aileron_feedback_right+4*$elevator_feedback_right"/>
<!-- if using INDI -->
<!-- <let var="forward_left" value="aileron_gain*$aileron_feedback_left+elevator_gain*$elevator_feedback_left"/>
<let var="forward_right" value="aileron_gain*$aileron_feedback_right+elevator_gain*$elevator_feedback_right"/>-->
<!-- if using PID with gain scheduling -->
<let var="forward_left" value="5*$aileron_feedback_left + 4*$elevator_feedback_left"/>
<let var="forward_right" value="5*$aileron_feedback_right + 4*$elevator_feedback_right"/>
<!-- This statement tells the autopilot to use the hover feedback if in mode attitude direct and to use the forward feedback in all other cases-->
<set servo="ELEVON_LEFT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_left : $forward_left" />
<set servo="ELEVON_RIGHT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_right : $forward_right" />
<!-- <set servo="ELEVON_RIGHT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? 9000 : -9000" /> -->
</command_laws>
<section name="MIXING" prefix="MOTOR_MIXING_">
@@ -56,6 +68,10 @@
<define name="ROLL_COEF" value="{ 256, -256, 256, -256 }"/>
<define name="YAW_COEF" value="{ -256, 256, 128, -128 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
<!-- <define name="PITCH_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="ROLL_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="YAW_COEF" value="{ 0, 0, 0, 0 }"/>
<define name="THRUST_COEF" value="{ 0, 0, 0, 0 }"/>-->
</section>
<!-- If you got a caliberation XML document from your IMU supplier, import this in the IMU section -->
@@ -74,12 +90,19 @@
<define name="ACCEL_Y_SENS" value="4.89957269597" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.82616398266" integer="16"/>
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<define name="MAG_X_SENS" value="4.19385009207" integer="16"/>
<define name="MAG_Y_SENS" value="4.32306399648" integer="16"/>
<define name="MAG_Z_SENS" value="4.63243801309" integer="16"/>
<!-- <define name="MAG_X_NEUTRAL" value="104"/> -->
<!-- <define name="MAG_Y_NEUTRAL" value="9"/> -->
<!-- <define name="MAG_Z_NEUTRAL" value="15"/> -->
<!-- <define name="MAG_X_SENS" value="3.57880347067" integer="16"/> -->
<!-- <define name="MAG_Y_SENS" value="3.54049742843" integer="16"/> -->
<!-- <define name="MAG_Z_SENS" value="4.0620533034" integer="16"/> -->
<define name="MAG_X_NEUTRAL" value="-86"/>
<define name="MAG_Y_NEUTRAL" value="-36"/>
<define name="MAG_Z_NEUTRAL" value="35"/>
<define name="MAG_X_SENS" value="3.98639595061" integer="16"/>
<define name="MAG_Y_SENS" value="4.00632407649" integer="16"/>
<define name="MAG_Z_SENS" value="4.17946082997" integer="16"/>
</section>
<section name="IMU" prefix="IMU_">
@@ -91,31 +114,47 @@
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_RATE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
<!-- <define name="USE_YAW_FOR_MOTOR_ARMING" value="TRUE"/> -->
</section>
<section name="BAT">
<define name="MIN_BAT_LEVEL" value="10.4" units="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<!-- <define name="MIN_BAT_LEVEL" value="10.8" units="V"/> -->
<define name="CRITIC_BAT_LEVEL" value="10.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.8" unit="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="10.1" unit="V"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="350"/>
<define name="GAIN_Q" value="250"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="200"/>
<define name="IGAIN_Q" value="200"/>
<define name="IGAIN_R" value="200"/>
</section>
<!-- These gains are used when the gain scheduling module is enabled (by loading it in the modules section)-->
<section name ="GAIN_SETS">
<define name="NUMBER_OF_GAINSETS" value="2"/>
<define name="SCHEDULING_VARIABLE" value="(radio.values[COMMAND_THRUST]+ transition_status"/>
<define name="SCHEDULING_POINTS" value="{1000, 6000}"/>
<define name="SCHEDULING_VARIABLE_FRAC" value="0"/>
<define name="SCHEDULING_VARIABLE" value="guidance_hybrid_norm_ref_airspeed"/>
<define name="SCHEDULING_POINTS" value="{4, 10}"/>
<define name="SCHEDULING_VARIABLE_FRAC" value="8"/>
<define name="PHI_P" value="{230, 230}"/>
<define name="PHI_D" value="{170, 170}"/>
<define name="PHI_P" value="{150, 150}"/>
<define name="PHI_D" value="{150, 150}"/>
<define name="PHI_I" value="{30, 30}"/>
<define name="PHI_DD" value="{0, 0}"/>
<define name="THETA_P" value="{200, 300}"/>
<define name="THETA_D" value="{100, 50}"/>
<define name="THETA_P" value="{100, 80}"/>
<define name="THETA_D" value="{100, 110}"/>
<define name="THETA_I" value="{40, 40}"/>
<define name="THETA_DD" value="{0, 0}"/>
@@ -130,7 +169,7 @@
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="SP_MAX_R" value="100." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
@@ -147,17 +186,17 @@
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="1500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="300." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(7000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="230"/>
<define name="PHI_DGAIN" value="170"/>
<define name="PHI_PGAIN" value="150"/>
<define name="PHI_DGAIN" value="150"/>
<define name="PHI_IGAIN" value="30"/>
<define name="THETA_PGAIN" value="300"/>
<define name="THETA_DGAIN" value="50"/>
<define name="THETA_PGAIN" value="100"/>
<define name="THETA_DGAIN" value="100"/>
<define name="THETA_IGAIN" value="40"/>
<define name="PSI_PGAIN" value="300"/>
@@ -170,71 +209,155 @@
<define name="PSI_DDGAIN" value=" 0"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="60." unit="deg"/>
<define name="SP_MAX_R" value="180." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
<!-- control effectiveness -->
<define name="G1_P" value="0.025"/>
<define name="G1_Q" value="0.0833"/>
<define name="G1_R" value="0.00588"/>
<define name="G2_R" value="0.0"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="70.0"/>
<define name="REF_ERR_Q" value="45.0"/>
<define name="REF_ERR_R" value="70.0"/>
<define name="REF_RATE_P" value="9.0"/>
<define name="REF_RATE_Q" value="16.0"/>
<define name="REF_RATE_R" value="12.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<!-- Gains for vertical navigation -->
<!-- Gains for vertical navigation -->
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="200"/>
<define name="HOVER_KD" value="175"/>
<define name="HOVER_KI" value="72"/>
<define name="NOMINAL_HOVER_THROTTLE" value ="0.4"/>
<define name="HOVER_KI" value="100"/>
<define name="NOMINAL_HOVER_THROTTLE" value ="0.2"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value=" 0.47577"/>
<define name="H_Y" value=" 0.11811"/>
<define name="H_Z" value=" 0.87161"/>
<!-- Delft magnetic field -->
<define name="H_X" value="0.39049610"/>
<define name="H_Y" value="0.00278894"/>
<define name="H_Z" value="0.92060036"/>
</section>
<!-- Gains for horizontal navigation-->
<!-- Gains for horizontal navigation-->
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="65"/>
<define name="MAX_BANK" value="RadOfDeg(55)"/>
<define name="FORWARD_MAX_BANK" value="RadOfDeg(35)"/>
<define name="USE_REF" value="FALSE"/>
</section>
<section name="NAV">
<define name="ARRIVED_AT_WAYPOINT" value="5" unit="m"/>
</section>
<section name="MISC">
<define name="REF_QUAT_INFINITESIMAL_STEP" value="TRUE"/>
<define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value="{1,1,1,1,1,1,1,1,1,1,1,1}"/>
<!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-82.0" unit="deg"/>
<define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value=" {1,1,-1,-1,1,-1,1,1,1,1,1,1}"/>
<define name="TRANSITION_MAX_OFFSET" value="-75.0" unit="deg"/>
<define name="RC_LOST_MODE" value="MODE_AUTO2"/>
<define name="SWITCH_STICKS_FOR_RATE_CONTROL" value="TRUE"/>
<define name="AIRSPEED_ETS_I2C_DEV" value="i2c2"/>
<define name="AHRS_MAG_UPDATE_ALL_AXES" value="FALSE"/>
<define name="THRESHOLD_GROUND_DETECT" value="25.0"/>
<define name="KILL_ON_GROUND_DETECT" value="TRUE"/>
<define name="FAILSAFE_GROUND_DETECT" value="TRUE"/>
<define name="HYBRID_NAVIGATION" value="TRUE"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="ADAPTIVE_INDI" value="FALSE"/>
</section>
<modules main_freq="512">
<module name="gps" type="ubx_ucenter"/>
<load name="gps_ubx_ucenter.xml"/>
<!-- The the led_safety_status module will make the Quadshot LEDs blink in certain patterns when there are safety violations-->
<module name="led_safety_status">
<define name="USE_LED_BODY" value="1"/>
<define name="SAFETY_WARNING_LED" value="BODY"/>
</module>
<load name="led_safety_status.xml">
<define name="USE_LED_BODY" value="1"/>
<define name="SAFETY_WARNING_LED" value="BODY"/>
</load>
<!--Use an airspeed sensor and get the measured airspeed in the messages-->
<module name="airspeed_ets">
<configure name="AIRSPEED_ETS_I2C_DEV" value="i2c2"/>
<load name="sonar_adc.xml">
<configure name="ADC_SONAR" value="ADC_2"/>
<!-- <define name="USE_SONAR"/> -->
<define name="SENSOR_SYNC_SEND_SONAR"/>
<define name="SONAR_SCALE" value="0.0032"/>
</load>
<!--Use an airspeed sensor and get the measured airspeed in the messages-->
<!-- <load name="airspeed_ets.xml">
<define name="AIRSPEED_ETS_SYNC_SEND"/>
</module>
</load>-->
<!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section -->
<!--module name="gain_scheduling"/-->
<module name="logger_spi_link"/>
<!-- <load name="AOA_adc.xml">
<configure name="ADC_AOA" value="ADC_2"/>
<define name="AOA_OFFSET" value="0"/>
<define name="AOA_FILTER" value="0"/>
<define name="USE_AOA"/>
<define name="AOA_SENS" value="2.0*M_PI/1024/4"/>
</load>-->
<!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section. Does not work with INDI -->
<!-- <module name="gain_scheduling"/> -->
</modules>
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0">
<module name="radio_control" type="spektrum">
<!-- Put the mode on channel AUX1-->
<define name="RADIO_MODE" value="RADIO_AUX1"/>
<!-- <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/> -->
</module>
<module name="radio_control" type="spektrum">
<define name="RADIO_KILL_SWITCH" value="5"/>
<!-- Put the mode on channel AUX1-->
<define name="RADIO_MODE" value="6"/>
<!-- <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/> -->
</module>
<!-- ONLY if flashing via JTAG cable -->
<configure name="FLASH_MODE" value="SWD"/>
<configure name="BMP_PORT" value="/dev/ttyACM0" />
<!-- To use an airspeed sensor on I2C, enable I2C2-->
<!-- <define name="USE_I2C2"/> -->
<define name="USE_AIRSPEED" value="TRUE"/>
<!-- on aspirin 2.1 the ms5611 baro is not connected via SPI, use BMP085 on Lisa/M board instead -->
<configure name="LISA_M_BARO" value="BARO_BOARD_BMP085"/>
<!-- If using aspirin 2.2, make sure to uncomment the following barometer configuration: -->
<!-- <configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/> -->
<configure name="LISA_M_BARO" value="BARO_BOARD_BMP085"/>
</target>
<module name="motor_mixing"/>
@@ -244,13 +367,16 @@
</module>
<module name="telemetry" type="xbee_api"/>
<!-- <module name="telemetry" type="transparent"/> -->
<module name="imu" type="aspirin_v2.1"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="int_quat"/>
<!-- <module name="stabilization" type="int_quat"/> -->
<module name="stabilization" type="indi"/>
<module name="guidance" type="hybrid"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<!--define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" value="TRUE"/-->
<configure name="USE_MAGNETOMETER" value="1"/>
<define name="AHRS_USE_GPS_HEADING" value="0"/>
</module>
<module name="ins"/>
conf/conf_example.xml
+3 -3
View File
@@ -183,7 +183,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_rate.xml] settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml settings/nps.xml"
settings_modules="modules/gps_ubx_ucenter.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml"
gui_color="#710080"
/>
<aircraft
@@ -281,8 +281,8 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml"
settings_modules="modules/gps_ubx_ucenter.xml"
settings="settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/gps.xml"
gui_color="blue"
/>
<aircraft
conf/conf_tests.xml
+3 -3
View File
@@ -403,7 +403,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_att_int.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/geo_mag.xml"
gui_color="blue"
/>
<aircraft
@@ -435,8 +435,8 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml"
settings_modules="modules/gps_ubx_ucenter.xml"
settings="settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/gps.xml"
gui_color="blue"
/>
<aircraft
conf/firmwares/subsystems/rotorcraft/guidance_hybrid.makefile
+2
View File
@@ -0,0 +1,2 @@
$(TARGET).CFLAGS += -DHYBRID_NAVIGATION=TRUE
$(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_hybrid.c
conf/flight_plans/DB/db_quadshot_delft.xml
+101
View File
@@ -0,0 +1,101 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<!-- Valkenburg -->
<!-- <flight_plan alt="40" ground_alt="0" lat0="52.170867" lon0="4.412194" max_dist_from_home="1000" name="Transitioning test" security_height="2"> -->
<!-- Delft -->
<flight_plan alt="40" ground_alt="0" lat0="51.979438" lon0="4.390148" max_dist_from_home="1000" name="Transitioning test" security_height="2">
<header>
#include "autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/electrical.h"
#include "subsystems/actuators.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="STDBY" x="-2.0" y="-5.0"/>
<waypoint name="p1" x="53.9" y="9.6"/>
<waypoint name="p2" x="-64.4" y="31.4"/>
<waypoint name="p3" x="-50.7" y="66.2"/>
<waypoint name="p4" x="60.0" y="41.3"/>
</waypoints>
<blocks>
<block name="Wait GPS">
<call fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<call fun="NavResurrect()"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 5.0" deroute="Climb"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<attitude pitch="0" roll="0" throttle="0.5" until="FALSE" vmode="throttle"/>
</block>
<block name="Climb">
<exception cond="stateGetPositionEnu_f()->z > 20.0" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<stay climb="2" vmode="climb" wp="STDBY"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<stay wp="p1"/>
</block>
<block name="go_p2_p1">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="pylon_racing">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<go wp="p2"/>
<go wp="p3"/>
<go wp="p4"/>
<go wp="p1"/>
<deroute block="stay_p1"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<call fun="NavSetWaypointHere(WP_STDBY)"/>
</block>
<block name="land">
<!-- <set value="FALSE" var="force_forward_flight"/> -->
<go wp="STDBY"/>
</block>
<block name="descent">
<exception cond="8.0 > stateGetPositionEnu_f()->z" deroute="descent_slow"/>
<stay climb="-1" vmode="climb" wp="STDBY"/>
</block>
<block name="descent_slow">
<exception cond="4.0 > stateGetPositionEnu_f()->z" deroute="flare"/>
<stay climb="-1" vmode="climb" wp="STDBY"/>
</block>
<block name="flare">
<exception cond="0.5 > sonar_adc.distance" deroute="landed"/>
<attitude pitch="0" roll="0" throttle="0.13" vmode="throttle"/>
</block>
<block name="landed">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/quadshot_delft.xml
+111
View File
@@ -0,0 +1,111 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<!-- Valkenburg -->
<!-- <flight_plan alt="40" ground_alt="0" lat0="52.170867" lon0="4.412194" max_dist_from_home="1000" name="Transitioning test" security_height="2"> -->
<!-- Delft -->
<flight_plan alt="40" ground_alt="0" lat0="51.979438" lon0="4.390148" max_dist_from_home="1000" name="Transitioning test" security_height="2">
<header>
#include "autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/electrical.h"
#include "subsystems/actuators.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="STDBY" x="-2.0" y="-5.0"/>
<waypoint name="p1" x="53.9" y="9.6"/>
<waypoint name="p2" x="-64.4" y="31.4"/>
<waypoint name="p3" x="-50.7" y="66.2"/>
<waypoint name="p4" x="60.0" y="41.3"/>
</waypoints>
<blocks>
<block name="Wait GPS">
<call fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="FALSE" var="force_forward_flight"/>
<call fun="NavResurrect()"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 5.0" deroute="Climb"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<set value="FALSE" var="force_forward_flight"/>
<attitude pitch="0" roll="0" throttle="0.5" until="FALSE" vmode="throttle"/>
</block>
<block name="Climb">
<exception cond="stateGetPositionEnu_f()->z > 20.0" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<set value="FALSE" var="force_forward_flight"/>
<stay climb="2" vmode="climb" wp="STDBY"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<set value="FALSE" var="force_forward_flight"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<set value="FALSE" var="force_forward_flight"/>
<stay wp="p1"/>
</block>
<block name="go_p2_p1">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<set value="FALSE" var="force_forward_flight"/>
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="pylon_racing">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<set value="FALSE" var="force_forward_flight"/>
<go wp="p2"/>
<go wp="p3"/>
<go wp="p4"/>
<go wp="p1"/>
<deroute block="stay_p1"/>
</block>
<block name="pylon_racing_forward">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<set value="TRUE" var="force_forward_flight"/>
<go wp="p2"/>
<go wp="p3"/>
<go wp="p4"/>
<set value="FALSE" var="force_forward_flight"/>
<go wp="p1"/>
<deroute block="stay_p1"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="FALSE" var="force_forward_flight"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
</block>
<block name="land">
<set value="FALSE" var="force_forward_flight"/>
<go wp="STDBY"/>
</block>
<block name="descent">
<exception cond="8.0 > stateGetPositionEnu_f()->z" deroute="descent_slow"/>
<stay climb="-1" vmode="climb" wp="STDBY"/>
</block>
<block name="descent_slow">
<exception cond="2.0 > stateGetPositionEnu_f()->z" deroute="flare"/>
<stay climb="-1" vmode="climb" wp="STDBY"/>
</block>
<block name="flare">
<exception cond="0.3 > sonar_adc.distance" deroute="landed"/>
<attitude pitch="0" roll="0" throttle="0.13" vmode="throttle"/>
</block>
<block name="landed">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/quadshot_land_takeoff_again.xml
+104
View File
@@ -0,0 +1,104 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="40" ground_alt="0" lat0="52.170867" lon0="4.412194" max_dist_from_home="1000" name="Transitioning test" security_height="2">
<!-- <flight_plan alt="40" ground_alt="0" lat0="51.979438" lon0="4.390148" max_dist_from_home="1000" name="Transitioning test" security_height="2"> -->
<header>
#include "autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/electrical.h"
#include "subsystems/actuators.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="STDBY" x="0.0" y="0.0"/>
<waypoint name="REMOTE_LANDING" x="-64.4" y="31.4"/>
</waypoints>
<blocks>
<block name="Wait GPS">
<call fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<call fun="NavResurrect()"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Climb"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<attitude pitch="0" roll="0" throttle="0.4" until="FALSE" vmode="throttle"/>
</block>
<block name="Climb">
<exception cond="stateGetPositionEnu_f()->z > 20.0" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_STDBY)"/>
<stay climb="2" vmode="climb" wp="STDBY"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<stay wp="STDBY" until="stage_time > 5"/>
</block>
<block name="Fly remote location">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<go wp="REMOTE_LANDING"/>
</block>
<block name="Remote landing">
<exception cond="8.0 > stateGetPositionEnu_f()->z" deroute="descent_slow remote"/>
<stay climb="-1" vmode="climb" wp="REMOTE_LANDING"/>
</block>
<block name="descent_slow remote">
<exception cond="2.0 > stateGetPositionEnu_f()->z" deroute="flare remote"/>
<stay climb="-0.5" vmode="climb" wp="REMOTE_LANDING"/>
</block>
<block name="flare remote">
<exception cond="0.3 > sonar_adc.distance" deroute="landed remote"/>
<attitude pitch="0" roll="0" throttle="0.13" vmode="throttle"/>
</block>
<block name="landed remote">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine2">
<call fun="NavSetWaypointHere(WP_REMOTE_LANDING)"/>
<call fun="NavResurrect()"/>
</block>
<block name="Takeoff2" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Climb2"/>
<attitude pitch="0" roll="0" throttle="0.4" until="FALSE" vmode="throttle"/>
</block>
<block name="Climb2">
<exception cond="stateGetPositionEnu_f()->z > 20.0" deroute="Remote hover"/>
<call fun="NavSetWaypointHere(WP_REMOTE_LANDING)"/>
<stay climb="2" vmode="climb" wp="REMOTE_LANDING"/>
</block>
<block name="Remote hover" strip_button="Standby" strip_icon="home.png">
<exception cond="radio_control.status > RC_OK" deroute="land"/>
<stay wp="REMOTE_LANDING" until="stage_time > 5"/>
</block>
<block name="land">
<go wp="STDBY"/>
</block>
<block name="descent">
<exception cond="8.0 > stateGetPositionEnu_f()->z" deroute="descent_slow"/>
<stay climb="-1" vmode="climb" wp="STDBY"/>
</block>
<block name="descent_slow">
<exception cond="2.0 > stateGetPositionEnu_f()->z" deroute="flare"/>
<stay climb="-0.5" vmode="climb" wp="STDBY"/>
</block>
<block name="flare">
<exception cond="0.3 > sonar_adc.distance" deroute="landed"/>
<attitude pitch="0" roll="0" throttle="0.13" vmode="throttle"/>
</block>
<block name="landed">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/gcs/DB/db_gcs_layout.xml
+30 -17
View File
@@ -7,9 +7,8 @@
<widget NAME="alarms" size="500"/>
</rows>
<rows>
<widget NAME="aircraft" size="258"/>
<widget NAME="altgraph" size="100"/>
<widget NAME="map2d" size="372">
<widget NAME="aircraft" size="318"/>
<widget NAME="map2d" size="418">
<papget type="message_field" display="text" x="717" y="45">
<property name="scale" value="0.0000019"/>
<property name="field" value="ROTORCRAFT_FP:vup"/>
@@ -17,20 +16,6 @@
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="756" y="315">
<property name="scale" value="0.01"/>
<property name="field" value="GPS_INT:pacc"/>
<property name="format" value="GPS Acc %.2f m"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="644" y="317">
<property name="scale" value="1."/>
<property name="field" value="GPS_INT:numsv"/>
<property name="format" value="GPS SV %.0f"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="gauge" x="41" y="291">
<property name="scale" value="1."/>
<property name="field" value="ROTORCRAFT_FP:thrust"/>
@@ -46,6 +31,34 @@
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="756" y="315">
<property name="scale" value="0.01"/>
<property name="field" value="GPS_INT:pacc"/>
<property name="format" value="GPS Acc %.2f m"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="786" y="100">
<property name="scale" value="0.0039063"/>
<property name="field" value="ROTORCRAFT_FP:up"/>
<property name="format" value="Up %.2f m"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="727" y="72">
<property name="scale" value="0.001"/>
<property name="field" value="GPS_INT:hmsl"/>
<property name="format" value="GPS HMSL %.2f m"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
<papget type="message_field" display="text" x="644" y="317">
<property name="scale" value="1."/>
<property name="field" value="GPS_INT:numsv"/>
<property name="format" value="GPS SV %.0f"/>
<property name="size" value="15."/>
<property name="color" value="yellow"/>
</papget>
</widget>
</rows>
</columns>
conf/gcs/horizontal.xml
+11 -13
View File
@@ -1,14 +1,12 @@
<!DOCTYPE layout SYSTEM "layout.dtd">
<layout width="1024" height="768">
<rows>
<widget size="500" name="map2d"/>
<columns>
<rows size="375">
<widget size="200" name="strips"/>
</rows>
<widget size="400" name="aircraft"/>
<widget name="alarms"/>
</columns>
</rows>
<layout width="1855" height="1056">
<rows>
<widget NAME="map2d" size="500"/>
<columns>
<rows SIZE="375">
<widget NAME="strips" size="548"/>
</rows>
<widget NAME="aircraft" size="1002"/>
<widget NAME="alarms" size="464"/>
</columns>
</rows>
</layout>
conf/modules/sonar_adc.xml
+1 -1
View File
@@ -17,7 +17,7 @@
</header>
<init fun="sonar_adc_init()"/>
<periodic fun="sonar_adc_read()" freq="10"/>
<periodic fun="sonar_adc_read()" freq="20"/>
<makefile target="ap|sim">
<file name="sonar_adc.c"/>
conf/settings/control/gain_scheduling.xml
+16
View File
@@ -0,0 +1,16 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="gain_set_2">
<dl_setting var="gainlibrary[1].p.x" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="phi_p"/>
<dl_setting var="gainlibrary[1].i.x" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="phi_i"/>
<dl_setting var="gainlibrary[1].d.x" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="phi_d"/>
<dl_setting var="gainlibrary[1].p.y" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="theta_p"/>
<dl_setting var="gainlibrary[1].i.y" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="theta_i"/>
<dl_setting var="gainlibrary[1].d.y" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="theta_d"/>
<dl_setting var="gainlibrary[1].p.z" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="psi_p"/>
<dl_setting var="gainlibrary[1].i.z" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="psi_i"/>
<dl_setting var="gainlibrary[1].d.z" min="0" step="1" max="1000" module="modules/gain_scheduling/gain_scheduling" shortname="psi_d"/>
</dl_settings>
</dl_settings>
</settings>
conf/settings/control/hybrid_guidance.xml
+15
View File
@@ -0,0 +1,15 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="Hybrid Guidance">
<dl_setting var="max_airspeed" MIN="4" STEP="1" MAX="15" module="guidance/guidance_hybrid"/>
<dl_setting var="wind_gain" MIN="1" STEP="1" MAX="256" module="guidance/guidance_hybrid"/>
<dl_setting var="horizontal_speed_gain" MIN="1" STEP="1" MAX="15" module="guidance/guidance_hybrid" shortname="hor_speed_div"/>
<dl_setting var="alt_pitch_gain" MIN="0" STEP="0.01" MAX="5.0" module="guidance/guidance_hybrid" shortname="alt_pitch_gain"/>
<dl_setting var="max_turn_bank" MIN="10.0" STEP="1.0" MAX="60.0" module="guidance/guidance_hybrid" shortname="max_turn_bank"/>
<dl_setting var="turn_bank_gain" MIN="0.1" STEP="0.01" MAX="3.0" module="guidance/guidance_hybrid" shortname="turn_bank_gain"/>
</dl_settings>
</dl_settings>
</settings>
conf/telemetry/DB/db_quadshot.xml
+167
View File
@@ -0,0 +1,167 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
<telemetry>
<process name="Main">
<mode name="default" key_press="d">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CMD" period=".25"/>
<message name="STAB_ATTITUDE_INDI" period=".5"/>
<message name="INS" period=".5"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="GPS_INT" period=".25"/>
</mode>
<mode name="ppm">
<message name="ALIVE" period="2.1"/>
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1"/>
<message name="BEBOP_ACTUATORS" period="0.2"/>
</mode>
<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
</mode>
<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>
<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<!-- <message name="AHRS_QUAT_INT" period=".25"/> -->
<message name="AHRS_EULER_INT" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>
<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>
<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>
<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
</mode>
<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS" period=".05"/>
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<!--<message name="STAB_ATTITUDE_REF_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="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>
<mode name="indi">
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INDI" 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"/>
<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_FLOAT" period="0.4"/>
<!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>
<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>
<mode name="hs_att_roll">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="0.9"/>
<message name="DL_VALUE" period="0.5"/>
<!-- <message name="STAB_ATTITUDE_HS_ROLL" period="0.02"/> -->
</mode>
<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>
</process>
</telemetry>
conf/telemetry/default_quadshot.xml
+165
View File
@@ -0,0 +1,165 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>
<process name="Main">
<mode name="default" key_press="d">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CMD" period=".25"/>
<message name="STAB_ATTITUDE_INDI" period=".5"/>
<message name="INS" period=".5"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
</mode>
<mode name="ppm">
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1"/>
<message name="ACTUATORS_BEBOP" period="0.2"/>
</mode>
<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
</mode>
<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>
<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<!-- <message name="AHRS_QUAT_INT" period=".25"/> -->
<message name="AHRS_EULER_INT" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>
<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>
<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>
<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE" period=".03"/>
<message name="STAB_ATTITUDE_REF" period=".03"/>
</mode>
<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS" period=".05"/>
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>
<mode name="indi">
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INDI" 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"/>
<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" period="0.062"/>
<message name="STAB_ATTITUDE" period="0.4"/>
<!--<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"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>
<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>
<mode name="hs_att_roll">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="0.9"/>
<message name="DL_VALUE" period="0.5"/>
<!-- <message name="STAB_ATTITUDE_HS_ROLL" period="0.02"/> -->
</mode>
<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>
</process>
</telemetry>
conf/telemetry/default_rotorcraft.xml
+13
View File
@@ -112,6 +112,19 @@
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>
<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+27
View File
@@ -26,6 +26,7 @@
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_flip.h"
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
@@ -201,6 +202,10 @@ void guidance_h_init(void)
#if GUIDANCE_INDI
guidance_indi_enter();
#endif
#if HYBRID_NAVIGATION
guidance_hybrid_init();
#endif
}
@@ -225,6 +230,10 @@ void guidance_h_mode_changed(uint8_t new_mode)
transition_theta_offset = 0;
}
#if HYBRID_NAVIGATION
guidance_hybrid_norm_ref_airspeed = 0;
#endif
switch (new_mode) {
case GUIDANCE_H_MODE_RC_DIRECT:
stabilization_none_enter();
@@ -416,7 +425,17 @@ void guidance_h_run(bool in_flight)
sp_cmd_i.psi = nav_heading;
stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
stabilization_attitude_run(in_flight);
#if HYBRID_NAVIGATION
//make sure the heading is right before leaving horizontal_mode attitude
guidance_hybrid_reset_heading(&sp_cmd_i);
#endif
} else {
#if HYBRID_NAVIGATION
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target);
guidance_hybrid_run();
#else
INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
guidance_h_update_reference();
@@ -424,6 +443,7 @@ void guidance_h_run(bool in_flight)
/* set psi command */
guidance_h.sp.heading = nav_heading;
INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
#if GUIDANCE_INDI
guidance_indi_run(in_flight, guidance_h.sp.heading);
#else
@@ -432,6 +452,8 @@ void guidance_h_run(bool in_flight)
/* set final attitude setpoint */
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
guidance_h.sp.heading);
#endif
#endif
stabilization_attitude_run(in_flight);
}
@@ -705,3 +727,8 @@ bool guidance_h_set_guided_heading_rate(float rate)
}
return false;
}
const struct Int32Vect2 *guidance_h_get_pos_err(void)
{
return &guidance_h_pos_err;
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+6
View File
@@ -147,6 +147,12 @@ extern bool guidance_h_set_guided_vel(float vx, float vy);
*/
extern bool guidance_h_set_guided_heading_rate(float rate);
/** Gets the position error
* @param none.
* @return Pointer to a structure containing x and y position errors
*/
extern const struct Int32Vect2 *guidance_h_get_pos_err(void);
/* Make sure that ref can only be temporarily disabled for testing,
* but not enabled if GUIDANCE_H_USE_REF was defined to FALSE.
*/
sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c
+409
View File
@@ -0,0 +1,409 @@
/*
* Copyright (C) 2014 Ewoud Smeur <e.j.j.smeur@tudelft.nl>
* This is code for guidance of hybrid UAVs. It needs a simple velocity
* model to control the ground velocity of the UAV while estimating the
* wind velocity.
*
* 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_hybrid.c
* Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
*
* Functionality:
* 1) hover with (helicopter) thrust vectoring and align the heading with the wind vector.
* 2) Forward flight with using pitch and a bit of thrust to control altitude and
* heading to control the velocity vector
* 3) Transition between the two, with the possibility to fly at any airspeed
*/
#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
/* for guidance_v_thrust_coeff */
#include "firmwares/rotorcraft/guidance/guidance_v.h"
// max airspeed for quadshot guidance
#define MAX_AIRSPEED 15
// high res frac for integration of angles
#define INT32_ANGLE_HIGH_RES_FRAC 18
// Variables used for settings
int32_t guidance_hybrid_norm_ref_airspeed;
float alt_pitch_gain = 0.3;
int32_t max_airspeed = MAX_AIRSPEED;
int32_t wind_gain;
int32_t horizontal_speed_gain;
float max_turn_bank;
float turn_bank_gain;
// Private variables
static struct Int32Eulers guidance_hybrid_ypr_sp;
static struct Int32Vect2 guidance_hybrid_airspeed_sp;
static struct Int32Vect2 guidance_h_pos_err;
static struct Int32Vect2 guidance_hybrid_airspeed_ref;
static struct Int32Vect2 wind_estimate;
static struct Int32Vect2 wind_estimate_high_res;
static struct Int32Vect2 guidance_hybrid_ref_airspeed;
static int32_t norm_sp_airspeed_disp;
static int32_t heading_diff_disp;
static int32_t omega_disp;
static int32_t high_res_psi;
static int32_t airspeed_sp_heading_disp;
static bool guidance_hovering;
static bool force_forward_flight;
static int32_t v_control_pitch;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_hybrid_guidance(struct transport_tx *trans, struct link_device *dev)
{
struct NedCoor_i *pos = stateGetPositionNed_i();
struct NedCoor_i *speed = stateGetSpeedNed_i();
pprz_msg_send_HYBRID_GUIDANCE(trans, dev, AC_ID,
&(pos->x), &(pos->y),
&(speed->x), &(speed->y),
&wind_estimate.x, &wind_estimate.y,
&guidance_h_pos_err.x,
&guidance_h_pos_err.y,
&guidance_hybrid_airspeed_sp.x,
&guidance_hybrid_airspeed_sp.y,
&guidance_hybrid_norm_ref_airspeed,
&heading_diff_disp,
&guidance_hybrid_ypr_sp.phi,
&guidance_hybrid_ypr_sp.theta,
&guidance_hybrid_ypr_sp.psi);
}
#endif
void guidance_hybrid_init(void)
{
INT_EULERS_ZERO(guidance_hybrid_ypr_sp);
INT_VECT2_ZERO(guidance_hybrid_airspeed_sp);
INT_VECT2_ZERO(guidance_hybrid_airspeed_ref);
high_res_psi = 0;
guidance_hovering = true;
horizontal_speed_gain = 8;
guidance_hybrid_norm_ref_airspeed = 0;
max_turn_bank = 40.0;
turn_bank_gain = 0.8;
wind_gain = 64;
force_forward_flight = 0;
INT_VECT2_ZERO(wind_estimate);
INT_VECT2_ZERO(guidance_hybrid_ref_airspeed);
INT_VECT2_ZERO(wind_estimate_high_res);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HYBRID_GUIDANCE, send_hybrid_guidance);
#endif
}
#define INT32_ANGLE_HIGH_RES_NORMALIZE(_a) { \
while ((_a) > (INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) -= (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
while ((_a) < (-INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) += (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
}
void guidance_hybrid_run(void)
{
guidance_hybrid_determine_wind_estimate();
guidance_hybrid_position_to_airspeed();
guidance_hybrid_airspeed_to_attitude(&guidance_hybrid_ypr_sp);
guidance_hybrid_set_cmd_i(&guidance_hybrid_ypr_sp);
}
void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd)
{
guidance_hybrid_ypr_sp.psi = sp_cmd->psi;
high_res_psi = sp_cmd->psi << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC);
stabilization_attitude_set_rpy_setpoint_i(sp_cmd);
}
/// Convert a required airspeed to a certain attitude for the Quadshot
void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
{
//notes:
//in forward flight, it is preferred to first get to min(airspeed_sp, airspeed_ref) and then change heading and then get to airspeed_sp
//in hover, just a gradual change is needed, or maybe not even needed
//changes between flight regimes should be handled
//determine the heading of the airspeed_sp vector
int32_t omega = 0;
float airspeed_sp_heading = atan2f((float) POS_FLOAT_OF_BFP(guidance_hybrid_airspeed_sp.y),
(float) POS_FLOAT_OF_BFP(guidance_hybrid_airspeed_sp.x));
//only for debugging
airspeed_sp_heading_disp = (int32_t)(DegOfRad(airspeed_sp_heading));
//The difference of the current heading with the required heading.
float heading_diff = airspeed_sp_heading - ANGLE_FLOAT_OF_BFP(ypr_sp->psi);
FLOAT_ANGLE_NORMALIZE(heading_diff);
//only for debugging
heading_diff_disp = (int32_t)(heading_diff / 3.14 * 180.0);
//calculate the norm of the airspeed setpoint
int32_t norm_sp_airspeed;
norm_sp_airspeed = int32_vect2_norm(&guidance_hybrid_airspeed_sp);
//reference goes with a steady pace towards the setpoint airspeed
//hold ref norm below 4 m/s until heading is aligned
if (!((norm_sp_airspeed > (4 << 8)) && (guidance_hybrid_norm_ref_airspeed < (4 << 8))
&& (guidance_hybrid_norm_ref_airspeed > ((4 << 8) - 10)) && (fabs(heading_diff) > (5.0 / 180.0 * 3.14)))) {
guidance_hybrid_norm_ref_airspeed = guidance_hybrid_norm_ref_airspeed + ((int32_t)(norm_sp_airspeed >
guidance_hybrid_norm_ref_airspeed) * 2 - 1) * 3 / 2;
}
norm_sp_airspeed_disp = norm_sp_airspeed;
int32_t psi = ypr_sp->psi;
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
guidance_hybrid_ref_airspeed.x = (guidance_hybrid_norm_ref_airspeed * c_psi) >> INT32_TRIG_FRAC;
guidance_hybrid_ref_airspeed.y = (guidance_hybrid_norm_ref_airspeed * s_psi) >> INT32_TRIG_FRAC;
if (guidance_hybrid_norm_ref_airspeed < (4 << 8)) {
/// if required speed is lower than 4 m/s act like a rotorcraft
// translate speed_sp into bank angle and heading
// change heading to direction of airspeed, faster if the airspeed is higher
if (heading_diff > 0.0) {
omega = (norm_sp_airspeed << (INT32_ANGLE_FRAC - INT32_POS_FRAC)) / 6;
} else if (heading_diff < 0.0) {
omega = (norm_sp_airspeed << (INT32_ANGLE_FRAC - INT32_POS_FRAC)) / -6;
}
if (omega > ANGLE_BFP_OF_REAL(0.8)) { omega = ANGLE_BFP_OF_REAL(0.8); }
if (omega < ANGLE_BFP_OF_REAL(-0.8)) { omega = ANGLE_BFP_OF_REAL(-0.8); }
// 2) calculate roll/pitch commands
struct Int32Vect2 hover_sp;
if (norm_sp_airspeed > (4 <<
8)) { //if the setpoint is beyond 4m/s but the ref is not, the norm of the hover sp will stay at 4m/s
hover_sp.x = (guidance_hybrid_airspeed_sp.x << 8) / norm_sp_airspeed * 4;
hover_sp.y = (guidance_hybrid_airspeed_sp.y << 8) / norm_sp_airspeed * 4;
} else {
hover_sp.x = guidance_hybrid_airspeed_sp.x;
hover_sp.y = guidance_hybrid_airspeed_sp.y;
}
// gain of 10 means that for 4 m/s an angle of 40 degrees is needed
ypr_sp->theta = (((- (c_psi * hover_sp.x + s_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * 10 * INT32_ANGLE_PI / 180) >> 8;
ypr_sp->phi = ((((- s_psi * hover_sp.x + c_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * 10 * INT32_ANGLE_PI / 180) >> 8;
} else {
/// if required speed is higher than 4 m/s act like a fixedwing
// translate speed_sp into theta + thrust
// coordinated turns to change heading
// calculate required pitch angle from airspeed_sp magnitude
if (guidance_hybrid_norm_ref_airspeed > (15 << 8)) {
ypr_sp->theta = -ANGLE_BFP_OF_REAL(RadOfDeg(78.0));
} else if (guidance_hybrid_norm_ref_airspeed > (8 << 8)) {
ypr_sp->theta = -(((guidance_hybrid_norm_ref_airspeed - (8 << 8)) * 2 * INT32_ANGLE_PI / 180) >> 8) - ANGLE_BFP_OF_REAL(
RadOfDeg(68.0));
} else {
ypr_sp->theta = -(((guidance_hybrid_norm_ref_airspeed - (4 << 8)) * 7 * INT32_ANGLE_PI / 180) >> 8) - ANGLE_BFP_OF_REAL(
RadOfDeg(40.0));
}
// if the sp_airspeed is within hovering range, don't start a coordinated turn
if (norm_sp_airspeed < (4 << 8)) {
omega = 0;
ypr_sp->phi = 0;
} else { // coordinated turn
ypr_sp->phi = ANGLE_BFP_OF_REAL(heading_diff * turn_bank_gain);
if (ypr_sp->phi > ANGLE_BFP_OF_REAL(max_turn_bank / 180.0 * M_PI)) { ypr_sp->phi = ANGLE_BFP_OF_REAL(max_turn_bank / 180.0 * M_PI); }
if (ypr_sp->phi < ANGLE_BFP_OF_REAL(-max_turn_bank / 180.0 * M_PI)) { ypr_sp->phi = ANGLE_BFP_OF_REAL(-max_turn_bank / 180.0 * M_PI); }
//feedforward estimate angular rotation omega = g*tan(phi)/v
omega = ANGLE_BFP_OF_REAL(9.81 / POS_FLOAT_OF_BFP(guidance_hybrid_norm_ref_airspeed) * tanf(ANGLE_FLOAT_OF_BFP(
ypr_sp->phi)));
if (omega > ANGLE_BFP_OF_REAL(0.7)) { omega = ANGLE_BFP_OF_REAL(0.7); }
if (omega < ANGLE_BFP_OF_REAL(-0.7)) { omega = ANGLE_BFP_OF_REAL(-0.7); }
}
}
//only for debugging purposes
omega_disp = omega;
//go to higher resolution because else the increment is too small to be added
high_res_psi += (omega << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)) / 512;
INT32_ANGLE_HIGH_RES_NORMALIZE(high_res_psi);
// go back to angle_frac
ypr_sp->psi = high_res_psi >> (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC);
ypr_sp->theta = ypr_sp->theta + v_control_pitch;
}
void guidance_hybrid_position_to_airspeed(void)
{
/* compute position error */
VECT2_DIFF(guidance_h_pos_err, guidance_h.sp.pos, *stateGetPositionNed_i());
// Compute ground speed setpoint
struct Int32Vect2 guidance_hybrid_groundspeed_sp;
VECT2_SDIV(guidance_hybrid_groundspeed_sp, guidance_h_pos_err, horizontal_speed_gain);
int32_t norm_groundspeed_sp;
norm_groundspeed_sp = int32_vect2_norm(&guidance_hybrid_groundspeed_sp);
//create setpoint groundspeed with a norm of 15 m/s
if (force_forward_flight) {
//scale the groundspeed_sp to 15 m/s if large enough to begin with
if (norm_groundspeed_sp > 1 << 8) {
guidance_hybrid_groundspeed_sp.x = guidance_hybrid_groundspeed_sp.x * (max_airspeed << 8) / norm_groundspeed_sp;
guidance_hybrid_groundspeed_sp.y = guidance_hybrid_groundspeed_sp.y * (max_airspeed << 8) / norm_groundspeed_sp;
} else { //groundspeed_sp is very small, so continue with the current heading
int32_t psi = guidance_hybrid_ypr_sp.psi;
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
guidance_hybrid_groundspeed_sp.x = (15 * c_psi) >> (INT32_TRIG_FRAC - 8);
guidance_hybrid_groundspeed_sp.y = (15 * s_psi) >> (INT32_TRIG_FRAC - 8);
}
}
struct Int32Vect2 airspeed_sp;
INT_VECT2_ZERO(airspeed_sp);
VECT2_ADD(airspeed_sp, guidance_hybrid_groundspeed_sp);
VECT2_ADD(airspeed_sp, wind_estimate);
int32_t norm_airspeed_sp;
norm_airspeed_sp = int32_vect2_norm(&airspeed_sp);
//Check if the airspeed_sp is larger than the max airspeed. If so, give the wind cancellatioin priority.
if (norm_airspeed_sp > (max_airspeed << 8) && norm_groundspeed_sp > 0) {
int32_t av = INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.x, guidance_hybrid_groundspeed_sp.x,
8) + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.y, guidance_hybrid_groundspeed_sp.y, 8);
int32_t bv = 2 * (INT_MULT_RSHIFT(wind_estimate.x, guidance_hybrid_groundspeed_sp.x,
8) + INT_MULT_RSHIFT(wind_estimate.y, guidance_hybrid_groundspeed_sp.y, 8));
int32_t cv = INT_MULT_RSHIFT(wind_estimate.x, wind_estimate.x, 8) + INT_MULT_RSHIFT(wind_estimate.y, wind_estimate.y,
8) - (max_airspeed << 8) * max_airspeed;
float dv = POS_FLOAT_OF_BFP(bv) * POS_FLOAT_OF_BFP(bv) - 4.0 * POS_FLOAT_OF_BFP(av) * POS_FLOAT_OF_BFP(cv);
float d_sqrt_f = sqrtf(dv);
int32_t d_sqrt = POS_BFP_OF_REAL(d_sqrt_f);
int32_t result = ((-bv + d_sqrt) << 8) / (2 * av);
guidance_hybrid_airspeed_sp.x = wind_estimate.x + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.x, result, 8);
guidance_hybrid_airspeed_sp.y = wind_estimate.y + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.y, result, 8);
} else {
// Add the wind to get the airspeed setpoint
guidance_hybrid_airspeed_sp = guidance_hybrid_groundspeed_sp;
VECT2_ADD(guidance_hybrid_airspeed_sp, wind_estimate);
}
// limit the airspeed setpoint to 15 m/s, because else saturation+windup will occur
norm_airspeed_sp = int32_vect2_norm(&guidance_hybrid_airspeed_sp);
if (norm_airspeed_sp > (max_airspeed << 8)) {
guidance_hybrid_airspeed_sp.x = guidance_hybrid_airspeed_sp.x * (max_airspeed << 8) / norm_airspeed_sp;
guidance_hybrid_airspeed_sp.y = guidance_hybrid_airspeed_sp.y * (max_airspeed << 8) / norm_airspeed_sp;
}
}
void guidance_hybrid_determine_wind_estimate(void)
{
/* compute speed error */
struct Int32Vect2 wind_estimate_measured;
struct Int32Vect2 measured_ground_speed;
INT32_VECT2_RSHIFT(measured_ground_speed, *stateGetSpeedNed_i(), 11);
VECT2_DIFF(wind_estimate_measured, guidance_hybrid_ref_airspeed, measured_ground_speed);
//Low pass wind_estimate, because we know the wind usually only changes slowly
//But not too slow, because the wind_estimate is also an adaptive element for the airspeed model inaccuracies
wind_estimate_high_res.x += (((wind_estimate_measured.x - wind_estimate.x) > 0) * 2 - 1) * wind_gain;
wind_estimate_high_res.y += (((wind_estimate_measured.y - wind_estimate.y) > 0) * 2 - 1) * wind_gain;
wind_estimate.x = ((wind_estimate_high_res.x) >> 8);
wind_estimate.y = ((wind_estimate_high_res.y) >> 8);
}
void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd)
{
/// @todo calc sp_quat in fixed-point
/* orientation vector describing simultaneous rotation of roll/pitch */
struct FloatVect3 ov;
ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi);
ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta);
ov.z = 0.0;
/* quaternion from that orientation vector */
struct FloatQuat q_rp;
float_quat_of_orientation_vect(&q_rp, &ov);
struct Int32Quat q_rp_i;
QUAT_BFP_OF_REAL(q_rp_i, q_rp);
// get the vertical vector to rotate the roll pitch setpoint around
struct Int32Vect3 zaxis = {0, 0, 1};
/* get current heading setpoint */
struct Int32Quat q_yaw_sp;
int32_quat_of_axis_angle(&q_yaw_sp, &zaxis, sp_cmd->psi);
// first apply the roll/pitch setpoint and then the yaw
int32_quat_comp(&stab_att_sp_quat, &q_yaw_sp, &q_rp_i);
int32_eulers_of_quat(&stab_att_sp_euler, &stab_att_sp_quat);
}
void guidance_hybrid_vertical(void)
{
if (guidance_hybrid_norm_ref_airspeed < (4 << 8)) {
//if airspeed ref < 4 only thrust
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
v_control_pitch = 0;
guidance_v_kp = GUIDANCE_V_HOVER_KP;
guidance_v_kd = GUIDANCE_V_HOVER_KD;
guidance_v_ki = GUIDANCE_V_HOVER_KI;
} else if (guidance_hybrid_norm_ref_airspeed > (8 << 8)) { //if airspeed ref > 8 only pitch,
//at 15 m/s the thrust has to be 33%
stabilization_cmd[COMMAND_THRUST] = MAX_PPRZ / 5 + (((guidance_hybrid_norm_ref_airspeed - (8 << 8)) / 7 *
(MAX_PPRZ / 3 - MAX_PPRZ / 5)) >> 8) + (guidance_v_delta_t - MAX_PPRZ / 2) / 10;
//stabilization_cmd[COMMAND_THRUST] = MAX_PPRZ/5;
// stabilization_cmd[COMMAND_THRUST] = ((guidance_hybrid_norm_ref_airspeed - (8<<8)) / 7 * (MAX_PPRZ/3 - MAX_PPRZ/5))>>8 + 9600/5;
//Control altitude with pitch, now only proportional control
float alt_control_pitch = (guidance_v_delta_t - MAX_PPRZ * guidance_v_nominal_throttle) * alt_pitch_gain;
v_control_pitch = ANGLE_BFP_OF_REAL(alt_control_pitch / (MAX_PPRZ * guidance_v_nominal_throttle));
guidance_v_kp = GUIDANCE_V_HOVER_KP / 2;
guidance_v_kd = GUIDANCE_V_HOVER_KD / 2;
guidance_v_ki = GUIDANCE_V_HOVER_KI / 2;
} else { //if airspeed ref > 4 && < 8 both
int32_t airspeed_transition = (guidance_hybrid_norm_ref_airspeed - (4 << 8)) / 4; //divide by 4 to scale it to 0-1 (<<8)
stabilization_cmd[COMMAND_THRUST] = ((MAX_PPRZ / 5 + (guidance_v_delta_t - MAX_PPRZ / 2) / 10) * airspeed_transition +
guidance_v_delta_t * ((1 << 8) - airspeed_transition)) >> 8;
float alt_control_pitch = (guidance_v_delta_t - MAX_PPRZ * guidance_v_nominal_throttle) * alt_pitch_gain;
v_control_pitch = INT_MULT_RSHIFT((int32_t) ANGLE_BFP_OF_REAL(alt_control_pitch / (MAX_PPRZ *
guidance_v_nominal_throttle)), airspeed_transition, 8);
guidance_v_kp = GUIDANCE_V_HOVER_KP;
guidance_v_kd = GUIDANCE_V_HOVER_KD;
guidance_v_ki = GUIDANCE_V_HOVER_KI;
}
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h
+80
View File
@@ -0,0 +1,80 @@
/*
* Copyright (C) 2014 Ewoud Smeur <e.j.j.smeur@tudelft.nl>
* This is code for guidance of hybrid UAVs. It needs a simple velocity
* model to control the ground velocity of the UAV while estimating the
* wind velocity.
*
* 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_hybrid.h
* Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
*
*/
#ifndef GUIDANCE_HYBRID_H
#define GUIDANCE_HYBRID_H
#include "math/pprz_algebra_int.h"
extern int32_t guidance_hybrid_norm_ref_airspeed;
extern float alt_pitch_gain;
extern int32_t max_airspeed;
extern int32_t wind_gain;
extern int32_t horizontal_speed_gain;
extern float max_turn_bank;
extern float turn_bank_gain;
/** Runs the Hybrid Guidance main functions.
*/
extern void guidance_hybrid_run(void);
/** Hybrid Guidance Initialization function.
* @param
*/
extern void guidance_hybrid_init(void);
/** Creates the attitude set-points from an orientation vector.
* @param sp_cmd The orientation vector
*/
extern void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd);
/** Convert a required airspeed to a certain attitude for the Hybrid.
* @param ypr_sp Attitude set-point
*/
extern void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp);
/** Description.
*/
extern void guidance_hybrid_position_to_airspeed(void);
/** Description.
*/
extern void guidance_hybrid_determine_wind_estimate(void);
/** Description.
* @param sp_cmd Add Description
*/
extern void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd);
/** Description.
*/
extern void guidance_hybrid_vertical(void);
#endif /* GUIDANCE_HYBRID_H */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+12 -3
View File
@@ -28,6 +28,7 @@
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/navigation.h"
@@ -231,7 +232,7 @@ void guidance_v_mode_changed(uint8_t new_mode)
switch (new_mode) {
case GUIDANCE_V_MODE_GUIDED:
case GUIDANCE_V_MODE_HOVER:
/* disable vertical velocity setpoints */
/* disable vertical velocity setpoints */
guidance_v_guided_vel_enabled = false;
/* set current altitude as setpoint */
@@ -327,8 +328,7 @@ void guidance_v_run(bool in_flight)
run_hover_loop(in_flight);
/* update z sp for telemetry/debuging */
guidance_v_z_sp = guidance_v_z_ref;
}
else {
} else {
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
@@ -366,6 +366,9 @@ void guidance_v_run(bool in_flight)
guidance_v_z_sum_err = 0;
guidance_v_delta_t = nav_throttle;
}
#if HYBRID_NAVIGATION
guidance_hybrid_vertical();
#else
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
@@ -373,6 +376,7 @@ void guidance_v_run(bool in_flight)
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
#endif
break;
}
@@ -452,6 +456,11 @@ static void run_hover_loop(bool in_flight)
/* feed forward command */
guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / guidance_v_thrust_coeff;
#if HYBRID_NAVIGATION
//FIXME: NOT USING FEEDFORWARD COMMAND BECAUSE OF QUADSHOT NAVIGATION
guidance_v_ff_cmd = guidance_v_nominal_throttle * MAX_PPRZ;
#endif
/* bound the nominal command to 0.9*MAX_PPRZ */
Bound(guidance_v_ff_cmd, 0, 8640);
sw/airborne/modules/ctrl/gain_scheduling.c
-4
View File
@@ -25,10 +25,6 @@
#include "modules/ctrl/gain_scheduling.h"
//Include for scheduling on transition_status
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/stabilization.h"
// #include "state.h"
#include "math/pprz_algebra_int.h"
sw/airborne/modules/loggers/high_speed_logger_spi_link.c
+1
View File
@@ -25,6 +25,7 @@
#include "subsystems/imu.h"
#include "mcu_periph/spi.h"
struct high_speed_logger_spi_link_data high_speed_logger_spi_link_data;
struct spi_transaction high_speed_logger_spi_link_transaction;
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: ebe2624e43...bc98b42d89
sw/tools/attitude_viz.py
+2 -2
View File
@@ -338,7 +338,7 @@ def run():
VEHICLE_QUATS = [["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]]
BAR_VALUES = [["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100]]
window_title = "Attitude_Viz"
rotate_theta = -90
rotate_theta = 0
try:
opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"])
for o, a in opts:
@@ -350,7 +350,7 @@ def run():
print(msg)
print("""usage:
-t, --title set window title
-r, --rotate_theta rotate the quaternion by n degrees over the pitch axis (default: -90)
-r, --rotate_theta rotate the quaternion by n degrees over the pitch axis (default: 0)
""")
pygame.init()
screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL | pygame.DOUBLEBUF)