Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Convergence rebase (#2946)

Browse Source 
* Added disco_rotorcraft airframefile to be able to fly hybrid code

* Created files (airframe and flightplan) and added bebop_convergence to tudelft conf

* allow higher version bebop

* logging motor commands and accel bebop2

* Updated hybrid guidance indi  variables for bebop2

* improvements

* Prepared bebop2 airframe for cyberzoo test

* Flying bebop in the cyberzoo

* increase outer loop gains

* bebop2 jsbsim file

* no failsafe thrust for rotorcraft

* use ekf2 for bebop2

* Prepared flightplan for outside test

* fix static heading

* Added disco_convergence

* Updated hybrid guidance with extra variables as macro for convergence test

* extended logger with nav target

* typo with the nav_target

* fix thrust estimate

* no ctrl eff scheduling

* Flown code Valkenburg

* Tuned Disco convergence and added nps sim

* 24-06-2021 flown disco code, noise on thrust

* Updated disco flight plan to increase and decrease altitude 20 meters in flight

* change thrust effectiveness

* cleanup

* logger default freq, lexer, etc.

* further small improvments

* lower frequency for AHRS_REF_QUAT

* use more lexer

Co-authored-by: Dennis van Wijngaarden <wijngaarden.dennis@gmail.com>
This commit is contained in:
Ewoud Smeur
2022-11-23 21:21:17 +01:00
committed by GitHub
parent 27c9c8a27f
commit 6a44445dd2
17 changed files with 1696 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop2_indi_convergence.xml
+237
View File
@@ -0,0 +1,237 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop2_indi">
<firmware name="rotorcraft">
<target name="ap" board="bebop2">
<define name="STABILIZATION_INDI_G2_R" value="0.2784"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</module>
</target>
<target name="nps" board="pc">
<module name="fdm" type="jsbsim"/>
<module name="udp"/>
<define name="STABILIZATION_INDI_G2_R" value="0.2784"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/"/>
</module>
</target>
<!-- Subsystem section -->
<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="motor_mixing"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop">
<define name="BEBOP_LOWPASS_FILTER" value="MPU60X0_DLPF_42HZ"/>
<define name="BEBOP_SMPLRT_DIV" value="0"/>
<define name="BEBOP_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_2000"/>
<define name="BEBOP_ACCEL_RANGE" value="MPU60X0_ACCEL_RANGE_16G"/>
</module>
<!--<module name="gps" type="datalink"/>-->
<module name="gps" type="ublox"/>
<module name="gps" type="ubx_ucenter"/>
<module name="stabilization" type="indi_simple"/>
<module name="guidance" type="indi_hybrid"/>
<module name="ins" type="ekf2"/>
<!--<module name="ahrs" type="int_cmpl_quat">-->
<!--<configure name="USE_MAGNETOMETER" value="FALSE"/>-->
<!--<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>-->
<!--<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>-->
<!--</module>-->
<!--<module name="ins" type="extended"/>-->
<module name="air_data"/>
</firmware>
<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>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="2500" neutral="2500" max="12000"/>
<servo name="TOP_RIGHT" no="1" min="2500" neutral="2500" max="12000"/>
<servo name="BOTTOM_RIGHT" no="2" min="2500" neutral="2500" max="12000"/>
<servo name="BOTTOM_LEFT" no="3" min="2500" neutral="2500" max="12000"/>
</servos>
<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="REVERSE" value="TRUE"/>
<define name="TYPE" value="QUAD_X"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="43"/>
<define name="MAG_Y_NEUTRAL" value="-151"/>
<define name="MAG_Z_NEUTRAL" value="-64"/>
<define name="MAG_X_SENS" value="7.21201776785" integer="16"/>
<define name="MAG_Y_SENS" value="7.20541442846" integer="16"/>
<define name="MAG_Z_SENS" value="7.55306977197" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-1"/>
<define name="ACCEL_Y_NEUTRAL" value="-16"/>
<define name="ACCEL_Z_NEUTRAL" value="-102"/>
<define name="ACCEL_X_SENS" value="4.90615997669" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.8601827035" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.85967415403" integer="16"/>
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
</section>
<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="300" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</section>
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.0390"/>
<define name="G1_Q" value="0.0473"/>
<define name="G1_R" value="0.00122"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="170.0"/>
<define name="REF_ERR_Q" value="600.0"/>
<define name="REF_ERR_R" value="600.0"/>
<define name="REF_RATE_P" value="14.3"/>
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_P" value="10.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="350"/>
<define name="HOVER_KD" value="85"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="35" unit="deg"/>
<define name="REF_MAX_SPEED" value="2" unit="m/s"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="120"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</section>
<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<define name="POS_GAIN" value="1.1"/>
<define name="POS_GAINZ" value="1.1"/>
<define name="SPEED_GAIN" value="3.0"/>
<define name="SPEED_GAINZ" value="3.0"/>
<define name="FILTER_CUTOFF" value="1.0"/>
<define name="HEADING_BANK_GAIN" value="1."/>
<define name="NAV_SPEED_MARGIN" value="0."/>
<define name="MAX_AIRSPEED" value="4."/>
<define name="ZERO_AIRSPEED" value="TRUE"/>
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="SPECIFIC_FORCE_GAIN" value="-400."/>
<define name="THRUST_DYNAMICS" value="0.06"/>
<define name="MIN_THROTTLE" value="0."/>
<define name="MIN_PITCH" value="-35."/>
<define name="MAX_PITCH" value="35."/>
</section>
<section name="FORWARD">
<!-- This is the pitch angle that the drone will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="0.0" unit="deg"/>
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
</section>
<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="2.0"/>
<define name="DESCEND_VSPEED" value="-1.0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="bebop2" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
</section>
<include href="conf/simulator/gazebo/airframes/bebop.xml"/>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="11.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/airframes/tudelft/disco_rotorcraft_indi.xml
+277
View File
@@ -0,0 +1,277 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
Parrot Disco
-->
<airframe name="Disco">
<firmware name="rotorcraft">
<configure name="PERIODIC_FREQUENCY" value="512"/>
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<target name="ap" board="disco">
<module name="radio_control" type="sbus"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</module>
<module name="airspeed" type="ms45xx_i2c">
<define name="MS45XX_I2C_DEV" value="i2c1"/>
<define name="MS45XX_PRESSURE_RANVE" value="0.05"/>
<define name="USE_AIRSPEED_MS45XX" value="TRUE"/>
</module>
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_FLAPS" value="RADIO_AUX4"/> <!-- Flaps control -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_BACK_THOLD" value="RADIO_AUX5"/>
</target>
<target name="nps" board="pc">
<define name="USE_AIRSPEED" value="TRUE"/>
<module name="fdm" type="jsbsim"/>
<module name="udp"/>
<module name="radio_control" type="datalink"/>
<!--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_FLAPS" value="0"/> <!-- Flaps control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
</target>
<module name="telemetry" type="transparent_udp"/>
<module name="imu" type="disco"/>
<!--<module name="ahrs" type="float_dcm"/>-->
<module name="gps" type="ublox"/>
<module name="gps" type="ubx_ucenter"/>
<module name="ins" type="ekf2"/>
<module name="air_data"/>
<module name="actuators" type="disco"/>
<!--<module name="control" type="new"/>-->
<module name="stabilization" type="indi">
<configure name="INDI_NUM_ACT" value="3"/>
</module>
<module name="guidance" type="indi_hybrid"/>
<!--<module name="sonar_bebop">
<define name="USE_SONAR"/>
<define name="SENSOR_SYNC_SEND_SONAR"/>
</module>-->
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
<!--Switch advanced INDI scheduling functions on or off-->
<define name="INDI_FUNCTIONS_RC_CHANNEL" value="6"/>
</firmware>
<servos>
<servo name="MOTOR" no="0" min="1000" neutral="1000" max="12500"/> <!-- 12500 max -->
<servo name="AILEVON_RIGHT" no="6" min="1850" neutral="1450" max="1050"/>
<servo name="AILEVON_LEFT" no="1" min="1050" neutral="1450" max="1850"/>
</servos>
<commands>
<axis name="THRUST" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
</rc_commands>
<section name="MIXER">
<define name="AILEVON_AILERON_RATE" value="0.75"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
</section>
<command_laws>
<let var="th_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let var="manual" value="LessThan(RadioControlValues(RADIO_MODE), -4800)"/>
<let var="autopil" value="(1-($manual))"/>
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
<let var="motor_thrust" value="$manual? @THRUST : actuators_pprz[2]"/>
<set servo="MOTOR" value="$th_hold? -9600 : $motor_thrust"/>
<set servo="AILEVON_LEFT" value="($elevator - $aileron) * $manual - actuators_pprz[0] * $autopil"/>
<set servo="AILEVON_RIGHT" value="($elevator + $aileron) * $manual + actuators_pprz[1] * $autopil"/>
</command_laws>
<section name="MISC">
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED" value="TRUE"/>
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 12.0877510901 * adc)"/>
<!--<define name="USE_AIRSPEED" value="TRUE"/>-->
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
</section>
<section name="FORWARD">
<!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-90.0" unit="deg"/>
<!-- For hybrid guidance by default set to 15-->
<!--<define name="MAX_AIRSPEED" value="20.0"/>-->
</section>
<!--<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.85"/>
<define name="MAX_PITCH" value="0.6"/>
</section>-->
<section name="IMU" prefix="IMU_">
<define name="ACCEL_X_NEUTRAL" value="-164"/>
<define name="ACCEL_Y_NEUTRAL" value="-19"/>
<define name="ACCEL_Z_NEUTRAL" value="-302"/>
<define name="ACCEL_X_SENS" value="2.4353263325616896" integer="16"/>
<define name="ACCEL_Y_SENS" value="2.430457186142149" integer="16"/>
<define name="ACCEL_Z_SENS" value="2.4083032197503895" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-195"/>
<define name="MAG_Y_NEUTRAL" value="-266"/>
<define name="MAG_Z_NEUTRAL" value="-806"/>
<define name="MAG_X_SENS" value="7.239846705729754" integer="16"/>
<define name="MAG_Y_SENS" value="7.037092768037254" integer="16"/>
<define name="MAG_Z_SENS" value="7.583294047780469" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0" unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1_ROLL" value="{ 0, 0, 0}"/>
<define name="G1_PITCH" value="{-8.79, 8.79, 0}"/>
<define name="G1_YAW" value="{-15.97, -15.97, 0}"/>
<define name="G1_THRUST" value="{ 0, 0, -1.8}"/>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{0, 0, 0}"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="100.0"/>
<define name="REF_ERR_Q" value="100.0"/>
<define name="REF_ERR_R" value="100.0"/>
<define name="REF_RATE_P" value="14.0"/>
<define name="REF_RATE_Q" value="15.0"/>
<define name="REF_RATE_R" value="15.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="100.0" unit="deg/s"/>
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
<define name="FILT_CUTOFF" value="5.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN" value="{0.1, 0.1, 0.045}"/>
<define name="ACT_RATE_LIMIT" value="{170, 170, 9600}"/>
<define name="ACT_IS_SERVO" value="{1, 1, 0}"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="40." unit="deg"/>
<define name="SP_MAX_THETA" value="25." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<define name="SP_PSI_DELTA_LIMIT" value="0.001" unit="deg"/>
</section>
<section name="ctrl_eff_scheduling" prefix="FWD_">
<!-- control effectiveness, scaled by INDI_G_SCALING (1000)-->
<define name="G1_ROLL" value="{ 0, 0, 0}"/>
<define name="G1_PITCH" value="{-8.79, 8.79, 0}"/>
<define name="G1_YAW" value="{-15.97, -15.97, 0}"/>
<define name="G1_THRUST" value="{ 0, 0, -0.7}"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="310"/>
<define name="HOVER_KD" value="130"/>
<define name="HOVER_KI" value="10"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.4"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="45" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="0"/>
<define name="IGAIN" value="20"/>
</section>
<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<define name="POS_GAIN" value="0.2"/>
<define name="POS_GAINZ" value="0.2"/>
<define name="SPEED_GAIN" value="1.0"/>
<define name="SPEED_GAINZ" value="1.0"/>
<define name="MAX_AIRSPEED" value="12."/>
<define name="ZERO_AIRSPEED" value="FALSE"/>
<define name="NAV_SPEED_MARGIN" value="10.0"/>
<define name="PITCH_EFF_SCALING" value="1.0"/>
<define name="PITCH_LIFT_EFF" value="0.12"/>
<define name="HEADING_BANK_GAIN" value="15."/>
<!--define name="SPECIFIC_FORCE_GAIN" value="-1300."/-->
<!--define name="THRUST_DYNAMICS" value="0.04"/-->
<define name="MIN_THROTTLE" value="0.0"/>
<define name="MIN_THROTTLE_FWD" value="0.0"/>
<define name="MIN_PITCH" value="-115."/>
<define name="MAX_PITCH" value="-75."/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.6" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.6" unit="V"/>
<define name="MilliAmpereOfAdc(_adc)" value="(_adc-632)*4.14"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="ele_left, ele_right, mot" type="string[]"/>
<define name="JSBSIM_MODEL" value="disco_rotorcraft" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="COMMANDS_NB" value="3"/>
</section>
</airframe>
conf/flight_plans/tudelft/delft_bebop_convergence.xml
+134
View File
@@ -0,0 +1,134 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="5" ground_alt="0" lat0="51 59 27.6" lon0="4 22 42.0" max_dist_from_home="200" name="Delft Basic" security_height="2">
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="CLIMB" x="0.0" y="5.0"/>
<waypoint name="STDBY" x="-2.0" y="-5.0"/>
<waypoint name="p1" x="-19.9" y="14.3"/>
<waypoint name="p2" x="-0.6" y="21.6"/>
<waypoint name="p3" x="22.2" y="-26.5"/>
<waypoint name="p4" x="4.9" y="-34.4"/>
<waypoint name="CAM" x="14.2" y="-29.4"/>
<waypoint name="TD" x="5.6" y="-10.9"/>
<waypoint name="_HG1" lat="51.99145087814413" lon="4.378416538238525"/>
<waypoint name="_HG2" lat="51.99130883618111" lon="4.377756714820861"/>
<waypoint name="_HG3" lat="51.99051933891883" lon="4.378344118595123"/>
<waypoint name="_HG4" lat="51.99071258535118" lon="4.378966391086578"/>
<waypoint name="SG1" lat="51.991269196483096" lon="4.377896189689636"/>
<waypoint name="SG2" lat="51.990608529683556" lon="4.378384351730347"/>
<waypoint name="SG3" lat="51.990745618845914" lon="4.378829598426819"/>
<waypoint name="SG4" lat="51.99136994731342" lon="4.378365576267242"/>
</waypoints>
<sectors>
<sector color="red" name="Flight_Area">
<corner name="_HG1"/>
<corner name="_HG2"/>
<corner name="_HG3"/>
<corner name="_HG4"/>
</sector>
<sector color="yellow" name="Soft_Geo">
<corner name="SG1"/>
<corner name="SG2"/>
<corner name="SG3"/>
<corner name="SG4"/>
</sector>
</sectors>
<exceptions>
<exception cond="Or(!InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 20) @AND
!(IndexOfBlock('Takeoff') @GEQ nav_block) @AND
!(nav_block @GEQ IndexOfBlock('landed'))" deroute="descend att 0"/>
<exception cond="!InsideSoft_Geo(GetPosX(), GetPosY()) @AND
!(IndexOfBlock('Takeoff') @GEQ nav_block) @AND
!(nav_block @GEQ IndexOfBlock('landed'))" deroute="ComeBackAndLand"/>
<exception cond="GetPosAlt() @GT GetAltRef() + 15 @AND
!(IndexOfBlock('Takeoff') @GEQ nav_block) @AND
!(nav_block @GEQ IndexOfBlock('land here'))" deroute="land here"/>
<exception cond="((radio_control.status == RC_REALLY_LOST) @AND
!(IndexOfBlock('Takeoff') @GEQ nav_block) @AND
!(nav_block @GEQ IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="ComeBackAndLand"/>
<exception cond="(datalink_time @GT 20 @AND
!(IndexOfBlock('Takeoff') @GEQ nav_block) @AND
!(nav_block @GEQ IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="descend att 0"/>
<exception cond="(datalink_time @GT 10 @AND
!(IndexOfBlock('Takeoff') @GEQ nav_block) @AND
!(nav_block @GEQ IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="ComeBackAndLand"/>
<exception cond="(electrical.bat_low @AND
!(IndexOfBlock('Holding point') @GEQ nav_block) @AND
!(nav_block @GEQ IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="ComeBackAndLand"/>
<exception cond="(electrical.bat_critical @AND
!(IndexOfBlock('Holding point') @GEQ nav_block) @AND
!(nav_block @GEQ IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z @GT 2.0" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<stay wp="p1"/>
</block>
<block name="go_p2">
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="line_p1_p2">
<go from="p1" hmode="route" wp="p2"/>
<stay until="stage_time>10" wp="p2"/>
<go from="p2" hmode="route" wp="p1"/>
<deroute block="stay_p1"/>
</block>
<block name="route">
<path wpts="p1,p2,p3,p4"/>
<deroute block="stay_p1"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<go wp="TD"/>
</block>
<block name="flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="ComeBackAndLand">
<go wp="HOME"/>
<deroute block="land here"/>
</block>
<block name="descend att 0">
<attitude pitch="0" roll="0" throttle="0.40" until="stage_time @GT 30" vmode="throttle"/>
<deroute block="Holding point"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/tudelft/disco_convergence.xml
+94
View File
@@ -0,0 +1,94 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="70" ground_alt="0" lat0="52.1682089" lon0="4.4128499" max_dist_from_home="500" name="Altura fp Valkenburg" security_height="30">
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="CLIMB" x="26.6" y="23.2"/>
<waypoint name="STDBY" x="49.5" y="15.8"/>
<waypoint name="p1" x="34.3" y="80.7"/>
<waypoint name="p2" x="141.1" y="129.5"/>
<waypoint name="p3" x="237.6" y="-38.1"/>
<waypoint name="p4" x="135.8" y="-91.8"/>
<waypoint name="p5" x="34.3" y="80.7" alt="90"/>
<waypoint name="p6" x="141.1" y="129.5" alt="90"/>
<waypoint name="p7" x="237.6" y="-38.1" alt="90"/>
<waypoint name="p8" x="135.8" y="-91.8" alt="90"/>
<waypoint name="TD" x="36.3" y="-4.0"/>
<waypoint name="GF1" x="-224.1" y="136.6"/>
<waypoint name="GF2" x="-89.4" y="-289.8"/>
<waypoint name="GF3" x="442.6" y="-139.6"/>
<waypoint name="GF4" x="219.7" y="301.2"/>
</waypoints>
<sectors>
<sector color="red" name="Geofence">
<corner name="GF1"/>
<corner name="GF2"/>
<corner name="GF3"/>
<corner name="GF4"/>
</sector>
</sectors>
<exceptions>
<exception cond="Or(!InsideGeofence(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND
!(nav_block @LEQ IndexOfBlock('Holding point'))" deroute="ReturnStbyNoThrottle"/>
<exception cond="datalink_time @GT 25 @AND
!(nav_block @LEQ IndexOfBlock('Holding point')) @AND
!(nav_block == IndexOfBlock('Standby'))" deroute="route"/>
<exception cond="radio_control.status == RC_REALLY_LOST @AND
!(nav_block @LEQ IndexOfBlock('Holding point')) @AND
!(nav_block == IndexOfBlock('Standby'))" deroute="route"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="true" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<set value="true" var="force_forward"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<set value="true" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="true" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<!--<block name="Takeoff1" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="true" var="force_forward"/>
<exception cond="stateGetPositionEnu_f()->z > 50.0" deroute="route"/>
<call_once fun="nav_set_heading_current()"/>
<attitude pitch="-10." roll="0" throttle="1.0" vmode="throttle"/>
</block>-->
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="true" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="route">
<set value="true" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<while cond="true">
<path wpts="p1,p2,p3,p4"/>
</while>
</block>
<block name="route_run">
<set value="true" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<while cond="true">
<path wpts="p1,p2,p3,p4,p5,p6,p7,p8,p5,p6,p7,p8"/>
</while>
</block>
<block name="ReturnStbyNoThrottle">
<exception cond="And(!(radio_control.status == RC_REALLY_LOST), And(InsideGeofence(GetPosX(), GetPosY()), And(MoreThan(GetPosAlt(), GetAltRef() + 20), LessThan(GetPosAlt(), GetAltRef() + 70))))" deroute="Standby"/>
<set value="1" var="autopilot.kill_throttle"/>
<set value="true" var="force_forward"/>
<call_once fun="NavKillThrottle()"/>
<circle radius="nav_radius" throttle="0.0" wp="STDBY"/>
</block>
</blocks>
</flight_plan>
conf/modules/actuators_disco.xml
+1
View File
@@ -10,6 +10,7 @@
<depends>i2c,actuators</depends> <depends>i2c,actuators</depends>
<provides>actuators</provides> <provides>actuators</provides>
</dep> </dep>
<autoload name="actuators" type="nps"/>
<header> <header>
<file name="actuators.h" dir="boards/disco"/> <file name="actuators.h" dir="boards/disco"/>
</header> </header>
conf/modules/logger_file.xml
+6 -3
View File
@@ -7,13 +7,16 @@
(only for linux) (only for linux)
</description> </description>
<define name="FILE_LOGGER_PATH" value="/data/video/usb" description="path where csv file is saved."/> <define name="FILE_LOGGER_PATH" value="/data/video/usb" description="path where csv file is saved."/>
<configure name="FILE_LOGGER_FREQUENCY" value="PERIODIC_FREQUENCY" description="frequency of logging, defaults to PERIODIC_FREQUENCY."/>
</doc> </doc>
<header> <header>
<file name="file_logger.h" /> <file name="file_logger.h"/>
</header> </header>
<periodic fun="file_logger_periodic()" start="file_logger_start()" <periodic fun="file_logger_periodic()" start="file_logger_start()"
stop="file_logger_stop()" autorun="FALSE" period="0.01" /> stop="file_logger_stop()" autorun="FALSE" freq="FILE_LOGGER_FREQUENCY" />
<makefile> <makefile>
<file name="file_logger.c"/> <file name="file_logger.c"/>
<configure name="FILE_LOGGER_FREQUENCY" default="PERIODIC_FREQUENCY"/>
<define name="FILE_LOGGER_FREQUENCY" value="$(FILE_LOGGER_FREQUENCY)"/>
</makefile> </makefile>
</module> </module>
conf/simulator/jsbsim/aircraft/Systems/aerodynamics_disco.xml
+159
View File
@@ -0,0 +1,159 @@
<aerodynamics>
<axis name="YAW">
<function name="elevon_yaw" frame="BODY" unit="FT*LBS">
<sum>
<product>
<property>fcs/ele_left_lag</property>
<value> -0.65 </value>
</product>
<product>
<property>fcs/ele_right_lag</property>
<value> -.65 </value>
</product>
</sum>
</function>
</axis>
<axis name="PITCH">
<function name="elevon_pitch" frame="BODY" unit="FT*LBS">
<sum>
<product>
<property>fcs/ele_left_lag</property>
<value> -0.0373430666 </value>
</product>
<product>
<property>fcs/ele_right_lag</property>
<value> 0.0373430666 </value>
</product>
</sum>
</function>
</axis>
<axis name="LIFT">
<function name="aero/force/Lift_alpha">
<description>Lift due to alpha</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<table>
<independentVar lookup="row">aero/alpha-rad</independentVar>
<tableData>
<!---1.87 -0.35
-1.57 0.0
-1.27 0.65
-0.80 0.65
0.00 0.0
0.80 0.2
1.27 0.2
1.57 0.0
1.87 -0.2-->
-1.77 -0.750
-1.57 0.250
-1.34 1.400
-0.97 0.710
0.00 0.0
0.80 0.2
1.27 0.2
1.57 0.0
1.87 -0.2
</tableData>
</table>
</product>
</function>
</axis>
<axis name="DRAG">
<function name="aero/force/Drag_basic">
<description>Drag at zero lift</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<table>
<independentVar lookup="row">aero/alpha-rad</independentVar>
<tableData>
-1.57 0.028
-1.31 0.036
0.00 1.500
1.31 0.036
1.57 0.028
<!---1.57 0.28
-1.31 0.36
0.00 1.500
1.31 0.36
1.57 0.28-->
</tableData>
</table>
</product>
</function>
<function name="aero/force/Drag_induced">
<description>Induced drag</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>aero/cl-squared</property>
<value>0.04</value>
</product>
</function>
<!--<function name="aero/force/Drag_beta">-->
<!--<description>Drag due to sideslip</description>-->
<!--<product>-->
<!--<property>aero/qbar-psf</property>-->
<!--<property>metrics/Sw-sqft</property>-->
<!--<table>-->
<!--<independentVar lookup="row">aero/beta-rad</independentVar>-->
<!--<tableData>-->
<!---1.57 1.230-->
<!---0.26 0.050-->
<!--0.00 0.000-->
<!--0.26 0.050-->
<!--1.57 1.230-->
<!--</tableData>-->
<!--</table>-->
<!--</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>-1</value>
</product>
</function>
</axis>
<axis name="ROLL">
<function name="aero/moment/Roll_beta">
<description>Yaw moment due to beta</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>metrics/bw-ft</property>
<property>aero/beta-rad</property>
<value>
0.24
<!--ifthen>
<gt><p>aero/qbar-psf</p> <v>0</v></gt>
<v>0.12</v>
<v>0.00</v>
</ifthen>-->
</value>
</product>
</function>
</axis>
</aerodynamics>
conf/simulator/jsbsim/aircraft/bebop2.xml
+504
View File
File diff suppressed because it is too large Load Diff
conf/simulator/jsbsim/aircraft/disco_rotorcraft.xml
+185
View File
@@ -0,0 +1,185 @@
<?xml version="1.0"?>
<?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?>
<fdm_config name="Disco Rotorcraft" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd">
<fileheader>
<author>Dennis van Wijngaarden</author>
<filecreationdate>21-06-2021</filecreationdate>
<version>Version 0.9 - beta</version>
<description>Disco rotorcraft</description>
</fileheader>
<metrics>
<wingarea unit="IN2"> 425 </wingarea>
<wingspan unit="IN"> 45 </wingspan>
<chord unit="IN"> 9.45 </chord>
<htailarea unit="FT2"> 0 </htailarea>
<htailarm unit="FT"> 0 </htailarm>
<vtailarea unit="FT2"> 0 </vtailarea>
<vtailarm unit="FT"> 0 </vtailarm>
<wing_incidence unit="DEG"> 90 </wing_incidence>
<location name="AERORP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="EYEPOINT" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="VRP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</metrics>
<mass_balance>
<ixx unit="SLUG*FT2"> 0.01143077 </ixx>
<iyy unit="SLUG*FT2"> 0.00088507457028 </iyy>
<izz unit="SLUG*FT2"> 0.01143077 </izz>
<ixy unit="SLUG*FT2"> 0. </ixy>
<ixz unit="SLUG*FT2"> 0. </ixz>
<iyz unit="SLUG*FT2"> 0. </iyz>
<emptywt unit="LBS"> 1.653 </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="ele_left_lag">
<input> fcs/ele_left </input>
<c1> 54 </c1>
<output> fcs/ele_left_lag</output>
</lag_filter>
<lag_filter name="ele_right_lag">
<input> fcs/ele_right </input>
<c1> 54 </c1>
<output> fcs/ele_right_lag</output>
</lag_filter>
<lag_filter name="mot_lag">
<input> fcs/mot </input>
<c1> 31.7 </c1>
<output> fcs/mot_lag</output>
</lag_filter>
</channel>
</flight_control>
<external_reactions>
<property>fcs/ele_left</property>
<property>fcs/ele_left_lag</property>
<property>fcs/ele_right</property>
<property>fcs/ele_right_lag</property>
<property>fcs/mot</property>
<property>fcs/mot_lag</property>
<property>fcs/none1</property>
<property>fcs/none2</property>
<property>fcs/none3</property>
<property>fcs/none4</property>
<!-- First the lift forces produced by each propeller -->
<force name="mot" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/mot_lag</property>
<value> 1.7 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<!--Yaw moments-->
</external_reactions>
<propulsion/>
<flight_control name="FGFCS"/>
<aerodynamics file="Systems/aerodynamics_disco.xml"/>
</fdm_config>
conf/telemetry/default_rotorcraft.xml
+2
View File
@@ -36,6 +36,7 @@
<message name="LIDAR" period="1.2"/> <message name="LIDAR" period="1.2"/>
<message name="INS_EKF2" period=".25"/> <message name="INS_EKF2" period=".25"/>
<message name="WIND_INFO_RET" period="1."/> <message name="WIND_INFO_RET" period="1."/>
<message name="AHRS_REF_QUAT" period="0.5"/>
</mode> </mode>
<mode name="ppm"> <mode name="ppm">
@@ -126,6 +127,7 @@
<mode name="vel_guidance" key_press="q"> <mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/> <message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>--> <!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/> <message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
conf/userconf/tudelft/conf.xml
+23 -1
View File
@@ -436,9 +436,20 @@
telemetry="telemetry/default_rotorcraft.xml" telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_bebop.xml" flight_plan="flight_plans/tudelft/delft_bebop.xml"
settings="settings/rotorcraft_basic.xml [settings/estimation/ahrs_secondary.xml]" settings="settings/rotorcraft_basic.xml [settings/estimation/ahrs_secondary.xml]"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml" settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="#baa3d698b729" gui_color="#baa3d698b729"
/> />
<aircraft
name="bebop2_convergence"
ac_id="5"
airframe="airframes/tudelft/bebop2_indi_convergence.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_bebop_convergence.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
/>
<aircraft <aircraft
name="bebop2_no_dampers" name="bebop2_no_dampers"
ac_id="26" ac_id="26"
@@ -505,6 +516,17 @@
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_capture.xml modules/video_rtp_stream.xml" settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="blue" gui_color="blue"
/> />
<aircraft
name="disco_convergence"
ac_id="2"
airframe="airframes/tudelft/disco_rotorcraft_indi.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/disco_convergence.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="blue"
/>
<aircraft <aircraft
name="fan_demo" name="fan_demo"
ac_id="28" ac_id="28"
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+3
View File
@@ -282,6 +282,9 @@ void guidance_h_mode_changed(uint8_t new_mode)
#endif #endif
case GUIDANCE_H_MODE_NAV: case GUIDANCE_H_MODE_NAV:
#if GUIDANCE_INDI
guidance_indi_enter();
#endif
guidance_h_nav_enter(); guidance_h_nav_enter();
#if NO_ATTITUDE_RESET_ON_MODE_CHANGE #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
/* reset attitude stabilization if previous mode was not using it */ /* reset attitude stabilization if previous mode was not using it */
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
+3
View File
@@ -44,4 +44,7 @@ extern float guidance_indi_pos_gain;
extern float guidance_indi_speed_gain; extern float guidance_indi_speed_gain;
extern float guidance_indi_max_bank; extern float guidance_indi_max_bank;
extern struct FloatVect3 sp_accel;
#endif /* GUIDANCE_INDI_H */ #endif /* GUIDANCE_INDI_H */
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+64 -25
View File
@@ -61,6 +61,11 @@
#define GUIDANCE_INDI_POS_GAINZ 0.5 #define GUIDANCE_INDI_POS_GAINZ 0.5
#endif #endif
#ifndef GUIDANCE_INDI_MIN_PITCH
#define GUIDANCE_INDI_MIN_PITCH -120
#define GUIDANCE_INDI_MAX_PITCH 25
#endif
struct guidance_indi_hybrid_params gih_params = { struct guidance_indi_hybrid_params gih_params = {
.pos_gain = GUIDANCE_INDI_POS_GAIN, .pos_gain = GUIDANCE_INDI_POS_GAIN,
.pos_gainz = GUIDANCE_INDI_POS_GAINZ, .pos_gainz = GUIDANCE_INDI_POS_GAINZ,
@@ -76,14 +81,28 @@ struct guidance_indi_hybrid_params gih_params = {
#endif #endif
float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED; float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
// Tell the guidance that the airspeed needs to be zeroed.
// Recomended to also put GUIDANCE_INDI_NAV_SPEED_MARGIN low in this case.
#ifndef GUIDANCE_INDI_ZERO_AIRSPEED
#define GUIDANCE_INDI_ZERO_AIRSPEED FALSE
#endif
// Max ground speed that will be commanded // Max ground speed that will be commanded
#define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + 10.0) #ifndef GUIDANCE_INDI_NAV_SPEED_MARGIN
#define GUIDANCE_INDI_NAV_SPEED_MARGIN 10.0
#endif
#define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)
float nav_max_speed = NAV_MAX_SPEED; float nav_max_speed = NAV_MAX_SPEED;
#ifndef MAX_DECELERATION #ifndef MAX_DECELERATION
#define MAX_DECELERATION 1. #define MAX_DECELERATION 1.
#endif #endif
/*Airspeed threshold where making a turn is "worth it"*/
#ifndef TURN_AIRSPEED_TH
#define TURN_AIRSPEED_TH 10.0
#endif
/*Boolean to force the heading to a static value (only use for specific experiments)*/ /*Boolean to force the heading to a static value (only use for specific experiments)*/
bool take_heading_control = false; bool take_heading_control = false;
@@ -144,7 +163,7 @@ float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
struct FloatEulers guidance_euler_cmd; struct FloatEulers guidance_euler_cmd;
float thrust_in; float thrust_in;
struct FloatVect3 speed_sp = {0.0, 0.0, 0.0}; struct FloatVect3 gi_speed_sp = {0.0, 0.0, 0.0};
void guidance_indi_propagate_filters(void); void guidance_indi_propagate_filters(void);
static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat); static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
@@ -167,9 +186,9 @@ static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_de
&filt_accel_ned[0].o[0], &filt_accel_ned[0].o[0],
&filt_accel_ned[1].o[0], &filt_accel_ned[1].o[0],
&filt_accel_ned[2].o[0], &filt_accel_ned[2].o[0],
&speed_sp.x, &gi_speed_sp.x,
&speed_sp.y, &gi_speed_sp.y,
&speed_sp.z); &gi_speed_sp.z);
} }
#endif #endif
@@ -241,31 +260,37 @@ void guidance_indi_run(float *heading_sp) {
float pos_z_err = POS_FLOAT_OF_BFP(guidance_v_z_ref - stateGetPositionNed_i()->z); float pos_z_err = POS_FLOAT_OF_BFP(guidance_v_z_ref - stateGetPositionNed_i()->z);
if(autopilot.mode == AP_MODE_NAV) { if(autopilot.mode == AP_MODE_NAV) {
speed_sp = nav_get_speed_setpoint(gih_params.pos_gain); gi_speed_sp = nav_get_speed_setpoint(gih_params.pos_gain);
} else{ } else{
speed_sp.x = pos_x_err * gih_params.pos_gain; gi_speed_sp.x = pos_x_err * gih_params.pos_gain;
speed_sp.y = pos_y_err * gih_params.pos_gain; gi_speed_sp.y = pos_y_err * gih_params.pos_gain;
speed_sp.z = pos_z_err * gih_params.pos_gainz; gi_speed_sp.z = pos_z_err * gih_params.pos_gainz;
} }
//for rc control horizontal, rotate from body axes to NED //for rc control horizontal, rotate from body axes to NED
float psi = eulers_zxy.psi; float psi = eulers_zxy.psi;
/*NAV mode*/ /*NAV mode*/
float speed_sp_b_x = cosf(psi) * speed_sp.x + sinf(psi) * speed_sp.y; float speed_sp_b_x = cosf(psi) * gi_speed_sp.x + sinf(psi) * gi_speed_sp.y;
float speed_sp_b_y =-sinf(psi) * speed_sp.x + cosf(psi) * speed_sp.y; float speed_sp_b_y =-sinf(psi) * gi_speed_sp.x + cosf(psi) * gi_speed_sp.y;
float airspeed = stateGetAirspeed_f(); // Get airspeed or zero it
float airspeed;
if (GUIDANCE_INDI_ZERO_AIRSPEED) {
airspeed = 0.0;
} else {
airspeed = stateGetAirspeed_f();
}
struct NedCoor_f *groundspeed = stateGetSpeedNed_f(); struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
struct FloatVect2 airspeed_v = {cos(psi)*airspeed, sin(psi)*airspeed}; struct FloatVect2 airspeed_v = {cos(psi)*airspeed, sin(psi)*airspeed};
struct FloatVect2 windspeed; struct FloatVect2 windspeed;
VECT2_DIFF(windspeed, *groundspeed, airspeed_v); VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
VECT2_DIFF(desired_airspeed, speed_sp, windspeed); // Use 2d part of speed_sp VECT2_DIFF(desired_airspeed, gi_speed_sp, windspeed); // Use 2d part of gi_speed_sp
float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed); float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
// Make turn instead of straight line // Make turn instead of straight line
if((airspeed > 10.0) && (norm_des_as > 12.0)) { if((airspeed > TURN_AIRSPEED_TH) && (norm_des_as > (TURN_AIRSPEED_TH+2.0) )) {
// Give the wind cancellation priority. // Give the wind cancellation priority.
if (norm_des_as > guidance_indi_max_airspeed) { if (norm_des_as > guidance_indi_max_airspeed) {
@@ -273,8 +298,8 @@ void guidance_indi_run(float *heading_sp) {
// if the wind is faster than we can fly, just fly in the wind direction // if the wind is faster than we can fly, just fly in the wind direction
if(FLOAT_VECT2_NORM(windspeed) < guidance_indi_max_airspeed) { if(FLOAT_VECT2_NORM(windspeed) < guidance_indi_max_airspeed) {
float av = speed_sp.x * speed_sp.x + speed_sp.y * speed_sp.y; float av = gi_speed_sp.x * gi_speed_sp.x + gi_speed_sp.y * gi_speed_sp.y;
float bv = -2 * (windspeed.x * speed_sp.x + windspeed.y * speed_sp.y); float bv = -2 * (windspeed.x * gi_speed_sp.x + windspeed.y * gi_speed_sp.y);
float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed; float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed;
float dv = bv * bv - 4.0 * av * cv; float dv = bv * bv - 4.0 * av * cv;
@@ -288,8 +313,8 @@ void guidance_indi_run(float *heading_sp) {
groundspeed_factor = (-bv + d_sqrt) / (2.0 * av); groundspeed_factor = (-bv + d_sqrt) / (2.0 * av);
} }
desired_airspeed.x = groundspeed_factor * speed_sp.x - windspeed.x; desired_airspeed.x = groundspeed_factor * gi_speed_sp.x - windspeed.x;
desired_airspeed.y = groundspeed_factor * speed_sp.y - windspeed.y; desired_airspeed.y = groundspeed_factor * gi_speed_sp.y - windspeed.y;
speed_sp_b_x = guidance_indi_max_airspeed; speed_sp_b_x = guidance_indi_max_airspeed;
} }
@@ -314,7 +339,7 @@ void guidance_indi_run(float *heading_sp) {
sp_accel.x = cosf(psi) * sp_accel_b.x - sinf(psi) * sp_accel_b.y; sp_accel.x = cosf(psi) * sp_accel_b.x - sinf(psi) * sp_accel_b.y;
sp_accel.y = sinf(psi) * sp_accel_b.x + cosf(psi) * sp_accel_b.y; sp_accel.y = sinf(psi) * sp_accel_b.x + cosf(psi) * sp_accel_b.y;
sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz; sp_accel.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
} else { // Go somewhere in the shortest way } else { // Go somewhere in the shortest way
if(airspeed > 10.0) { if(airspeed > 10.0) {
@@ -327,12 +352,12 @@ void guidance_indi_run(float *heading_sp) {
speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed; speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
} }
} }
speed_sp.x = cosf(psi) * speed_sp_b_x - sinf(psi) * speed_sp_b_y; gi_speed_sp.x = cosf(psi) * speed_sp_b_x - sinf(psi) * speed_sp_b_y;
speed_sp.y = sinf(psi) * speed_sp_b_x + cosf(psi) * speed_sp_b_y; gi_speed_sp.y = sinf(psi) * speed_sp_b_x + cosf(psi) * speed_sp_b_y;
sp_accel.x = (speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain; sp_accel.x = (gi_speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain;
sp_accel.y = (speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain; sp_accel.y = (gi_speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain;
sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz; sp_accel.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
} }
// Bound the acceleration setpoint // Bound the acceleration setpoint
@@ -401,7 +426,7 @@ void guidance_indi_run(float *heading_sp) {
//Bound euler angles to prevent flipping //Bound euler angles to prevent flipping
Bound(guidance_euler_cmd.phi, -guidance_indi_max_bank, guidance_indi_max_bank); Bound(guidance_euler_cmd.phi, -guidance_indi_max_bank, guidance_indi_max_bank);
Bound(guidance_euler_cmd.theta, -RadOfDeg(120.0), RadOfDeg(25.0)); Bound(guidance_euler_cmd.theta, RadOfDeg(GUIDANCE_INDI_MIN_PITCH), RadOfDeg(GUIDANCE_INDI_MAX_PITCH));
// Use the current roll angle to determine the corresponding heading rate of change. // Use the current roll angle to determine the corresponding heading rate of change.
float coordinated_turn_roll = eulers_zxy.phi; float coordinated_turn_roll = eulers_zxy.phi;
@@ -429,6 +454,20 @@ void guidance_indi_run(float *heading_sp) {
} else { } else {
*heading_sp += omega / PERIODIC_FREQUENCY; *heading_sp += omega / PERIODIC_FREQUENCY;
FLOAT_ANGLE_NORMALIZE(*heading_sp); FLOAT_ANGLE_NORMALIZE(*heading_sp);
// limit heading setpoint to be within bounds of current heading
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
float delta_limit = STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
float heading = stabilization_attitude_get_heading_f();
float delta_psi = *heading_sp - heading;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit) {
*heading_sp = heading + delta_limit;
} else if (delta_psi < -delta_limit) {
*heading_sp = heading - delta_limit;
}
FLOAT_ANGLE_NORMALIZE(*heading_sp);
#endif
} }
#endif #endif
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+3 -1
View File
@@ -39,7 +39,6 @@ extern void guidance_indi_enter(void);
extern void guidance_indi_run(float *heading_sp); extern void guidance_indi_run(float *heading_sp);
extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading); extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading);
extern void guidance_indi_init(void); extern void guidance_indi_init(void);
extern void guidance_indi_propagate_filters(void);
struct guidance_indi_hybrid_params { struct guidance_indi_hybrid_params {
float pos_gain; float pos_gain;
@@ -49,6 +48,9 @@ struct guidance_indi_hybrid_params {
float heading_bank_gain; float heading_bank_gain;
}; };
extern struct FloatVect3 sp_accel;
extern struct FloatVect3 gi_speed_sp;
extern struct guidance_indi_hybrid_params gih_params; extern struct guidance_indi_hybrid_params gih_params;
extern float guidance_indi_specific_force_gain; extern float guidance_indi_specific_force_gain;
extern float guidance_indi_max_airspeed; extern float guidance_indi_max_airspeed;
sw/airborne/firmwares/rotorcraft/navigation.h
-4
View File
@@ -126,11 +126,7 @@ extern void nav_parse_MOVE_WP(uint8_t *buf);
extern void set_exception_flag(uint8_t flag_num); extern void set_exception_flag(uint8_t flag_num);
extern float nav_max_speed;
extern bool force_forward; extern bool force_forward;
extern struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain);
extern struct FloatVect3 nav_get_speed_setpoint(float pos_gain);
extern struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v, struct FloatVect2 to_end_v, struct EnuCoor_i target, float pos_gain);
extern float get_dist2_to_waypoint(uint8_t wp_id); extern float get_dist2_to_waypoint(uint8_t wp_id);
extern float get_dist2_to_point(struct EnuCoor_i *p); extern float get_dist2_to_point(struct EnuCoor_i *p);
sw/tools/parrot/bebop.py
+1 -1
View File
@@ -160,7 +160,7 @@ class Bebop(ParrotUtils):
# Parse the extra arguments # Parse the extra arguments
self.parser.add_argument('--min_version', metavar='MIN', default='3.3.0', self.parser.add_argument('--min_version', metavar='MIN', default='3.3.0',
help='force minimum version allowed') help='force minimum version allowed')
self.parser.add_argument('--max_version', metavar='MAX', default='4.4.2', self.parser.add_argument('--max_version', metavar='MAX', default='4.7.1',
help='force maximum version allowed') help='force maximum version allowed')
ss = self.subparsers.add_parser('networkid', help='Set the network ID (SSID) to join in managed mode') ss = self.subparsers.add_parser('networkid', help='Set the network ID (SSID) to join in managed mode')