mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-13 11:29:12 +08:00
Command filter for vibrating airframes (#2226)
* Filtering commands + example airframe Command filter + example airframe * more generic implementation * airframe move to the new tudelft location, gain tuning * revert pprzlink * revert opencv * revert libopencm * style fixed * fix indenting & modules section moved * added function declaration
This commit is contained in:
matejkarasek
committed by
Kirk Scheper
Kirk Scheper
parent
d45b2d7149
commit
cd659a78da
@@ -1,202 +0,0 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!-- Delfly (http://www.delfly.nl/home.html)
|
||||
Lisa/S v0.1 board (http://wiki.paparazziuav.org/wiki/Lisa/S)
|
||||
Using Lisa/S V1.0 board file as it is software compatible.
|
||||
-->
|
||||
|
||||
<airframe name="Delfly Lisa/S 0.1">
|
||||
<description>DelFly Outdoor Auto2
|
||||
</description>
|
||||
|
||||
<firmware name="fixedwing">
|
||||
<target name="ap" board="lisa_s_1.0">
|
||||
<module name="radio_control" type="spektrum">
|
||||
<define name="RADIO_MODE" value="RADIO_FLAP"/>
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
|
||||
<configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="0"/>
|
||||
</module>
|
||||
|
||||
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
|
||||
</target>
|
||||
|
||||
<define name="AGR_CLIMB"/>
|
||||
<define name="LOITER_TRIM"/>
|
||||
<define name="STRONG_WIND"/>
|
||||
|
||||
<!-- Communication -->
|
||||
<module name="telemetry" type="transparent">
|
||||
<!--<configure name="MODEM_BAUD" value="B57600"/>
|
||||
<configure name="MODEM_PORT" value="UART1"/>-->
|
||||
</module>
|
||||
|
||||
<!-- Sensors -->
|
||||
<module name="imu" type="lisa_s_v1.0"/>
|
||||
<module name="gps" type="ublox"/>
|
||||
|
||||
<module name="actuators" type="pwm">
|
||||
<define name="SERVO_HZ" value="50"/>
|
||||
<define name="USE_SERVOS_1AND2"/>
|
||||
</module>
|
||||
|
||||
<module name="actuators" type="dualpwm">
|
||||
<define name="DUAL_PWM_ON"/>
|
||||
</module>
|
||||
|
||||
<!--<module name="ahrs" type="int_cmpl_quat"/>-->
|
||||
<module name="control"/>
|
||||
<module name="navigation"/>
|
||||
<module name="ins" type="alt_float">
|
||||
<define name="USE_BAROMETER" value="TRUE"/>
|
||||
</module>
|
||||
|
||||
<!--<configure name="SYS_TIME_LED" value="none"/>-->
|
||||
</firmware>
|
||||
|
||||
<modules>
|
||||
<module name="baro_sim"/>
|
||||
<module name="switch" type="servo"/>
|
||||
<module name="gps" type="ubx_ucenter"/>
|
||||
</modules>
|
||||
|
||||
<servos driver="Pwm">
|
||||
<servo name="MOTOR" no="0" min="1290" neutral="1290" max="1810"/>
|
||||
<!--<servo name="SWITCH" no="2" min="1100" neutral="1500" max="1900"/>
|
||||
<servo name="SWITCH2" no="3" min="1100" neutral="1500" max="1900"/>-->
|
||||
</servos>
|
||||
|
||||
<!-- comboPWM / mergedPWM / -->
|
||||
<servos driver="Dualpwm">
|
||||
<servo name="SWITCH" no="3" min="1100" neutral="1500" max="1900"/>
|
||||
<servo name="pitchator" no="2" min="1100" neutral="1500" max="1900"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="THROTTLE" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="4000"/>
|
||||
<axis name="PITCH" failsafe_value="-8000"/>
|
||||
</commands>
|
||||
|
||||
<rc_commands>
|
||||
<set command="THROTTLE" value="@THROTTLE"/>
|
||||
<set command="ROLL" value="@ROLL"/>
|
||||
<set command="PITCH" value="@PITCH"/>
|
||||
</rc_commands>
|
||||
|
||||
<command_laws>
|
||||
<set servo="MOTOR" value="@THROTTLE"/>
|
||||
<set servo="pitchator" value="@PITCH"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- ACCEL and GYRO calibration left out to take default datasheet values -->
|
||||
|
||||
<!-- replace this with your own mag calibration -->
|
||||
<define name="MAG_X_NEUTRAL" value="-45"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="334"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="7"/>
|
||||
<define name="MAG_X_SENS" value="4.47647816128" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="4.71085671542" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="4.41585354498" 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="AHRS" prefix="AHRS_">
|
||||
<!-- replace this with your local magnetic field -->
|
||||
<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_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
<define name="POWER_CTL_BAT_NOMINAL" value="7.3" unit="volt"/>
|
||||
<!-- outer loop proportional gain -->
|
||||
<define name="ALTITUDE_PGAIN" value="0.03"/>
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
|
||||
|
||||
<!-- auto throttle inner loop -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
|
||||
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
|
||||
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="0.01"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||
|
||||
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="1.0"/>
|
||||
<define name="COURSE_DGAIN" value="0.3"/>
|
||||
|
||||
<define name="ROLL_MAX_SETPOINT" value="35" unit="deg"/>
|
||||
<define name="PITCH_MAX_SETPOINT" value="30" unit="deg"/>
|
||||
<define name="PITCH_MIN_SETPOINT" value="-30" unit="deg"/>
|
||||
|
||||
<define name="PITCH_PGAIN" value="12000."/>
|
||||
<define name="PITCH_DGAIN" value="1.5"/>
|
||||
|
||||
<define name="ELEVATOR_OF_ROLL" value="1250"/>
|
||||
|
||||
<define name="ROLL_SLEW" value="0.1"/>
|
||||
|
||||
<define name="ROLL_ATTITUDE_GAIN" value="7500"/>
|
||||
<define name="ROLL_RATE_GAIN" value="1500"/>
|
||||
</section>
|
||||
|
||||
<section name="AGGRESSIVE" prefix="AGR_">
|
||||
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
|
||||
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
|
||||
<define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
|
||||
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
|
||||
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
|
||||
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
|
||||
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
|
||||
<define name="DESCENT_NAV_RATIO" value="1.0"/>
|
||||
</section>
|
||||
|
||||
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
|
||||
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
|
||||
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
|
||||
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="40." unit="deg"/>
|
||||
<define name="MAX_PITCH" value="35." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="1000"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="2.9" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="3.1" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="3.4" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="4.2" unit="V"/>
|
||||
<!--<define name="VoltageOfAdc(adc)" value ="8"/>-->
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
|
||||
<define name="CARROT" value="5." unit="s"/>
|
||||
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||
<define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||
|
||||
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||
<!--<define name="RC_LOST_MODE" value="mode_auto2"/>-->
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,255 @@
|
||||
<!-- this is a generic DelFly frame equiped with Lisa/S 1.0 -->
|
||||
|
||||
<!--
|
||||
Applicable configuration:
|
||||
airframe="airframes/tudelft/delfly_lisas.xml"
|
||||
radio="radios/cockpitSX.xml" (SuperbitRF) / radio="radios/delfly_Rx31_Devo10.xml" (DelTang Rx31)
|
||||
telemetry="telemetry/default_rotorcraft.xml" (no logger) / telemetry="telemetry/rotorcraft_with_logger.xml" (with logger)
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml ../conf/modules/logger_sd_spi_direct.xml (if SD logger is used)"
|
||||
-->
|
||||
|
||||
<airframe name="delfly_lisa_s">
|
||||
|
||||
<!-- FIRMWARE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%-->
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="lisa_s_1.0">
|
||||
<!-- SuperbitRF RC -->
|
||||
<!-- module name="radio_control" type="superbitrf_rc"/-->
|
||||
<!-- DelTang Rx31 -->
|
||||
<module name="radio_control" type="ppm">
|
||||
<configure name="RADIO_CONTROL_PPM_PIN" value="SERVO6"/>
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
|
||||
<define name="RADIO_YAW" value="RADIO_ROLL"/>
|
||||
</module>
|
||||
|
||||
<configure name="USE_MAGNETOMETER" value="0"/>
|
||||
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
|
||||
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
|
||||
</target>
|
||||
|
||||
<!-- Necessary to build succesfully, but not used -->
|
||||
<module name="motor_mixing"/>
|
||||
|
||||
<!-- Rpm sensing settings (cannot be pushed to master) -->
|
||||
<!--subsystem name="rpm_sensor" type="eagletree"-->
|
||||
<!--define name="PULSES_PER_ROTATION" value="5"/-->
|
||||
<!--/subsystem-->
|
||||
|
||||
<module name="actuators" type="pwm">
|
||||
<define name="SERVO_HZ" value="400"/>
|
||||
<define name="USE_SERVOS_1AND2"/>
|
||||
</module>
|
||||
|
||||
<!-- TELEMETRY -->
|
||||
<!-- SuperbitRF telemetry -->
|
||||
<!--module name="telemetry" type="superbitrf"/-->
|
||||
<!-- Debug serial interface telemetry -->
|
||||
<module name="telemetry" type="transparent">
|
||||
<configure name="MODEM_BAUD" value="B115200"/>
|
||||
<configure name="MODEM_PORT" value="UART1"/>
|
||||
</module>
|
||||
|
||||
<module name="imu" type="lisa_s_v1.0"/>
|
||||
<module name="gps" type="ublox"/>
|
||||
<module name="stabilization" type="int_euler" prefix="STABILIZATION_">
|
||||
<define name="FILTER_CMD_ROLL_PITCH" value="1"/>
|
||||
<define name="FILTER_CMD_YAW" value="1"/>
|
||||
<define name="FILTER_CMD_ROLL_CUTOFF" value="15.0"/>
|
||||
<define name="FILTER_CMD_PITCH_CUTOFF" value="15.0"/>
|
||||
<define name="FILTER_CMD_YAW_CUTOFF" value="10.0"/>
|
||||
<define name="ATTITUDE_SP_PSI_DELTA_LIMIT" value="1.57"/>
|
||||
</module>
|
||||
<module name="ahrs" type="int_cmpl_quat"/>
|
||||
<module name="ins"/>
|
||||
|
||||
<module name="send_imu_mag_current"/>
|
||||
|
||||
<!-- When sd logger is used, then radio should be DelTang (PPM) and telemetry should
|
||||
be transparent (cable/WiFi) -->
|
||||
<module name="logger_sd_spi_direct.xml">
|
||||
<define name="SDLOGGER_ON_ARM" value="TRUE"/>
|
||||
<!--define name="LOGGER_LED" value="3"/-->
|
||||
</module>
|
||||
</firmware>
|
||||
|
||||
<!-- ACTUATORS & COMMANDS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%-->
|
||||
|
||||
<servos driver="Pwm">
|
||||
<servo name="MOTOR" no="4" min="1100" neutral="1100" max="1900"/>
|
||||
<servo name="ELEVATOR" no="5" min="1100" neutral="1500" max="1900"/>
|
||||
<servo name="RUDDER" no="0" min="1100" neutral="1500" max="1900"/>
|
||||
</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="MOTOR" value="@THRUST"/>
|
||||
<set servo="ELEVATOR" value="@PITCH"/>
|
||||
<set servo="RUDDER" value="@YAW"/>
|
||||
</command_laws>
|
||||
|
||||
<!-- SETTINGS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%-->
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="245." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
|
||||
<!-- MAGNETO CALIBRATION DELFT - calibration still needs to be done !!! -->
|
||||
<define name="MAG_X_NEUTRAL" value="10"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="238"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="-74"/>
|
||||
<define name="MAG_X_SENS" value="3.67001348283" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="3.98840260231" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="4.32568461736" integer="16"/>
|
||||
|
||||
<!-- MAGNETO CURRENT CALIBRATION -->
|
||||
<define name= "MAG_X_CURRENT_COEF" value="-0.0176704713698"/>
|
||||
<define name= "MAG_Y_CURRENT_COEF" value="0.0174222594634"/>
|
||||
<define name= "MAG_Z_CURRENT_COEF" value="0.0187570316492"/>
|
||||
</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"/>
|
||||
<define name="USE_GPS_HEADING" value="0"/>
|
||||
|
||||
<!-- For vibrating airfames -->
|
||||
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_STARTUP" value="AP_MODE_RC_DIRECT"/>
|
||||
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
|
||||
<define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
</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_PSI" value="45." unit="deg"/>
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="SP_MAX_P" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="250"/>
|
||||
<define name="DEADBAND_A" value="250"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
|
||||
<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="800" unit="deg/s"/>
|
||||
<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="500" unit="deg/s"/>
|
||||
<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(900.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="1"/>
|
||||
<define name="PHI_DGAIN" value="1"/>
|
||||
<define name="PHI_IGAIN" value="1"/>
|
||||
|
||||
<define name="THETA_PGAIN" value="4024"/>
|
||||
<define name="THETA_DGAIN" value="200"/>
|
||||
<define name="THETA_IGAIN" value="800"/>
|
||||
|
||||
<define name="PSI_PGAIN" value="4000"/>
|
||||
<define name="PSI_DGAIN" value="2000"/>
|
||||
<define name="PSI_IGAIN" value="60"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="0"/>
|
||||
<define name="THETA_DDGAIN" value="0"/>
|
||||
<define name="PSI_DDGAIN" value=" 0"/>
|
||||
|
||||
<define name="PHI_AGAIN" value="0"/>
|
||||
<define name="THETA_AGAIN" value="0"/>
|
||||
<define name="PSI_AGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="USE_GPS_ALT" value="1"/>
|
||||
</section>
|
||||
|
||||
<section name="GPS" prefix="GPS_">
|
||||
<define name="USE_DATALINK_SMALL" value="1"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="276"/>
|
||||
<define name="HOVER_KD" value="455"/>
|
||||
<define name="HOVER_KI" value="100"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.65"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="PGAIN" value="120"/>
|
||||
<define name="DGAIN" value="40"/>
|
||||
<define name="IGAIN" value="0"/>
|
||||
<define name="REF_MAX_SPEED" value="0.5"/>
|
||||
<define name="USE_REF" value="FALSE"/>
|
||||
</section>
|
||||
|
||||
<section name="NAVIGATION" prefix="NAV_">
|
||||
<define name="CLIMB_VSPEED" value="1.0"/>
|
||||
<define name="DESCEND_VSPEED" value="-0.5"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="0.2"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="3.2" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="ne_motor, se_motor, sw_motor, nw_motor" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<!--Motor mixing not used, presence necessary for a succesful build...-->
|
||||
<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="ROLL_COEF" value="{ -256, -256, 256, 256 }"/>
|
||||
<define name="PITCH_COEF" value="{ 256, -256, -256, 256 }"/>
|
||||
<define name="YAW_COEF" value="{ 256, -256, 256, -256 }"/>
|
||||
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -7,6 +7,13 @@
|
||||
Also provide the direct mode 'stabilization_none'
|
||||
As a durty hack, it also provides navigation and guidance: this should be done in the airframe at some point
|
||||
</description>
|
||||
<section name="STABILIZATION_FILTER_CMD" prefix="STABILIZATION_FILTER_CMD_">
|
||||
<define name="ROLL_PITCH" value="0" description="roll and pitch command filter toggle" unit="bool"/>
|
||||
<define name="YAW" value="0" description="yaw command filter toggle" unit="bool"/>
|
||||
<define name="ROLL_CUTOFF" value="20.0" description="roll command filter cut-off" unit="Hz"/>
|
||||
<define name="PITCH_CUTOFF" value="20.0" description="pitch command filter cut-off" unit="Hz"/>
|
||||
<define name="YAW_CUTOFF" value="20.0" description="yaw command filter cut-off" unit="Hz"/>
|
||||
</section>
|
||||
</doc>
|
||||
<autoload name="nav_basic_rotorcraft"/>
|
||||
<header>
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
<?xml version="1.0"?>
|
||||
<!DOCTYPE radio SYSTEM "radio.dtd">
|
||||
<radio name="DelFly Rx31 DEVO10" data_min="900" data_max="2050" sync_min ="5000" sync_max ="15000" pulse_type="NEGATIVE">
|
||||
<channel ctl="A" function="THROTTLE" min="1110" neutral="1110" max="1900" average="0"/>
|
||||
<channel ctl="D" function="ROLL" max="1110" neutral="1500" min="1900" average="0"/>
|
||||
<channel ctl="C" function="PITCH" max="1110" neutral="1500" min="1900" average="0"/>
|
||||
<channel ctl="B" function="GEAR" max="1110" neutral="1500" min="1900" average="0"/>
|
||||
<channel ctl="G" function="FLAP" min="1110" neutral="1500" max="1900" average="1"/>
|
||||
<channel ctl="E" function="MODE" min="1110" neutral="1500" max="1900" average="1"/>
|
||||
<channel ctl="F" function="ELEV_DR" min="1110" neutral="1500" max="1900" average="1"/>
|
||||
</radio>
|
||||
@@ -0,0 +1,13 @@
|
||||
<conf>
|
||||
<aircraft
|
||||
name="DelFly_LisaS"
|
||||
ac_id="111"
|
||||
airframe="airframes/tudelft/delfly_lisas.xml"
|
||||
radio="radios/delfly_Rx31_DEVO10.xml"
|
||||
telemetry="telemetry/rotorcraft_with_logger.xml"
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml ../conf/modules/logger_sd_spi_direct.xml"
|
||||
settings_modules="modules/logger_sd_spi_direct.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
</conf>
|
||||
@@ -319,7 +319,7 @@ void guidance_h_read_rc(bool in_flight)
|
||||
stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
|
||||
break;
|
||||
case GUIDANCE_H_MODE_HOVER:
|
||||
stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE );
|
||||
stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
|
||||
#if GUIDANCE_H_USE_SPEED_REF
|
||||
read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
|
||||
/* enable x,y velocity setpoints */
|
||||
@@ -374,6 +374,12 @@ void guidance_h_run(bool in_flight)
|
||||
transition_run(false);
|
||||
}
|
||||
stabilization_attitude_run(in_flight);
|
||||
#if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW)
|
||||
if (in_flight) {
|
||||
stabilization_filter_commands();
|
||||
}
|
||||
#endif
|
||||
|
||||
break;
|
||||
|
||||
case GUIDANCE_H_MODE_HOVER:
|
||||
@@ -605,7 +611,7 @@ void guidance_h_from_nav(bool in_flight)
|
||||
/* set final attitude setpoint */
|
||||
int32_t heading_sp_i = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
|
||||
stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
|
||||
heading_sp_i);
|
||||
heading_sp_i);
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
@@ -25,11 +25,66 @@
|
||||
|
||||
#include "firmwares/rotorcraft/stabilization.h"
|
||||
|
||||
#if (STABILIZATION_FILTER_COMMANDS_ROLL_PITCH || STABILIZATION_FILTER_COMMANDS_YAW)
|
||||
#include "filters/low_pass_filter.h"
|
||||
#endif
|
||||
|
||||
int32_t stabilization_cmd[COMMANDS_NB];
|
||||
|
||||
#if STABILIZATION_FILTER_CMD_ROLL_PITCH
|
||||
#ifndef STABILIZATION_FILTER_CMD_ROLL_CUTOFF
|
||||
#define STABILIZATION_FILTER_CMD_ROLL_CUTOFF 20.0
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZATION_FILTER_CMD_PITCH_CUTOFF
|
||||
#define STABILIZATION_FILTER_CMD_PITCH_CUTOFF 20.0
|
||||
#endif
|
||||
|
||||
struct SecondOrderLowPass_int filter_roll;
|
||||
struct SecondOrderLowPass_int filter_pitch;
|
||||
#endif
|
||||
|
||||
#if STABILIZATION_FILTER_CMD_YAW
|
||||
#ifndef STABILIZATION_FILTER_CMD_YAW_CUTOFF
|
||||
#define STABILIZATION_FILTER_CMD_YAW_CUTOFF 20.0
|
||||
#endif
|
||||
|
||||
struct SecondOrderLowPass_int filter_yaw;
|
||||
#endif
|
||||
|
||||
void stabilization_init(void)
|
||||
{
|
||||
for (uint8_t i = 0; i < COMMANDS_NB; i++) {
|
||||
stabilization_cmd[i] = 0;
|
||||
}
|
||||
|
||||
// Initialize low pass filters
|
||||
#if STABILIZATION_FILTER_CMD_ROLL_PITCH
|
||||
init_second_order_low_pass_int(&filter_roll, STABILIZATION_FILTER_CMD_ROLL_CUTOFF, 0.7071, 1.0 / PERIODIC_FREQUENCY,
|
||||
0.0);
|
||||
init_second_order_low_pass_int(&filter_pitch, STABILIZATION_FILTER_CMD_PITCH_CUTOFF, 0.7071, 1.0 / PERIODIC_FREQUENCY,
|
||||
0.0);
|
||||
#endif
|
||||
|
||||
#if STABILIZATION_FILTER_CMD_YAW
|
||||
init_second_order_low_pass_int(&filter_yaw, STABILIZATION_FILTER_CMD_YAW_CUTOFF, 0.7071, 1.0 / PERIODIC_FREQUENCY, 0.0);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
void stabilization_filter_commands(void)
|
||||
{
|
||||
/* Filter the commands & bound the result */
|
||||
#if STABILIZATION_FILTER_CMD_ROLL_PITCH
|
||||
stabilization_cmd[COMMAND_ROLL] = update_second_order_low_pass_int(&filter_roll, stabilization_cmd[COMMAND_ROLL]);
|
||||
stabilization_cmd[COMMAND_PITCH] = update_second_order_low_pass_int(&filter_pitch, stabilization_cmd[COMMAND_PITCH]);
|
||||
|
||||
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
|
||||
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
|
||||
#endif
|
||||
#if STABILIZATION_FILTER_CMD_YAW
|
||||
stabilization_cmd[COMMAND_YAW] = update_second_order_low_pass_int(&filter_yaw, stabilization_cmd[COMMAND_YAW]);
|
||||
|
||||
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -31,6 +31,7 @@
|
||||
#include "generated/airframe.h"
|
||||
|
||||
extern void stabilization_init(void);
|
||||
extern void stabilization_filter_commands(void);
|
||||
|
||||
/** Stabilization commands.
|
||||
* Contains the resulting stabilization commands,
|
||||
|
||||
Reference in New Issue
Block a user