mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +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
220 lines
8.2 KiB
XML
220 lines
8.2 KiB
XML
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
|
|
|
|
|
<airframe name="Quadrotor with floating point unit">
|
|
|
|
<firmware name="rotorcraft">
|
|
<target name="ap" board="lisa_m_2.0">
|
|
<configure name="FLASH_MODE" value="SWD"/>
|
|
<configure name="USE_MAGNETOMETER" value="1"/>
|
|
</target>
|
|
|
|
<target name="nps" board="pc">
|
|
<subsystem name="fdm" type="jsbsim"/>
|
|
</target>
|
|
|
|
<subsystem name="radio_control" type="ppm"/>
|
|
|
|
<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="transparent">
|
|
<configure name="MODEM_PORT" value="UART1"/>
|
|
</subsystem>
|
|
<subsystem name="imu" type="aspirin_v2.1"/>
|
|
<subsystem name="gps" type="ublox"/>
|
|
<subsystem name="stabilization" type="float_quat"/>
|
|
<subsystem name="ahrs" type="float_mlkf"/>
|
|
<subsystem name="ins"/>
|
|
</firmware>
|
|
|
|
<modules main_freq="512">
|
|
<load name="sys_mon.xml"/>
|
|
</modules>
|
|
|
|
<servos driver="Pwm">
|
|
<servo name="FRONT" no="0" min="1000" neutral="1100" max="1900"/>
|
|
<servo name="BACK" no="1" min="1000" neutral="1100" max="1900"/>
|
|
<servo name="RIGHT" no="2" min="1000" neutral="1100" max="1900"/>
|
|
<servo name="LEFT" no="3" min="1000" neutral="1100" max="1900"/>
|
|
</servos>
|
|
|
|
<commands>
|
|
<axis name="ROLL" failsafe_value="0"/>
|
|
<axis name="PITCH" failsafe_value="0"/>
|
|
<axis name="YAW" failsafe_value="0"/>
|
|
<axis name="THRUST" failsafe_value="0"/>
|
|
</commands>
|
|
|
|
<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"/>
|
|
<!-- front/back turning CW, right/left CCW -->
|
|
<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>
|
|
|
|
<command_laws>
|
|
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
|
|
<set servo="FRONT" value="motor_mixing.commands[SERVO_FRONT]"/>
|
|
<set servo="BACK" value="motor_mixing.commands[SERVO_BACK]"/>
|
|
<set servo="RIGHT" value="motor_mixing.commands[SERVO_RIGHT]"/>
|
|
<set servo="LEFT" value="motor_mixing.commands[SERVO_LEFT]"/>
|
|
</command_laws>
|
|
|
|
<section name="IMU" prefix="IMU_">
|
|
<define name="ACCEL_X_NEUTRAL" value="11"/>
|
|
<define name="ACCEL_Y_NEUTRAL" value="11"/>
|
|
<define name="ACCEL_Z_NEUTRAL" value="-25"/>
|
|
|
|
<!-- replace this with your own calibration -->
|
|
<define name="MAG_X_NEUTRAL" value="-179"/>
|
|
<define name="MAG_Y_NEUTRAL" value="-21"/>
|
|
<define name="MAG_Z_NEUTRAL" value="79"/>
|
|
<define name="MAG_X_SENS" value="4.17334785618" integer="16"/>
|
|
<define name="MAG_Y_SENS" value="3.98885954135" integer="16"/>
|
|
<define name="MAG_Z_SENS" value="4.40442339014" integer="16"/>
|
|
<define name="MAG_X_SIGN" value="1"/>
|
|
<define name="MAG_Y_SIGN" value="1"/>
|
|
<define name="MAG_Z_SIGN" value="1"/>
|
|
|
|
<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="AHRS" prefix="AHRS_">
|
|
<define name="H_X" value="0.3770441"/>
|
|
<define name="H_Y" value="0.0193986"/>
|
|
<define name="H_Z" value="0.9259921"/>
|
|
</section>
|
|
|
|
<section name="INS" prefix="INS_">
|
|
</section>
|
|
|
|
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
|
<!-- setpoints -->
|
|
<define name="SP_MAX_P" value="10000"/>
|
|
<define name="SP_MAX_Q" value="10000"/>
|
|
<define name="SP_MAX_R" value="10000"/>
|
|
<define name="DEADBAND_P" value="20"/>
|
|
<define name="DEADBAND_Q" value="20"/>
|
|
<define name="DEADBAND_R" value="200"/>
|
|
<define name="REF_TAU" value="4"/>
|
|
|
|
<!-- feedback -->
|
|
<define name="GAIN_P" value="400"/>
|
|
<define name="GAIN_Q" value="400"/>
|
|
<define name="GAIN_R" value="350"/>
|
|
|
|
<define name="IGAIN_P" value="75"/>
|
|
<define name="IGAIN_Q" value="75"/>
|
|
<define name="IGAIN_R" value="50"/>
|
|
|
|
<!-- feedforward -->
|
|
<define name="DDGAIN_P" value="300"/>
|
|
<define name="DDGAIN_Q" value="300"/>
|
|
<define name="DDGAIN_R" value="300"/>
|
|
</section>
|
|
|
|
<section name="STABILIZATION_ATTITUDE" prefix="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_A" value="0"/>
|
|
<define name="DEADBAND_E" value="0"/>
|
|
<define name="DEADBAND_R" value="250"/>
|
|
|
|
<!-- reference -->
|
|
<define name="REF_OMEGA_P" value="{RadOfDeg(800)}"/>
|
|
<define name="REF_ZETA_P" value="{0.85}"/>
|
|
<define name="REF_MAX_P" value="400." unit="deg/s"/>
|
|
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
|
|
|
|
<define name="REF_OMEGA_Q" value="{RadOfDeg(800)}"/>
|
|
<define name="REF_ZETA_Q" value="{0.85}"/>
|
|
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
|
|
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
|
|
|
|
<define name="REF_OMEGA_R" value="{RadOfDeg(500)}"/>
|
|
<define name="REF_ZETA_R" value="{0.85}"/>
|
|
<define name="REF_MAX_R" value="180." unit="deg/s"/>
|
|
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
|
|
|
|
<!-- feedback -->
|
|
<define name="PHI_PGAIN" value="{1000}"/>
|
|
<define name="PHI_DGAIN" value="{1000}"/>
|
|
<define name="PHI_IGAIN" value="{200}"/>
|
|
|
|
<define name="THETA_PGAIN" value="{1000}"/>
|
|
<define name="THETA_DGAIN" value="{1000}"/>
|
|
<define name="THETA_IGAIN" value="{200}"/>
|
|
|
|
<define name="PSI_PGAIN" value="{500}"/>
|
|
<define name="PSI_DGAIN" value="{500}"/>
|
|
<define name="PSI_IGAIN" value="{10}"/>
|
|
|
|
<!-- feedback angular acceleration error -->
|
|
<define name="PHI_DGAIN_D" value="{100}"/>
|
|
<define name="THETA_DGAIN_D" value="{100}"/>
|
|
<define name="PSI_DGAIN_D" value="{100}"/>
|
|
|
|
<!-- feedforward -->
|
|
<define name="PHI_DDGAIN" value="{300}"/>
|
|
<define name="THETA_DDGAIN" value="{300}"/>
|
|
<define name="PSI_DDGAIN" value="{300}"/>
|
|
</section>
|
|
|
|
<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="150"/>
|
|
<define name="HOVER_KD" value="80"/>
|
|
<define name="HOVER_KI" value="20"/>
|
|
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
|
|
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
|
</section>
|
|
|
|
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
|
<define name="MAX_BANK" value="20" unit="deg"/>
|
|
<define name="PGAIN" value="50"/>
|
|
<define name="DGAIN" value="100"/>
|
|
<define name="AGAIN" value="100"/>
|
|
<define name="IGAIN" value="20"/>
|
|
</section>
|
|
|
|
<section name="SIMULATOR" prefix="NPS_">
|
|
<define name="ACTUATOR_NAMES" value="{"front_motor", "back_motor", "right_motor", "left_motor"}"/>
|
|
<define name="JSBSIM_INIT" value=""reset00""/>
|
|
<define name="SENSORS_PARAMS" value=""nps_sensors_params_default.h""/>
|
|
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
|
|
<define name="JS_AXIS_MODE" value="4"/>
|
|
</section>
|
|
|
|
<section name="AUTOPILOT">
|
|
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
|
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
|
|
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
|
</section>
|
|
|
|
<section name="BAT">
|
|
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
|
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
|
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
|
|
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
|
</section>
|
|
|
|
</airframe>
|