mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-30 19:47:50 +08:00
Oneloop Controller Updates (#3261)
This commit is contained in:
Christophe De Wagter
@@ -0,0 +1,314 @@
|
||||
<!-- This is a 7kg Rotating Wing Drone C
|
||||
* Airframe: TUD00???
|
||||
* Autopilot: Cube orange
|
||||
* Datalink: Herelink
|
||||
* GPS: UBlox F9P
|
||||
* RC: SBUS Crossfire
|
||||
-->
|
||||
|
||||
<airframe name="RotatingWingV3C">
|
||||
<description>RotatingWingV3C for outdoor flight and simulation with INS EKF2</description>
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<autopilot name="rotorcraft_oneloop_andi_indi.xml"/>
|
||||
<target name="ap" board="cube_orange">
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/> <!-- Configure the main periodic frequency to 500Hz -->
|
||||
<configure name="NAVIGATION_FREQUENCY" value="500"/>
|
||||
<module name="radio_control" type="sbus">
|
||||
<configure name="SBUS_PORT" value="UART3"/> <!-- On the TELEM2 port -->
|
||||
</module>
|
||||
<module name="sys_mon"/>
|
||||
<module name="flight_recorder"/>
|
||||
|
||||
<module name="lidar_tfmini">
|
||||
<configure name="TFMINI_PORT" value="UART8"/>
|
||||
<configure name="USE_TFMINI_AGL" value="TRUE"/>
|
||||
</module>
|
||||
<!-- RC switches -->
|
||||
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
|
||||
<define name="RADIO_FMODE" value="RADIO_AUX2"/>
|
||||
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/>
|
||||
<define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/>
|
||||
<!-- EKF EXT POSE configure inputs -->
|
||||
<define name="INS_EXT_POSE_MAG_ID" vaue="MAG_RM3100_SENDER_ID"/>
|
||||
<define name="INS_EXT_POSE_IMU_ID" value="IMU_CUBE1_ID"/>
|
||||
<!--Only send gyro and accel that is being used-->
|
||||
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
|
||||
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
|
||||
<!--Use adc rot sensor-->
|
||||
<module name="wing_rotation_adc_sensor">
|
||||
<define name="ADC_OFFSET" value="-49.1731"/>
|
||||
<define name="ADC_SCALE" value="0.0029986"/>
|
||||
</module>
|
||||
<!-- Log in high speed (Remove for outdoor flights) -->
|
||||
<!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> -->
|
||||
<define name="I2C2_CLOCK_SPEED" value="100000"/>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/>
|
||||
<configure name="NAVIGATION_FREQUENCY" value="500"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
|
||||
<!--Not dealing with these in the simulation-->
|
||||
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
|
||||
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
|
||||
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
|
||||
<define name="RADIO_KILL_SWITCH" value="5"/> <!-- Kill switch -->
|
||||
<define name="RADIO_CONTROL_THRUST_X" value="0"/>
|
||||
</target>
|
||||
|
||||
<!-- Datalink -->
|
||||
<module name="telemetry" type="transparent">
|
||||
<configure name="MODEM_BAUD" value="B115200"/> <!-- herelink-->
|
||||
<!-- <configure VALUE="B57600" name="MODEM_BAUD"/> xBee -->
|
||||
</module>
|
||||
|
||||
<!-- Sensors -->
|
||||
<module name="mag" type="rm3100">
|
||||
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
|
||||
<configure name="MAG_RM3100_I2C_DEV" value="I2C2"/>
|
||||
</module>
|
||||
<module name="airspeed" type="ms45xx_i2c">
|
||||
<define name="MS45XX_I2C_DEV" value="i2c2"/>
|
||||
<define name="MS45XX_PRESSURE_SCALE" value="1.9077609"/>
|
||||
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
|
||||
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
|
||||
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
|
||||
</module>
|
||||
<module name="airspeed" type="uavcan">
|
||||
<define name="AIRSPEED_UAVCAN_LOWPASS_FILTER" value="TRUE" />
|
||||
<define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" />
|
||||
<define name="AIRSPEED_UAVCAN_LOWPASS_TAU" value="0.15" />
|
||||
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
|
||||
</module>
|
||||
<module name="air_data"/>
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="UBX_GPS_BAUD" value="B460800"/>
|
||||
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
|
||||
</module>
|
||||
|
||||
<!-- IMU / INS -->
|
||||
<module name="imu" type="cube"/>
|
||||
<module name="ins" type="ekf2"/>
|
||||
|
||||
<!-- Actuators on dual CAN bus -->
|
||||
<module name="actuators" type="uavcan">
|
||||
<configure value="TRUE" name="UAVCAN_USE_CAN1"/>
|
||||
<configure value="TRUE" name="UAVCAN_USE_CAN2"/>
|
||||
</module>
|
||||
|
||||
<!-- Actuators on PWM -->
|
||||
<module name="actuators" type="pwm" >
|
||||
<define name="SERVO_HZ" value="400"/>
|
||||
</module>
|
||||
|
||||
<!-- Control -->
|
||||
<module name="stabilization" type="oneloop" >
|
||||
<configure name="INDI_NUM_ACT" value="8"/>
|
||||
</module>
|
||||
|
||||
<module name="guidance" type="oneloop"/>
|
||||
<module name="nav" type="hybrid">
|
||||
<define name="NAV_HYBRID_MAX_AIRSPEED" value="5.0f"/>
|
||||
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
|
||||
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
|
||||
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
|
||||
</module>
|
||||
|
||||
<module name="oneloop" type="andi">
|
||||
<configure name="ANDI_NUM_ACT_TOT" value="7"/>
|
||||
<configure name="ANDI_NUM_ACT" value="5"/>
|
||||
<configure name="ANDI_NUM_VIRTUAL_ACT" value="2"/>
|
||||
<configure name="ANDI_OUTPUTS" value="6"/>
|
||||
</module>
|
||||
|
||||
<module name="wls">
|
||||
<define name="WLS_N_U" value = "7"/>
|
||||
<define name="WLS_N_V" value = "6"/>
|
||||
</module>
|
||||
|
||||
<module name="ground_detect_sensor"/>
|
||||
<!-- <module name="rotwing_state">
|
||||
<define name = "ROT_MECH_IDX" value = "9"/>
|
||||
<define name = "USE_ROTMECH_VIRTUAL" value = "FALSE"/>
|
||||
<define name = "ROTMECH_DYN" value = "3.0"/>
|
||||
</module> -->
|
||||
<module name="agl_dist"/>
|
||||
</firmware>
|
||||
|
||||
<!-- PWM actuators -->
|
||||
<servos driver="Pwm">
|
||||
<servo no="0" name="ROTATION_MECH" min="1360" neutral="1586" max="1812"/>
|
||||
</servos>
|
||||
|
||||
<!-- Can bus 1 actuators -->
|
||||
<servos driver="Uavcan1">
|
||||
<servo no="0" name="MOTOR_FRONT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="2" name="MOTOR_BACK" min="0" neutral="600" max="8191"/>
|
||||
<servo no="3" name="MOTOR_LEFT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="4" name="MOTOR_PUSH" min="0" neutral="200" max="8191"/>
|
||||
</servos>
|
||||
|
||||
<!-- CAN BUS 1 command outputs-->
|
||||
<servos driver="Uavcan1Cmd">
|
||||
<servo no="5" name="SERVO_ELEVATOR" min="3250" neutral="3250" max="-3250"/>
|
||||
<servo no="6" name="SERVO_RUDDER" min="-3250" neutral="0" max="3250"/>
|
||||
</servos>
|
||||
|
||||
<!-- Can bus 2 actuators -->
|
||||
<servos driver="Uavcan2">
|
||||
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="2" name="BMOTOR_BACK" min="0" neutral="600" max="8191"/>
|
||||
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="600" max="8191"/>
|
||||
</servos>
|
||||
|
||||
<!-- CAN BUS 2 command outputs-->
|
||||
<servos driver="Uavcan2Cmd">
|
||||
<servo no="7" name="AIL_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
|
||||
<servo no="8" name="FLAP_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
|
||||
<servo no="9" name="FLAP_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
|
||||
<servo no="10" name="AIL_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
|
||||
</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>
|
||||
|
||||
<auto_rc_commands>
|
||||
<set VALUE="@THROTTLE" COMMAND="THRUST"/>
|
||||
<set VALUE="@ROLL" COMMAND="ROLL"/>
|
||||
<set VALUE="@PITCH" COMMAND="PITCH"/>
|
||||
<set VALUE="@YAW" COMMAND="YAW"/>
|
||||
<set VALUE="@AUX4" COMMAND="THRUST_X"/>
|
||||
</auto_rc_commands>
|
||||
|
||||
<command_laws>
|
||||
<let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
|
||||
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[0])" SERVO="MOTOR_FRONT"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[1])" SERVO="MOTOR_RIGHT"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[2])" SERVO="MOTOR_BACK"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[3])" SERVO="MOTOR_LEFT"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : actuators_pprz[5])" SERVO="SERVO_RUDDER"/>
|
||||
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[6]))" SERVO="SERVO_ELEVATOR"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[4])" SERVO="MOTOR_PUSH"/>
|
||||
<!-- <set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="ROTATION_MECH"/> -->
|
||||
<set VALUE="-9600" SERVO="ROTATION_MECH"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="AIL_LEFT"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="AIL_RIGHT"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[8])" SERVO="FLAP_LEFT"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[8])" SERVO="FLAP_RIGHT"/>
|
||||
|
||||
<!-- Backup commands -->
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
|
||||
</command_laws>
|
||||
|
||||
<section NAME="MISC">
|
||||
<!-- Voltage and current measurements -->
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
|
||||
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
|
||||
<!-- Others -->
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
<define name="NAV_CLIMB_VSPEED" value="2.0" />
|
||||
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
|
||||
<define name="NAV_CARROT_DIST" value="15"/>
|
||||
<define name="CLOSE_TO_WAYPOINT" value="15"/>
|
||||
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="500"/>
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
|
||||
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
|
||||
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
|
||||
<define name="USE_AIRSPEED" value="TRUE"/>
|
||||
<define name="STABILIZATION_ATTITUDE_SP_MAX_PHI" value="45." unit="deg" />
|
||||
<define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" />
|
||||
<define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" />
|
||||
<define name="FWD_SIDESLIP_GAIN" value="2.0"/>
|
||||
</section>
|
||||
<section name="GROUND_DETECT">
|
||||
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
|
||||
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
|
||||
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
|
||||
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
|
||||
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
|
||||
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
|
||||
<define name="AGL_DIST_FILTER" value="0.07"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-12,-9,20}, .scale={{30726,37910,15728},{3133,3871,1611}}, .filter_sample_freq=1134.1, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-9,-30,26}, .scale={{45288,818,33359},{8935,167,6832}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-35,-5,13}, .scale={{26152,56165,62837},{5357,11479,12884}}}}"/>
|
||||
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-14,3,42}, .scale={{17279,2209,36874},{30247,3800,64095}}}}"/>
|
||||
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1134.1, .filter_freq=30}}"/>
|
||||
<define name="BODY_TO_IMU_PHI" value="-2.6" unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI">
|
||||
<define name = "G1_ROLL" value = "{ 0.0f, -13.0f, 0.0f, 13.0f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "G1_PITCH" value = "{ 1.6f, 0.0f, -1.6f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "G1_YAW" value = "{-0.4f, 0.4f, -0.4f, 0.4f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "G1_THRUST" value = "{-0.575f, -0.575f, -0.575f, -0.575f, 0.55f, 0.0f, 0.0f}"/>
|
||||
<define name = "G1_ZERO" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "G2" value = "{-0.02f, 0.02f, -0.02f, 0.02f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "MAX_R" value = "120.0" unit="deg/s"/>
|
||||
<define name = "FILT_CUTOFF" value = "3.0" />
|
||||
<define name = "FILT_CUTOFF_ACC" value = "5.0"/>
|
||||
<define name = "FILT_CUTOFF_VEL" value = "5.0"/>
|
||||
<define name = "FILT_CUTOFF_POS" value = "10.0"/>
|
||||
<define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" />
|
||||
<define name = "FILT_CUTOFF_P" value = "3.0"/>
|
||||
<define name = "FILT_CUTOFF_Q" value = "3.0"/>
|
||||
<define name = "FILT_CUTOFF_R" value = "3.0" />
|
||||
<define name = "FILT_CUTOFF_RDOT" value = "0.5" />
|
||||
<define name = "FILTER_YAW_RATE" value = "TRUE"/>
|
||||
<define name = "ACT_DYN" value = "{10.1f, 10.1f, 10.1f, 10.1f, 24.07f, 0.0f,0.0f}" />
|
||||
<define name = "ACT_IS_SERVO" value = "{0, 0, 0, 0, 0, 0, 0}" />
|
||||
<define name = "ACT_MAX" value = "{9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/>
|
||||
<define name = "ACT_MIN" value = "{0.0f , 0.0f, 0.0f, 0.0f, 0.0f, -M_PI_4, -M_PI_4}"/>
|
||||
<define name = "ACT_MAX_NORM" value = "{1.0f , 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/>
|
||||
<define name = "ACT_MIN_NORM" value = "{0.0f , 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, -1.0f}"/>
|
||||
<define name = "DEBUG_MODE" value = "FALSE"/>
|
||||
<define name = "AC_HAS_PUSHER" value = "TRUE"/>
|
||||
<define name = "PUSHER_IDX" value = "4"/>
|
||||
<define name = "WV" value = "{1.0f, 1.0f, 1.0f, 1000.0f, 1000.0f, 100.0f}"/>
|
||||
<define name = "WU" value = "{0.75f, 0.75f, 0.75f, 0.75f, 1.0f, 2.0f, 2.0f}"/>
|
||||
<define name = "U_PREF" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
|
||||
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
|
||||
<define name="BAT_NB_CELLS" value="6"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, pusher" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="rotwingv3c_SI" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
<define name="COMMANDS_NB" value="5"/>
|
||||
<define name="NO_MOTOR_MIXING" value="TRUE"/>
|
||||
<define name="JS_AXIS_MODE" value="4"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -0,0 +1,315 @@
|
||||
<!-- This is a 7kg Rotating Wing Drone C
|
||||
* Airframe: TUD00???
|
||||
* Autopilot: Cube orange
|
||||
* Datalink: Herelink
|
||||
* GPS: UBlox F9P
|
||||
* RC: SBUS Crossfire
|
||||
-->
|
||||
|
||||
<airframe name="RotatingWingV3C">
|
||||
<description>RotatingWingV3C with optitrack and INS ext pose</description>
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<autopilot name="rotorcraft_oneloop_andi_indi.xml"/>
|
||||
<target name="ap" board="cube_orange">
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/> <!-- Configure the main periodic frequency to 500Hz -->
|
||||
<configure name="NAVIGATION_FREQUENCY" value="500"/>
|
||||
<module name="radio_control" type="sbus">
|
||||
<configure name="SBUS_PORT" value="UART3"/> <!-- On the TELEM2 port -->
|
||||
</module>
|
||||
<module name="sys_mon"/>
|
||||
<module name="flight_recorder"/>
|
||||
|
||||
<module name="lidar_tfmini">
|
||||
<configure name="TFMINI_PORT" value="UART8"/>
|
||||
<configure name="USE_TFMINI_AGL" value="TRUE"/>
|
||||
</module>
|
||||
<!-- RC switches -->
|
||||
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
|
||||
<define name="RADIO_FMODE" value="RADIO_AUX2"/>
|
||||
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/>
|
||||
<define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/>
|
||||
<!-- EKF EXT POSE configure inputs -->
|
||||
<define name="INS_EXT_POSE_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
|
||||
<define name="INS_EXT_POSE_IMU_ID" value="IMU_CUBE1_ID"/>
|
||||
<!--Only send gyro and accel that is being used-->
|
||||
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
|
||||
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
|
||||
<!--Use adc rot sensor-->
|
||||
<module name="wing_rotation_adc_sensor">
|
||||
<define name="ADC_OFFSET" value="-49.1731"/>
|
||||
<define name="ADC_SCALE" value="0.0029986"/>
|
||||
</module>
|
||||
<!-- Log in high speed (Remove for outdoor flights) -->
|
||||
<!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> -->
|
||||
<define name="I2C2_CLOCK_SPEED" value="100000"/>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/>
|
||||
<configure name="NAVIGATION_FREQUENCY" value="500"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
|
||||
<!--Not dealing with these in the simulation-->
|
||||
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
|
||||
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
|
||||
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
|
||||
<define name="RADIO_KILL_SWITCH" value="5"/> <!-- Kill switch -->
|
||||
<define name="RADIO_CONTROL_THRUST_X" value="0"/>
|
||||
</target>
|
||||
|
||||
<!-- Datalink -->
|
||||
<module name="telemetry" type="transparent">
|
||||
<configure name="MODEM_BAUD" value="B115200"/> <!-- herelink-->
|
||||
<!-- <configure VALUE="B57600" name="MODEM_BAUD"/> xBee -->
|
||||
</module>
|
||||
|
||||
<!-- Sensors -->
|
||||
<module name="mag" type="rm3100">
|
||||
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
|
||||
<configure name="MAG_RM3100_I2C_DEV" value="I2C2"/>
|
||||
</module>
|
||||
<module name="airspeed" type="ms45xx_i2c">
|
||||
<define name="MS45XX_I2C_DEV" value="i2c2"/>
|
||||
<define name="MS45XX_PRESSURE_SCALE" value="1.9077609"/>
|
||||
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
|
||||
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
|
||||
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
|
||||
</module>
|
||||
<module name="airspeed" type="uavcan">
|
||||
<define name="AIRSPEED_UAVCAN_LOWPASS_FILTER" value="TRUE" />
|
||||
<define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" />
|
||||
<define name="AIRSPEED_UAVCAN_LOWPASS_TAU" value="0.15" />
|
||||
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
|
||||
</module>
|
||||
<module name="air_data"/>
|
||||
<module name="gps" type="datalink"/>
|
||||
|
||||
<!-- IMU / INS -->
|
||||
<module name="imu" type="cube"/>
|
||||
<module name="ins" type="ext_pose"/>
|
||||
|
||||
<!-- Actuators on dual CAN bus -->
|
||||
<module name="actuators" type="uavcan">
|
||||
<configure value="TRUE" name="UAVCAN_USE_CAN1"/>
|
||||
<configure value="TRUE" name="UAVCAN_USE_CAN2"/>
|
||||
</module>
|
||||
|
||||
<!-- Actuators on PWM -->
|
||||
<module name="actuators" type="pwm" >
|
||||
<define name="SERVO_HZ" value="400"/>
|
||||
</module>
|
||||
|
||||
<!-- Control -->
|
||||
<module name="stabilization" type="oneloop" >
|
||||
<configure name="INDI_NUM_ACT" value="9"/>
|
||||
</module>
|
||||
|
||||
<module name="guidance" type="oneloop"/>
|
||||
<module name="nav" type="hybrid">
|
||||
<define name="NAV_HYBRID_MAX_AIRSPEED" value="5.0f"/>
|
||||
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
|
||||
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
|
||||
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
|
||||
<define name="NAV_HYBRID_GOTO_MODE" value="NAV_SETPOINT_MODE_POS"/>
|
||||
</module>
|
||||
|
||||
<module name="oneloop" type="andi">
|
||||
<configure name="ANDI_NUM_ACT_TOT" value="7"/>
|
||||
<configure name="ANDI_NUM_ACT" value="5"/>
|
||||
<configure name="ANDI_NUM_VIRTUAL_ACT" value="2"/>
|
||||
<configure name="ANDI_OUTPUTS" value="6"/>
|
||||
</module>
|
||||
|
||||
<module name="wls">
|
||||
<define name="WLS_N_U" value = "7"/>
|
||||
<define name="WLS_N_V" value = "6"/>
|
||||
</module>
|
||||
|
||||
<module name="ground_detect_sensor"/>
|
||||
<!-- <module name="rotwing_state">
|
||||
<define name = "ROT_MECH_IDX" value = "9"/>
|
||||
<define name = "USE_ROTMECH_VIRTUAL" value = "FALSE"/>
|
||||
<define name = "ROTMECH_DYN" value = "3.0"/>
|
||||
</module> -->
|
||||
<module name="agl_dist"/>
|
||||
</firmware>
|
||||
|
||||
<!-- PWM actuators -->
|
||||
<servos driver="Pwm">
|
||||
<servo no="0" name="ROTATION_MECH" min="1360" neutral="1586" max="1812"/>
|
||||
</servos>
|
||||
|
||||
<!-- Can bus 1 actuators -->
|
||||
<servos driver="Uavcan1">
|
||||
<servo no="0" name="MOTOR_FRONT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="2" name="MOTOR_BACK" min="0" neutral="600" max="8191"/>
|
||||
<servo no="3" name="MOTOR_LEFT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="4" name="MOTOR_PUSH" min="0" neutral="200" max="8191"/>
|
||||
</servos>
|
||||
|
||||
<!-- CAN BUS 1 command outputs-->
|
||||
<servos driver="Uavcan1Cmd">
|
||||
<servo no="5" name="SERVO_ELEVATOR" min="3980" neutral="3980" max="-5280"/>
|
||||
<servo no="6" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/>
|
||||
</servos>
|
||||
|
||||
<!-- Can bus 2 actuators -->
|
||||
<servos driver="Uavcan2">
|
||||
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="600" max="8191"/>
|
||||
<servo no="2" name="BMOTOR_BACK" min="0" neutral="600" max="8191"/>
|
||||
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="600" max="8191"/>
|
||||
</servos>
|
||||
|
||||
<!-- CAN BUS 2 command outputs-->
|
||||
<servos driver="Uavcan2Cmd">
|
||||
<servo no="7" name="AIL_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
|
||||
<servo no="8" name="FLAP_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
|
||||
<servo no="9" name="FLAP_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
|
||||
<servo no="10" name="AIL_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
|
||||
</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>
|
||||
|
||||
<auto_rc_commands>
|
||||
<set VALUE="@THROTTLE" COMMAND="THRUST"/>
|
||||
<set VALUE="@ROLL" COMMAND="ROLL"/>
|
||||
<set VALUE="@PITCH" COMMAND="PITCH"/>
|
||||
<set VALUE="@YAW" COMMAND="YAW"/>
|
||||
<set VALUE="@AUX4" COMMAND="THRUST_X"/>
|
||||
</auto_rc_commands>
|
||||
|
||||
<command_laws>
|
||||
<let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
|
||||
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[0])" SERVO="MOTOR_FRONT"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[1])" SERVO="MOTOR_RIGHT"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[2])" SERVO="MOTOR_BACK"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[3])" SERVO="MOTOR_LEFT"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : actuators_pprz[5])" SERVO="SERVO_RUDDER"/>
|
||||
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[6]))" SERVO="SERVO_ELEVATOR"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[4])" SERVO="MOTOR_PUSH"/>
|
||||
<!-- <set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="ROTATION_MECH"/> -->
|
||||
<set VALUE="-9600" SERVO="ROTATION_MECH"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="AIL_LEFT"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="AIL_RIGHT"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[8])" SERVO="FLAP_LEFT"/>
|
||||
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[8])" SERVO="FLAP_RIGHT"/>
|
||||
|
||||
<!-- Backup commands -->
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
|
||||
<set VALUE="($th_hold? -9600 : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
|
||||
</command_laws>
|
||||
|
||||
<section NAME="MISC">
|
||||
<!-- Voltage and current measurements -->
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
|
||||
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
|
||||
<!-- Others -->
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
<define name="NAV_CLIMB_VSPEED" value="2.0" />
|
||||
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
|
||||
<define name="NAV_CARROT_DIST" value="15"/>
|
||||
<define name="CLOSE_TO_WAYPOINT" value="15"/>
|
||||
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="500"/>
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
|
||||
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
|
||||
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
|
||||
<define name="USE_AIRSPEED" value="TRUE"/>
|
||||
<define name="STABILIZATION_ATTITUDE_SP_MAX_PHI" value="45." unit="deg" />
|
||||
<define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" />
|
||||
<define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" />
|
||||
<define name="FWD_SIDESLIP_GAIN" value="2.0"/>
|
||||
<!-- <define name="THRESHOLD_GROUND_DETECT" value="40"/> -->
|
||||
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
|
||||
</section>
|
||||
<section name="GROUND_DETECT">
|
||||
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
|
||||
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
|
||||
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
|
||||
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
|
||||
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
|
||||
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
|
||||
<define name="AGL_DIST_FILTER" value="0.07"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-12,-9,20}, .scale={{30726,37910,15728},{3133,3871,1611}}, .filter_sample_freq=1134.1, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-9,-30,26}, .scale={{45288,818,33359},{8935,167,6832}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-35,-5,13}, .scale={{26152,56165,62837},{5357,11479,12884}}}}"/>
|
||||
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-14,3,42}, .scale={{17279,2209,36874},{30247,3800,64095}}}}"/>
|
||||
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1134.1, .filter_freq=30}}"/>
|
||||
<define name="BODY_TO_IMU_PHI" value="-4.54" unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="1.84" unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI">
|
||||
<define name = "G1_ROLL" value = "{ 0.0f, -13.0f, 0.0f, 13.0f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "G1_PITCH" value = "{ 1.6f, 0.0f, -1.6f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "G1_YAW" value = "{-0.4f, 0.4f, -0.4f, 0.4f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "G1_THRUST" value = "{-0.575f, -0.575f, -0.575f, -0.575f, 0.52f, 0.0f, 0.0f}"/>
|
||||
<define name = "G1_ZERO" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "G2" value = "{-0.02f, 0.02f, -0.02f, 0.02f, 0.0f, 0.0f, 0.0f}"/>
|
||||
<define name = "MAX_R" value = "120.0" unit="deg/s"/>
|
||||
<define name = "FILT_CUTOFF" value = "3.0" />
|
||||
<define name = "FILT_CUTOFF_ACC" value = "5.0"/>
|
||||
<define name = "FILT_CUTOFF_VEL" value = "5.0"/>
|
||||
<define name = "FILT_CUTOFF_POS" value = "10.0"/>
|
||||
<define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" />
|
||||
<define name = "FILT_CUTOFF_P" value = "3.0"/>
|
||||
<define name = "FILT_CUTOFF_Q" value = "3.0"/>
|
||||
<define name = "FILT_CUTOFF_R" value = "3.0" />
|
||||
<define name = "FILT_CUTOFF_RDOT" value = "0.5" />
|
||||
<define name = "FILTER_YAW_RATE" value = "TRUE"/>
|
||||
<define name = "ACT_DYN" value = "{10.1f, 10.1f, 10.1f, 10.1f, 24.07f, 0.0f,0.0f}" />
|
||||
<define name = "ACT_IS_SERVO" value = "{0, 0, 0, 0, 0, 0, 0}" />
|
||||
<define name = "ACT_MAX" value = "{9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/>
|
||||
<define name = "ACT_MIN" value = "{0.0f , 0.0f, 0.0f, 0.0f, 0.0f, -M_PI_4, -M_PI_4}"/>
|
||||
<define name = "ACT_MAX_NORM" value = "{1.0f , 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/>
|
||||
<define name = "ACT_MIN_NORM" value = "{0.0f , 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, -1.0f}"/>
|
||||
<define name = "DEBUG_MODE" value = "FALSE"/>
|
||||
<define name = "AC_HAS_PUSHER" value = "TRUE"/>
|
||||
<define name = "PUSHER_IDX" value = "4"/>
|
||||
<define name = "WV" value = "{1.0f, 1.0f, 1.0f, 1000.0f, 1000.0f, 100.0f}"/>
|
||||
<define name = "WU" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 10.0f, 1.0f}"/>
|
||||
<!-- <define name = "WU" value = "{0.75f, 0.75f, 0.75f, 0.75f, 1.0f, 2.0f, 2.0f}"/> -->
|
||||
<define name = "U_PREF" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<!-- <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="TRUE"/> -->
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
|
||||
<define name="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
|
||||
<define name="BAT_NB_CELLS" value="6"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, pusher, rudder, elevator, aileron_left, aileron_right" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="rotwingv3c" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
<define name="COMMANDS_NB" value="9"/>
|
||||
<define name="NO_MOTOR_MIXING" value="TRUE"/>
|
||||
<define name="JS_AXIS_MODE" value="4"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -122,7 +122,7 @@
|
||||
<mode name="MODULE" shortname="ONE">
|
||||
<select cond="RCMode1() && DLModeModule()" exception="HOME"/>
|
||||
<on_enter>
|
||||
<call fun="oneloop_andi_enter(false)"/>
|
||||
<call fun="oneloop_andi_enter(false, CTRL_ANDI)"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_periodic_task()"/>
|
||||
|
||||
@@ -0,0 +1,191 @@
|
||||
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
|
||||
|
||||
<autopilot name="Oneloop Autopilot Rotorcraft">
|
||||
|
||||
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
|
||||
|
||||
<includes>
|
||||
<include name="generated/airframe.h"/>
|
||||
<include name="autopilot.h"/>
|
||||
<include name="autopilot_rc_helpers.h"/>
|
||||
<include name="navigation.h"/>
|
||||
<include name="guidance.h"/>
|
||||
<include name="oneloop/oneloop_andi.h"/>
|
||||
<include name="stabilization/stabilization_attitude.h"/>
|
||||
<include name="modules/radio_control/radio_control.h"/>
|
||||
<include name="modules/gps/gps.h"/>
|
||||
<include name="modules/actuators/actuators.h"/>
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT" cond="ifndef MODE_MANUAL"/>
|
||||
<define name="MODE_ONELOOP" value="AP_MODE_MODULE" cond="ifndef MODE_AUTO1"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
|
||||
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
|
||||
<define name="DLModeNav()" value="(autopilot_mode_auto2 == AP_MODE_NAV)"/>
|
||||
<define name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
|
||||
<define name="DLModeModule()" value="(MODE_AUTO1 == AP_MODE_MODULE)"/>
|
||||
<define name="DLModeAZH()" value="(MODE_AUTO1 == AP_MODE_ATTITUDE_Z_HOLD)"/>
|
||||
<define name="ONELOOP_CONTROLLER" value="TRUE"/>
|
||||
</includes>
|
||||
|
||||
<settings>
|
||||
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
|
||||
<dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
|
||||
</settings>
|
||||
|
||||
<control_block name="set_commands">
|
||||
<call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
|
||||
</control_block>
|
||||
|
||||
<control_block name="attitude_loop">
|
||||
<call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/>
|
||||
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
|
||||
</control_block>
|
||||
|
||||
<control_block name="throttle_direct">
|
||||
<call fun="guidance_v_read_rc()"/>
|
||||
<call fun="guidance_v_set_z(stateGetPositionNed_i()->z)"/>
|
||||
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
|
||||
</control_block>
|
||||
|
||||
<control_block name="altitude_loop">
|
||||
<call fun="gv_update_ref_from_z_sp(guidance_v.z_sp)"/>
|
||||
<call fun="guidance_v.delta_t = guidance_v_run_pos(autopilot_in_flight(), &guidance_v)"/>
|
||||
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
|
||||
</control_block>
|
||||
|
||||
<exceptions>
|
||||
<exception cond="nav.too_far_from_home" deroute="HOME"/>
|
||||
<exception cond="kill_switch_is_on()" deroute="KILL"/>
|
||||
</exceptions>
|
||||
|
||||
<mode name="ATTITUDE_DIRECT" shortname="ATT">
|
||||
<select cond="RCMode0()"/>
|
||||
<on_enter>
|
||||
<call fun="stabilization_attitude_enter()"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_periodic_task()"/>
|
||||
</control>
|
||||
<control>
|
||||
<call_block name="attitude_loop"/>
|
||||
<call_block name="throttle_direct"/>
|
||||
<call_block name="set_commands"/>
|
||||
</control>
|
||||
<exception cond="RCLost()" deroute="FAILSAFE"/>
|
||||
</mode>
|
||||
|
||||
<mode name="NAV">
|
||||
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
|
||||
<on_enter>
|
||||
<call fun="oneloop_andi_enter(false, CTRL_INDI)"/>
|
||||
</on_enter>
|
||||
<control>
|
||||
<call fun="nav_periodic_task()"/>
|
||||
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
|
||||
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
|
||||
</control>
|
||||
<exception cond="RCLost()" deroute="FAILSAFE"/>
|
||||
</mode>
|
||||
|
||||
<mode name="GUIDED">
|
||||
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
|
||||
<on_enter>
|
||||
<call fun="guidance_h_hover_enter()"/>
|
||||
<call fun="stabilization_attitude_enter()"/>
|
||||
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
|
||||
<call fun="guidance_v_guided_enter()"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_periodic_task()"/>
|
||||
</control>
|
||||
<control>
|
||||
<call fun="guidance_v_read_rc()"/>
|
||||
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
|
||||
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
|
||||
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
|
||||
<call_block name="set_commands"/>
|
||||
</control>
|
||||
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
|
||||
</mode>
|
||||
|
||||
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
|
||||
<select cond="RCMode1() && DLModeAZH()"/>
|
||||
<on_enter>
|
||||
<call fun="guidance_h_hover_enter()"/>
|
||||
<call fun="stabilization_attitude_enter()"/>
|
||||
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
|
||||
<call fun="guidance_v_guided_enter()"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_periodic_task()"/>
|
||||
</control>
|
||||
<control>
|
||||
<call fun="guidance_v_read_rc()"/>
|
||||
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
|
||||
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
|
||||
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
|
||||
<call_block name="set_commands"/>
|
||||
</control>
|
||||
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
|
||||
</mode>
|
||||
|
||||
<mode name="MODULE" shortname="ONE">
|
||||
<select cond="RCMode1() && DLModeModule()" exception="HOME"/>
|
||||
<on_enter>
|
||||
<call fun="oneloop_andi_enter(false, CTRL_ANDI)"/>
|
||||
</on_enter>
|
||||
<control>
|
||||
<call fun="nav_periodic_task()"/>
|
||||
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
|
||||
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
|
||||
</control>
|
||||
<exception cond="RCLost()" deroute="FAILSAFE"/>
|
||||
</mode>
|
||||
|
||||
<mode name="HOME">
|
||||
<on_enter>
|
||||
<call fun="guidance_h_nav_enter()"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_home()"/>
|
||||
</control>
|
||||
<control>
|
||||
<call fun="guidance_v_read_rc()"/>
|
||||
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
|
||||
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
|
||||
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
|
||||
</control>
|
||||
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
|
||||
</mode>
|
||||
|
||||
<!-- Safe landing -->
|
||||
<mode name="FAILSAFE" shortname="FAIL">
|
||||
<!-- Failsafe does not work needs to be fixed-->
|
||||
<on_enter>
|
||||
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
|
||||
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
|
||||
</on_enter>
|
||||
<control>
|
||||
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
|
||||
<call fun="guidance_v_update_ref()"/>
|
||||
<call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
|
||||
<call_block name="set_commands"/>
|
||||
</control>
|
||||
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
|
||||
</mode>
|
||||
|
||||
<!-- Kill throttle mode -->
|
||||
<mode name="KILL">
|
||||
<select cond="kill_switch_is_on()"/>
|
||||
<on_enter>
|
||||
<call fun="autopilot_set_in_flight(false)"/>
|
||||
<call fun="autopilot_set_motors_on(false)"/>
|
||||
<call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
|
||||
</on_enter>
|
||||
<control>
|
||||
<call fun="SetCommands(commands_failsafe)"/>
|
||||
</control>
|
||||
</mode>
|
||||
|
||||
</state_machine>
|
||||
|
||||
</autopilot>
|
||||
@@ -23,6 +23,7 @@
|
||||
<define name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
|
||||
<define name="DLModeModule()" value="(MODE_AUTO1 == AP_MODE_MODULE)"/>
|
||||
<define name="DLModeAZH()" value="(MODE_AUTO1 == AP_MODE_ATTITUDE_Z_HOLD)"/>
|
||||
<define name="ONELOOP_CONTROLLER" value="TRUE"/>
|
||||
</includes>
|
||||
|
||||
<settings>
|
||||
@@ -74,9 +75,9 @@
|
||||
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
|
||||
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<!-- <control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_periodic_task()"/>
|
||||
</control>
|
||||
</control> -->
|
||||
<control>
|
||||
<call_block name="run_guidance_control"/>
|
||||
<call_block name="set_commands"/>
|
||||
@@ -121,12 +122,14 @@
|
||||
<mode name="MODULE" shortname="ONE">
|
||||
<select cond="RCMode1() && DLModeModule()" exception="HOME"/>
|
||||
<on_enter>
|
||||
<call fun="oneloop_andi_enter(false)"/>
|
||||
<call fun="oneloop_andi_enter(false, CTRL_ANDI)"/>
|
||||
</on_enter>
|
||||
<control freq="NAVIGATION_FREQUENCY">
|
||||
<!-- <control freq="NAVIGATION_FREQUENCY">
|
||||
<call fun="nav_periodic_task()"/>
|
||||
</control>
|
||||
</control> -->
|
||||
<control>
|
||||
<call fun="nav_periodic_task()"/>
|
||||
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
|
||||
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
|
||||
</control>
|
||||
<exception cond="RCLost()" deroute="FAILSAFE"/>
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="8.0" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="5000" name="Oneloop Valkenburg" security_height="2">
|
||||
<flight_plan alt="70.0" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="1070" name="Oneloop Valkenburg" security_height="2">
|
||||
<header>
|
||||
#include "autopilot.h"
|
||||
#include "modules/datalink/datalink.h"
|
||||
@@ -9,15 +9,15 @@
|
||||
#include "modules/ahrs/ahrs.h"
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" lat="52.1682196" lon="4.4134865"/>
|
||||
<waypoint name="HOME" x="12.6" y="-48.7"/>
|
||||
<waypoint name="CLIMB" x="62.6" y="-80.4"/>
|
||||
<waypoint name="STDBY" x="106.0" y="-55.1"/>
|
||||
<waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/>
|
||||
<waypoint name="p1" x="322.0" y="-254.3"/>
|
||||
<waypoint name="p2" x="696.0" y="-148.1"/>
|
||||
<waypoint name="p3" x="600.4" y="73.4"/>
|
||||
<waypoint name="p4" x="238.6" y="-48.5"/>
|
||||
<waypoint name="TD" x="-4.1" y="-23.7"/>
|
||||
<waypoint name="FOLLOW" x="123.2" y="-87.8"/>
|
||||
<waypoint name="TD" lat="52.1681602" lon="4.4127708"/>
|
||||
<waypoint name="FOLLOW" lat="52.16850964562752" lon="4.413635008734417"/>
|
||||
<waypoint name="S1" lat="52.1669450" lon="4.4174372"/>
|
||||
<waypoint name="S2" lat="52.1689871" lon="4.4208804"/>
|
||||
<waypoint lat="52.169189" lon="4.410820" name="C1"/>
|
||||
|
||||
@@ -27,6 +27,7 @@
|
||||
|
||||
<makefile target="ap">
|
||||
<define name="INS_TYPE_H" value="modules/ins/ins_ext_pose.h" type="string"/>
|
||||
<define name="INS_EXT_POSE" value="TRUE"/>
|
||||
<file name="ins_ext_pose.c"/>
|
||||
<file name="ins.c"/>
|
||||
</makefile>
|
||||
|
||||
@@ -6,15 +6,34 @@
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="oneloop">
|
||||
<dl_setting var="p_att_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_att_e_omega_n"/>
|
||||
<dl_setting var="p_att_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_att_e_zeta"/>
|
||||
<dl_setting var="p_head_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_head_e_omega_n"/>
|
||||
<dl_setting var="p_head_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_head_e_zeta"/>
|
||||
<dl_setting var="p_pos_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_pos_e_omega_n"/>
|
||||
<dl_setting var="p_pos_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_pos_e_zeta"/>
|
||||
<dl_setting var="p_alt_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_alt_e_omega_n"/>
|
||||
<dl_setting var="p_alt_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_alt_e_zeta"/>
|
||||
<dl_setting var="psi_des_deg" min="-180.0" step="0.1" max="180.0" shortname="psi_des"/>
|
||||
<dl_setting var="chirp_on" min="0" step="1" max="1" values="OFF|ON" shortname="chirp_on"/>
|
||||
<dl_setting var="chirp_axis" min="0" step="1" max="3" shortname="chirp_axis"/>
|
||||
<dl_setting var="f0_chirp" min="0.01" step="0.001" max="10" shortname="f0_chirp"/>
|
||||
<dl_setting var="f1_chirp" min="0.01" step="0.001" max="10" shortname="f1_chirp"/>
|
||||
<dl_setting var="t_chirp" min="0.01" step="0.001" max="60" shortname="t_chirp"/>
|
||||
<dl_setting var="A_chirp" min="0.01" step="0.001" max="10" shortname="A_chirp"/>
|
||||
<dl_setting var="heading_manual" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/>
|
||||
<dl_setting var="yaw_stick_in_auto" min="0" step="1" max="1" values="OFF|ON" shortname="yaw_stick_on"/>
|
||||
<dl_setting var="fwd_sideslip_gain" min="0.1" step="0.001" max="20.0" shortname="fwd_sideslip_gain"/>
|
||||
<dl_setting var="psi_des_deg" min="-180.0" step="0.1" max="180.0" shortname="psi_des"/>
|
||||
<dl_setting var="p_att_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_att_e_omega_n"/>
|
||||
<dl_setting var="p_att_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_att_e_zeta"/>
|
||||
<dl_setting var="p_att_rm.omega_n" min="0.1" step="0.001" max="70.0" shortname="p_att_rm_omega_n"/>
|
||||
<dl_setting var="p_att_rm.zeta" min="0.1" step="0.001" max="1.0" shortname="p_att_rm_zeta"/>
|
||||
<dl_setting var="p_att_rm.p3" min="0.1" step="0.001" max="70.0" shortname="p_att_rm_p3"/>
|
||||
<dl_setting var="p_head_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_head_e_omega_n"/>
|
||||
<dl_setting var="p_head_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_head_e_zeta"/>
|
||||
<dl_setting var="p_head_rm.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_head_rm_omega_n"/>
|
||||
<dl_setting var="p_head_rm.zeta" min="0.1" step="0.001" max="1.0" shortname="p_head_rm_zeta"/>
|
||||
<dl_setting var="p_pos_e.omega_n" min="0.1" step="0.001" max="70.0" shortname="p_pos_e_omega_n"/>
|
||||
<dl_setting var="p_pos_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_pos_e_zeta"/>
|
||||
<dl_setting var="p_pos_e.p3" min="0.1" step="0.001" max="70.0" shortname="p_pos_e_p3"/>
|
||||
<dl_setting var="p_pos_rm.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_pos_rm_omega_n"/>
|
||||
<dl_setting var="p_pos_rm.zeta" min="0.1" step="0.001" max="1.0" shortname="p_pos_rm_zeta"/>
|
||||
<dl_setting var="p_alt_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_alt_e_omega_n"/>
|
||||
<dl_setting var="p_alt_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_alt_e_zeta"/>
|
||||
<dl_setting var="p_alt_rm.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_alt_rm_omega_n"/>
|
||||
<dl_setting var="p_alt_rm.zeta" min="0.1" step="0.001" max="1.0" shortname="p_alt_rm_zeta"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
@@ -0,0 +1,485 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?>
|
||||
<fdm_config name="QUAD COMPLETE EXT" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd">
|
||||
|
||||
<fileheader>
|
||||
<author>Tomaso De Ponti</author>
|
||||
<filecreationdate>07-03-2017</filecreationdate>
|
||||
<version>Version 0.9 - beta</version>
|
||||
<description>RW3C with actuator dynamics</description>
|
||||
</fileheader>
|
||||
|
||||
<metrics>
|
||||
<wingarea unit="M2"> 0 </wingarea>
|
||||
<wingspan unit="M"> 0 </wingspan>
|
||||
<chord unit="M"> 0 </chord>
|
||||
<htailarea unit="M2"> 0 </htailarea>
|
||||
<htailarm unit="M"> 0 </htailarm>
|
||||
<vtailarea unit="M2"> 0 </vtailarea>
|
||||
<vtailarm unit="M"> 0 </vtailarm>
|
||||
<location name="AERORP" unit="M">
|
||||
<x> 0 </x>
|
||||
<y> 0 </y>
|
||||
<z> 0 </z>
|
||||
</location>
|
||||
<location name="EYEPOINT" unit="M">
|
||||
<x> 0 </x>
|
||||
<y> 0 </y>
|
||||
<z> 0 </z>
|
||||
</location>
|
||||
<location name="VRP" unit="M">
|
||||
<x> 0 </x>
|
||||
<y> 0 </y>
|
||||
<z> 0 </z>
|
||||
</location>
|
||||
</metrics>
|
||||
|
||||
<mass_balance>
|
||||
<ixx unit="KG*M2"> 0.126323</ixx>
|
||||
<iyy unit="KG*M2"> 1.064109</iyy>
|
||||
<izz unit="KG*M2"> 1.00000</izz>
|
||||
<ixy unit="KG*M2"> 0.0 </ixy>
|
||||
<ixz unit="KG*M2"> 0.0 </ixz>
|
||||
<iyz unit="KG*M2"> 0.0 </iyz>
|
||||
<emptywt unit="KG"> 7.97 </emptywt>
|
||||
<location name="CG" unit="M">
|
||||
<x> 0 </x>
|
||||
<y> 0 </y>
|
||||
<z> 0 </z>
|
||||
</location>
|
||||
</mass_balance>
|
||||
|
||||
<ground_reactions>
|
||||
<contact type="STRUCTURE" name="CONTACT_FRONT">
|
||||
<location unit="M">
|
||||
<x>-0.15 </x>
|
||||
<y> 0 </y>
|
||||
<z>-0.1 </z>
|
||||
</location>
|
||||
<static_friction> 0.8 </static_friction>
|
||||
<dynamic_friction> 0.5 </dynamic_friction>
|
||||
<spring_coeff unit="N/M"> 500 </spring_coeff>
|
||||
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
|
||||
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
|
||||
<max_steer unit="DEG"> 0.0 </max_steer>
|
||||
<brake_group> NONE </brake_group>
|
||||
<retractable>0</retractable>
|
||||
</contact>
|
||||
|
||||
<contact type="STRUCTURE" name="CONTACT_BACK">
|
||||
<location unit="M">
|
||||
<x> 0.15</x>
|
||||
<y> 0 </y>
|
||||
<z>-0.1 </z>
|
||||
</location>
|
||||
<static_friction> 0.8 </static_friction>
|
||||
<dynamic_friction> 0.5 </dynamic_friction>
|
||||
<spring_coeff unit="N/M"> 500 </spring_coeff>
|
||||
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
|
||||
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
|
||||
<max_steer unit="DEG"> 0.0 </max_steer>
|
||||
<brake_group> NONE </brake_group>
|
||||
<retractable>0</retractable>
|
||||
</contact>
|
||||
|
||||
<contact type="STRUCTURE" name="CONTACT_RIGHT">
|
||||
<location unit="M">
|
||||
<x> 0. </x>
|
||||
<y> 0.15</y>
|
||||
<z>-0.1 </z>
|
||||
</location>
|
||||
<static_friction> 0.8 </static_friction>
|
||||
<dynamic_friction> 0.5 </dynamic_friction>
|
||||
<spring_coeff unit="N/M"> 500 </spring_coeff>
|
||||
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
|
||||
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
|
||||
<max_steer unit="DEG"> 0.0 </max_steer>
|
||||
<brake_group> NONE </brake_group>
|
||||
<retractable>0</retractable>
|
||||
</contact>
|
||||
|
||||
<contact type="STRUCTURE" name="CONTACT_LEFT">
|
||||
<location unit="M">
|
||||
<x> 0. </x>
|
||||
<y>-0.15</y>
|
||||
<z>-0.1 </z>
|
||||
</location>
|
||||
<static_friction> 0.8 </static_friction>
|
||||
<dynamic_friction> 0.5 </dynamic_friction>
|
||||
<spring_coeff unit="N/M"> 500 </spring_coeff>
|
||||
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
|
||||
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
|
||||
<max_steer unit="DEG"> 0.0 </max_steer>
|
||||
<brake_group> NONE </brake_group>
|
||||
<retractable>0</retractable>
|
||||
</contact>
|
||||
</ground_reactions>
|
||||
|
||||
<flight_control name="actuator_dynamics">
|
||||
<channel name="filtering">
|
||||
|
||||
<!--First order filter represents actuator dynamics-->
|
||||
<lag_filter name="front_motor_lag">
|
||||
<input> fcs/front_motor </input>
|
||||
<c1> 10.1 </c1>
|
||||
<output> fcs/front_motor_lag</output>
|
||||
</lag_filter>
|
||||
<lag_filter name="right_motor_lag">
|
||||
<input> fcs/right_motor </input>
|
||||
<c1> 10.1 </c1>
|
||||
<output> fcs/right_motor_lag</output>
|
||||
</lag_filter>
|
||||
<lag_filter name="back_motor_lag">
|
||||
<input> fcs/back_motor </input>
|
||||
<c1> 10.1 </c1>
|
||||
<output> fcs/back_motor_lag</output>
|
||||
</lag_filter>
|
||||
<lag_filter name="left_motor_lag">
|
||||
<input> fcs/left_motor </input>
|
||||
<c1> 10.1 </c1>
|
||||
<output> fcs/left_motor_lag</output>
|
||||
</lag_filter>
|
||||
<lag_filter name="pusher_lag">
|
||||
<input> fcs/pusher </input>
|
||||
<c1> 24.07 </c1>
|
||||
<output> fcs/pusher_lag</output>
|
||||
</lag_filter>
|
||||
|
||||
<!--Derivative of actuator dynamics for spinup torque-->
|
||||
<washout_filter name="front_motor_d">
|
||||
<input> fcs/front_motor </input>
|
||||
<c1> 10.1 </c1>
|
||||
<output> fcs/front_motor_d</output>
|
||||
</washout_filter>
|
||||
<washout_filter name="right_motor_d">
|
||||
<input> fcs/right_motor </input>
|
||||
<c1> 10.1 </c1>
|
||||
<output> fcs/right_motor_d</output>
|
||||
</washout_filter>
|
||||
<washout_filter name="back_motor_d">
|
||||
<input> fcs/back_motor </input>
|
||||
<c1> 10.1 </c1>
|
||||
<output> fcs/back_motor_d</output>
|
||||
</washout_filter>
|
||||
<washout_filter name="left_motor_d">
|
||||
<input> fcs/left_motor </input>
|
||||
<c1> 10.1 </c1>
|
||||
<output> fcs/left_motor_d</output>
|
||||
</washout_filter>
|
||||
</channel>
|
||||
</flight_control>
|
||||
|
||||
<external_reactions>
|
||||
|
||||
<property>fcs/front_motor</property>
|
||||
<property>fcs/front_motor_lag</property>
|
||||
<property>fcs/front_motor_d</property>
|
||||
<property>fcs/right_motor</property>
|
||||
<property>fcs/right_motor_lag</property>
|
||||
<property>fcs/right_motor_d</property>
|
||||
<property>fcs/back_motor</property>
|
||||
<property>fcs/back_motor_lag</property>
|
||||
<property>fcs/back_motor_d</property>
|
||||
<property>fcs/left_motor</property>
|
||||
<property>fcs/left_motor_lag</property>
|
||||
<property>fcs/left_motor_d</property>
|
||||
<!-- <property>fcs/rudder</property>
|
||||
<property>fcs/elevator</property>
|
||||
<property>fcs/aileron</property>-->
|
||||
<property>fcs/pusher</property>
|
||||
<property>fcs/pusher_lag</property>
|
||||
<!-- <property>fcs/flap</property>
|
||||
<property>fcs/skew</property> -->
|
||||
|
||||
<!-- First the lift forces produced by each propeller -->
|
||||
|
||||
<force name="front_motor" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/front_motor_lag</property>
|
||||
<value> 8.69 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>-0.423</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="right_motor" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/right_motor_lag</property>
|
||||
<value> 8.69 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>0.408</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="back_motor" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/back_motor_lag</property>
|
||||
<value> 8.69 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0.423</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="left_motor" frame="BODY" unit="LBS">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/left_motor_lag</property>
|
||||
<value> 8.69 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>-0.408</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="pusher_motor" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/pusher_lag</property>
|
||||
<value> 36.96 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>1</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<!-- Now the moment couples -->
|
||||
<moment name="front_motor_couple" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/front_motor_lag</property>
|
||||
<value> 10.0 </value>
|
||||
<value> 0.192 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>-0.423</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="right_motor_couple" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/right_motor_lag</property>
|
||||
<value> 10.0 </value>
|
||||
<value> 0.192 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>0.408</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="back_motor_couple" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/front_motor_lag</property>
|
||||
<value> 10.0 </value>
|
||||
<value> 0.192 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0.423</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="left_motor_couple" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/right_motor_lag</property>
|
||||
<value> 10.0 </value>
|
||||
<value> 0.192 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>-0.408</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<!-- Now the G2 moment couples -->
|
||||
<moment name="front_motor_couple_G2" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/front_motor_d</property>
|
||||
<value> 10.0 </value>
|
||||
<value> 0.0096 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>-0.423</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="right_motor_couple_G2" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/right_motor_lag</property>
|
||||
<value> 10.0 </value>
|
||||
<value> 0.0096 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>0.408</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="back_motor_couple_G2" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/front_motor_lag</property>
|
||||
<value> 10.0 </value>
|
||||
<value> 0.0096 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0.423</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="left_motor_couple_G2" frame="BODY" unit="N">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/right_motor_lag</property>
|
||||
<value> 10.0 </value>
|
||||
<value> 0.0096 </value>
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>-0.408</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
</external_reactions>
|
||||
|
||||
<propulsion/>
|
||||
|
||||
<flight_control name="FGFCS"/>
|
||||
|
||||
<aerodynamics>
|
||||
<axis name="DRAG">
|
||||
<function name="aero/coefficient/CD">
|
||||
<description>Drag</description>
|
||||
<product>
|
||||
<property>aero/qbar-psf</property>
|
||||
<value>47.9</value> <!-- Conversion to pascals -->
|
||||
<value>0.0151</value> <!-- CD x Area (m^2) -->
|
||||
<value>0.224808943</value> <!-- N to LBS -->
|
||||
</product>
|
||||
</function>
|
||||
</axis>
|
||||
|
||||
<axis name="SIDE">
|
||||
<function name="aero/force/Side_beta">
|
||||
<description>Side force due to beta</description>
|
||||
<product>
|
||||
<property>aero/qbar-psf</property>
|
||||
<property>metrics/Sw-sqft</property>
|
||||
<property>aero/beta-rad</property>
|
||||
<value>-4</value>
|
||||
</product>
|
||||
</function>
|
||||
|
||||
</axis>
|
||||
</aerodynamics>
|
||||
|
||||
</fdm_config>
|
||||
@@ -7,7 +7,7 @@
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
|
||||
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="#ffffc457c457"
|
||||
/>
|
||||
<aircraft
|
||||
@@ -615,4 +615,15 @@
|
||||
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
|
||||
gui_color="red"
|
||||
/>
|
||||
<aircraft
|
||||
name="RotatingWingV3C"
|
||||
ac_id="34"
|
||||
airframe="airframes/tudelft/rot_wing_v3c_oneloop.xml"
|
||||
radio="radios/crossfire_sbus.xml"
|
||||
telemetry="telemetry/highspeed_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/oneloop_valkenburg.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/oneloop_andi.xml"
|
||||
gui_color="red"
|
||||
/>
|
||||
</conf>
|
||||
|
||||
Reference in New Issue
Block a user