mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 06:54:49 +08:00
Felix Ruess
ede94e0375
Rotorcraft guidance fixes/improvements So now horizontal guidance returns commands in north/east earth frame and doesn't already rotate it to body frame. This allows for better handling of them in different controllers. New and properly working transformations from earth commands to quaternions added. No proper fixedpoint implementation so far... no change there compared to current master though. Should still be done for efficiency. Tested the int_quat stablizitation with this in simulation and it works perfectly as expected. IMPORTANT: This does currently not have the feature to "add" roll/pitch setpoints via RC in nav/hover anymore. Is that really used, instead of switching to manual if needed? Instead you can give velocity commands in hover mode via RC. Includes max bank improvements proposed in #546 Split PD and I-gain with separate max bank no wind: integrator (trim attitude) = 0, PD maxbank = -20 to 20 extra: total: -20 to 20 huge wind: integrator (trim attitude) = 20, PD maxbank = -20 to 20 extra: total: 0 to 40 Put gain before integrator to get better insight in the saturation values Reduce integration overshoots while increasing integration speed by adding the speed error as well no speed error: integrate as before counterproductive speed error, integrate faster if the position error is already decreasing fast, no integrator is needed or it will overshoot closes #539
271 lines
9.1 KiB
XML
271 lines
9.1 KiB
XML
<!--DOCTYPE airframe SYSTEM "../../airframe.dtd"-->
|
|
|
|
<airframe name="Booz Mkk1 Enac">
|
|
|
|
<modules main_freq="512">
|
|
<load name="vision_cmucam.xml"/>
|
|
<load name="sonar_adc_ins.xml"/>
|
|
</modules>
|
|
|
|
<servos>
|
|
<servo name="FRONT" no="0" min="0" neutral="0" max="255"/>
|
|
<servo name="BACK" no="1" min="0" neutral="0" max="255"/>
|
|
<servo name="RIGHT" no="2" min="0" neutral="0" max="255"/>
|
|
<servo name="LEFT" no="3" min="0" neutral="0" max="255"/>
|
|
</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>
|
|
|
|
<section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
|
|
<define name="NB" value="4"/>
|
|
<define name="ADDR" value="{ 0x58, 0x52, 0x54, 0x56 }"/>
|
|
</section>
|
|
|
|
<section name="SUPERVISION" prefix="SUPERVISION_">
|
|
<define name="MIN_MOTOR" value="2"/>
|
|
<define name="MAX_MOTOR" value="210"/>
|
|
<define name="TRIM_A" value="0"/>
|
|
<define name="TRIM_E" value="0"/>
|
|
<define name="TRIM_R" value="0"/>
|
|
<define name="NB_MOTOR" value="4"/>
|
|
<define name="SCALE" value="256"/>
|
|
<define name="ROLL_COEF" value="{ 0, 0, -256, 256}"/>
|
|
<define name="PITCH_COEF" value="{ 256, -256, 0, 0}"/>
|
|
<define name="YAW_COEF" value="{ -256, -256, 256, 256}"/>
|
|
<define name="THRUST_COEF" value="{ 256, 256, 256, 256}"/>
|
|
</section>
|
|
|
|
<section name="IMU" prefix="IMU_">
|
|
|
|
<define name="GYRO_P_CHAN" value="1"/>
|
|
<define name="GYRO_Q_CHAN" value="0"/>
|
|
<define name="GYRO_R_CHAN" value="2"/>
|
|
|
|
<define name="GYRO_P_NEUTRAL" value="30907"/>
|
|
<define name="GYRO_Q_NEUTRAL" value="33253"/>
|
|
<define name="GYRO_R_NEUTRAL" value="32329"/>
|
|
|
|
<define name="GYRO_P_SENS" value="1.00" integer="16"/>
|
|
<define name="GYRO_Q_SENS" value="1.00" integer="16"/>
|
|
<define name="GYRO_R_SENS" value="1.00" integer="16"/>
|
|
|
|
<define name="ACCEL_X_CHAN" value="5"/>
|
|
<define name="ACCEL_Y_CHAN" value="3"/>
|
|
<define name="ACCEL_Z_CHAN" value="4"/>
|
|
|
|
<define name="ACCEL_X_NEUTRAL" value="32913"/>
|
|
<define name="ACCEL_Y_NEUTRAL" value="33032"/>
|
|
<define name="ACCEL_Z_NEUTRAL" value="32843"/>
|
|
|
|
<define name="ACCEL_X_SENS" value="2.5749914646" integer="16"/>
|
|
<define name="ACCEL_Y_SENS" value="2.56940516083" integer="16"/>
|
|
<define name="ACCEL_Z_SENS" value="2.56940374188" integer="16"/>
|
|
|
|
<define name="MAG_X_CHAN" value="0"/>
|
|
<define name="MAG_Y_CHAN" value="1"/>
|
|
<define name="MAG_Z_CHAN" value="2"/>
|
|
|
|
<define name="MAG_X_NEUTRAL" value="-62"/>
|
|
<define name="MAG_Y_NEUTRAL" value="-13"/>
|
|
<define name="MAG_Z_NEUTRAL" value="-95"/>
|
|
|
|
<define name="MAG_X_SENS" value="6.42348671436" integer="16"/>
|
|
<define name="MAG_Y_SENS" value="6.95371185551" integer="16"/>
|
|
<define name="MAG_Z_SENS" value="3.12322557454" integer="16"/>
|
|
|
|
|
|
<!--define name="MAG_X_NEUTRAL" value="-12"/>
|
|
<define name="MAG_Y_NEUTRAL" value="-10"/>
|
|
<define name="MAG_Z_NEUTRAL" value="-11"/>
|
|
|
|
<define name="MAG_X_SENS" value="22.008352" integer="16"/>
|
|
<define name="MAG_Y_SENS" value="-21.79885" integer="16"/>
|
|
<define name="MAG_Z_SENS" value="-14.675745" integer="16"/-->
|
|
|
|
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
|
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
|
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
|
|
|
</section>
|
|
|
|
<section name="STABILIZATION_RATE" prefix="BOOZ_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="400"/>
|
|
<define name="GAIN_Q" value="400"/>
|
|
<define name="GAIN_R" value="350"/>
|
|
|
|
</section>
|
|
|
|
<section name="STABILIZATION_RATE" prefix="BOOZ_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="400"/>
|
|
<define name="GAIN_Q" value="400"/>
|
|
<define name="GAIN_R" value="350"/>
|
|
|
|
</section>
|
|
|
|
|
|
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
|
|
|
|
<!-- setpoints -->
|
|
<define name="SP_MAX_PHI" value="45." unit="deg"/>
|
|
<define name="SP_MAX_THETA" value="45." unit="deg"/>
|
|
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
|
<define name="DEADBAND_R" value="250"/>
|
|
|
|
<!-- reference -->
|
|
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
|
|
<define name="REF_ZETA_P" value="0.9"/>
|
|
<define name="REF_MAX_P" value="300." unit="deg/s"/>
|
|
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
|
|
|
|
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
|
|
<define name="REF_ZETA_Q" value="0.9"/>
|
|
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
|
|
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
|
|
|
|
<define name="REF_OMEGA_R" value="500" 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.)"/>
|
|
|
|
<!-- feedback -->
|
|
<define name="PHI_PGAIN" value="650"/>
|
|
<define name="PHI_DGAIN" value="200"/>
|
|
<define name="PHI_IGAIN" value="100"/>
|
|
|
|
<define name="THETA_PGAIN" value="650"/>
|
|
<define name="THETA_DGAIN" value="200"/>
|
|
<define name="THETA_IGAIN" value="100"/>
|
|
|
|
<define name="PSI_PGAIN" value="800"/>
|
|
<define name="PSI_DGAIN" value="320"/>
|
|
<define name="PSI_IGAIN" value="20"/>
|
|
|
|
<!-- feedforward -->
|
|
<define name="PHI_DDGAIN" value=" 300"/>
|
|
<define name="THETA_DDGAIN" value=" 300"/>
|
|
<define name="PSI_DDGAIN" value=" 300"/>
|
|
|
|
</section>
|
|
|
|
<section name="INS" prefix="BOOZ_INS_">
|
|
<!--define name="SONAR_SENS" value="2.146" integer="16"/-->
|
|
<define name="SONAR_SENS" value="3." integer="16"/>
|
|
</section>
|
|
|
|
<section name="HORIZONTAL_FILTER" prefix="B2_HFF_">
|
|
<define name="ACCEL_NOISE" value="10."/>
|
|
<define name="R_POS" value="0.01"/>
|
|
<define name="R_POS_MIN" value="0."/>
|
|
<define name="R_SPEED" value="0.01"/>
|
|
<define name="R_SPEED_MIN" value="0."/>
|
|
</section>
|
|
|
|
<section name="GUIDANCE_V" prefix="BOOZ2_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="REF_MIN_ZDD" value="-1.5*9.81"/>
|
|
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
|
|
<define name="REF_MIN_ZD" value="-1.5"/>
|
|
<define name="REF_MAX_ZD" value=" 1.5"/>
|
|
<define name="HOVER_KP" value="200"/>
|
|
<define name="HOVER_KD" value="100"/>
|
|
<define name="HOVER_KI" value="0"/>
|
|
</section>
|
|
|
|
|
|
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
|
|
<define name="PGAIN" value="250"/>
|
|
<define name="DGAIN" value="150"/>
|
|
<define name="IGAIN" value="35"/>
|
|
<define name="NGAIN" value="0"/>
|
|
<!-- feedforward -->
|
|
<define name="AGAIN" value="100"/>
|
|
</section>
|
|
|
|
<section name="BAT">
|
|
<define name="CATASTROPHIC_BAT_LEVEL" value="9." unit="V"/>
|
|
</section>
|
|
|
|
<section name="AUTOPILOT">
|
|
<define name="BOOZ2_MODE_MANUAL" value="BOOZ2_AP_MODE_ATTITUDE_DIRECT"/>
|
|
<define name="BOOZ2_MODE_AUTO1" value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
|
|
<define name="BOOZ2_MODE_AUTO2" value="BOOZ2_AP_MODE_NAV"/>
|
|
</section>
|
|
|
|
<section name="FMS">
|
|
<define name="BOOZ_FMS_TIMEOUT" value="0"/>
|
|
</section>
|
|
|
|
<section name="MISC">
|
|
<define name="BOOZ2_FACE_REINJ_1" value="1024"/>
|
|
</section>
|
|
|
|
|
|
<section name="SIMULATOR" prefix="NPS_">
|
|
<define name="ACTUATOR_NAMES" value="{"front_motor", "back_motor", "right_motor", "left_motor"}"/>
|
|
<define name="SENSORS_PARAMS" value=""nps_sensors_params_booz2_a1.h""/>
|
|
</section>
|
|
|
|
|
|
|
|
|
|
<makefile>
|
|
ARCH=lpc21
|
|
ARCHI=arm7
|
|
BOARD_CFG = \"boards/booz2_v1_0.h\"
|
|
FLASH_MODE=IAP
|
|
|
|
# prevents motors from ever starting
|
|
#ap.CFLAGS += -DKILL_MOTORS
|
|
|
|
include $(PAPARAZZI_SRC)/conf/firmwares/booz2_common.makefile
|
|
include $(CFG_BOOZ)/booz2_autopilot.makefile
|
|
include $(CFG_BOOZ)/booz2_test_progs.makefile
|
|
ap.CFLAGS += -DBOOZ_FAILSAFE_GROUND_DETECT
|
|
|
|
ap.CFLAGS += -DMODEM_BAUD=B57600
|
|
include $(CFG_BOOZ)/subsystems/booz2_actuators_mkk.makefile
|
|
include $(CFG_BOOZ)/subsystems/booz2_radio_control_ppm.makefile
|
|
include $(CFG_BOOZ)/subsystems/booz2_imu_b2v1_1.makefile
|
|
#include $(CFG_BOOZ)/subsystems/booz2_gps.makefile
|
|
include $(CFG_BOOZ)/subsystems/booz2_analog_bat_baro.makefile
|
|
#ap.CFLAGS += -DBOOZ2_SONAR -DUSE_SONAR_1
|
|
#include $(CFG_BOOZ)/subsystems/booz2_analog_bat_baro_sonar.makefile
|
|
include $(CFG_BOOZ)/subsystems/booz2_fms_datalink.makefile
|
|
|
|
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
|
|
|
|
#include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
|
|
|
|
ap.CFLAGS += -DB2_GUIDANCE_H_USE_REF
|
|
ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT
|
|
|
|
ap.CFLAGS += -DUSE_MODULES
|
|
|
|
include $(CFG_BOOZ)/booz2_simulator_nps.makefile
|
|
#sim.CFLAGS += -DB2_GUIDANCE_H_USE_REF
|
|
sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT
|
|
sim.CFLAGS += -DUSE_MODULES
|
|
|
|
</makefile>
|
|
|
|
</airframe>
|