mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-22 12:28:03 +08:00
Nederdrone reintegration (#2723)
* [ctrl] Add simple INDI scheduling module * [conf] Cleanup Nederdrones - Fix magnetometer orientation - Remove unused defines - Fix ailerons orientation Co-authored-by: Ewoud Smeur <ewoud_smeur@msn.com>
This commit is contained in:
Freek van Tienen
GitHub
@@ -26,8 +26,7 @@
|
||||
<define name="MS45XX_I2C_DEV" value="i2c4"/>
|
||||
</module>
|
||||
|
||||
<!-- TODO: add this module -->
|
||||
<!--module name="scheduling_indi_simple"/-->
|
||||
<module name="scheduling_indi_simple"/>
|
||||
|
||||
<!-- Forward FuelCell data back to the GCS -->
|
||||
<!--module name="generic_uart_sensor"/-->
|
||||
@@ -50,7 +49,7 @@
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
|
||||
<!--module name="scheduling_indi_simple"/-->
|
||||
<module name="scheduling_indi_simple"/>
|
||||
|
||||
<module name="logger_file">
|
||||
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
|
||||
@@ -74,8 +73,6 @@
|
||||
<module name="imu" type="mpu6000"/>
|
||||
<module name="gps" type="ublox"/>
|
||||
<module name="gps" type="ubx_ucenter"/>
|
||||
<!--<module name="stabilization" type="int_quat"/>-->
|
||||
<!--<module name="gain_scheduling"/>-->
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="stabilization" type="rate_indi"/>
|
||||
<module name="ins" type="ekf2" />
|
||||
@@ -92,13 +89,14 @@
|
||||
<module name="mag_lis3mdl">
|
||||
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
|
||||
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
|
||||
<define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
|
||||
<define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
|
||||
</module>
|
||||
<!--module name="lidar" type="tfmini">
|
||||
<configure name="TFMINI_PORT" value="UART4"/>
|
||||
<configure name="USE_TFMINI_AGL" value="FALSE"/>
|
||||
</module-->
|
||||
|
||||
<!--<module name="guidance" type="hybrid"/>-->
|
||||
<module name="guidance" type="indi_hybrid">
|
||||
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
|
||||
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
|
||||
@@ -206,11 +204,12 @@
|
||||
</command_laws>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="WINDTUNNEL_TO_BODY_THETA" value="-90." unit="deg"/>
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 17.9024749557 * adc)"/><!-- TODO: verify/calibrate -->
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
|
||||
<!-- Basic navigation settings -->
|
||||
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
|
||||
<define name="NAV_DESCEND_VSPEED" value="-0.5"/>
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 17.9024749557 * adc)"/>
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
|
||||
|
||||
<!-- Avoid GPS loss behavior when having RC or datalink -->
|
||||
@@ -219,15 +218,15 @@
|
||||
</section>
|
||||
|
||||
<section name="FORWARD">
|
||||
<!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
|
||||
<!--The Nederdrone 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-->
|
||||
<!-- This is the pitch angle that the Nederdrone will have in forward flight, where 0 degrees is hover-->
|
||||
<define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
|
||||
<!-- For RC coordinated turns, lower because the yawing was too slow -->
|
||||
<define name="MAX_FWD_SPEED" value="20.0"/>
|
||||
<!-- For hybrid guidance -->
|
||||
<define name="MAX_AIRSPEED" value="20.0"/>
|
||||
|
||||
<!-- Enable airspeed measurements -->
|
||||
<define name="USE_AIRSPEED" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
@@ -236,7 +235,6 @@
|
||||
<define name="MPU_CHAN_X" value="1"/>
|
||||
<define name="MPU_CHAN_Y" value="0"/>
|
||||
<define name="MPU_CHAN_Z" value="2"/>
|
||||
|
||||
<define name="MPU_X_SIGN" value="1"/>
|
||||
<define name="MPU_Y_SIGN" value="1"/>
|
||||
<define name="MPU_Z_SIGN" value="-1"/>
|
||||
@@ -249,11 +247,7 @@
|
||||
<define name="ACCEL_Y_SENS" value="4.9016250738902425" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="4.846689188075245" integer="16"/>
|
||||
|
||||
<!--define name= "MAG_X_CURRENT_COEF" value="-1.7139708082316483"/>
|
||||
<define name= "MAG_Y_CURRENT_COEF" value="-0.7696784114750511"/>
|
||||
<define name= "MAG_Z_CURRENT_COEF" value="1.1626106815908253"/-->
|
||||
|
||||
<!-- Calibrated at valkenburg 20-05-2020 -->
|
||||
<!-- Calibrated at valkenburg 20-05-2020 (external magnetometer) -->
|
||||
<define name="MAG_X_NEUTRAL" value="866"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="-1530"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="-3313"/>
|
||||
@@ -267,33 +261,6 @@
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
|
||||
<define name="H_X" value="0.3770441"/>
|
||||
<define name="H_Y" value="0.0193986"/>
|
||||
<define name="H_Z" value="0.9259921"/>
|
||||
<define name="USE_GPS_HEADING" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_P" value="140" unit="deg/s"/>
|
||||
<define name="SP_MAX_Q" value="140" unit="deg/s"/>
|
||||
<define name="SP_MAX_R" value="140" unit="deg/s"/>
|
||||
<define name="DEADBAND_P" value="20"/>
|
||||
<define name="DEADBAND_Q" value="20"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="GAIN_P" value="400"/>
|
||||
<define name="GAIN_Q" value="400"/>
|
||||
<define name="GAIN_R" value="350"/>
|
||||
|
||||
<define name="IGAIN_P" value="75"/>
|
||||
<define name="IGAIN_Q" value="75"/>
|
||||
<define name="IGAIN_R" value="50"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="60." unit="deg"/>
|
||||
@@ -301,55 +268,20 @@
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.85"/>
|
||||
<define name="REF_MAX_P" value="100." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(3000.)"/>
|
||||
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.85"/>
|
||||
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
|
||||
<define name="REF_OMEGA_R" value="300" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.85"/>
|
||||
<define name="REF_MAX_R" value="180." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="800"/>
|
||||
<define name="PHI_DGAIN" value="350"/>
|
||||
<define name="PHI_IGAIN" value="40"/>
|
||||
<define name="THETA_PGAIN" value="800"/>
|
||||
<define name="THETA_DGAIN" value="350"/>
|
||||
<define name="THETA_IGAIN" value="40"/>
|
||||
<define name="PSI_PGAIN" value="1250"/>
|
||||
<define name="PSI_DGAIN" value="320"/>
|
||||
<define name="PSI_IGAIN" value="10"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="0"/>
|
||||
<define name="THETA_DDGAIN" value="0"/>
|
||||
<define name="PSI_DDGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<!-- control effectiveness (hover) -->
|
||||
<define name="G1_P" value="0.0030"/>
|
||||
<define name="G1_Q" value="0.0035"/>
|
||||
<define name="G1_R" value="0.0004"/>
|
||||
<define name="G2_R" value="0.00015"/>
|
||||
|
||||
<!-- control effectiveness (forward) -->
|
||||
<define name="FORWARD_G1_P" value="0.0020"/>
|
||||
<define name="FORWARD_G1_Q" value="0.0077"/>
|
||||
<define name="FORWARD_G1_R" value="0.004"/>
|
||||
|
||||
<!--P effectiveness if center props are switched off-->
|
||||
<define name="FORWARD_G1_P_FEW_PROPS" value="0.0020"/>
|
||||
<!--Part of yaw eff that comes from motors-->
|
||||
<define name="MOT_YAW_EFF" value="0.00"/>
|
||||
<define name="MOT_PITCH_EFF" value="0.003"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="30.0"/>
|
||||
<define name="REF_ERR_Q" value="30.0"/>
|
||||
@@ -380,28 +312,6 @@
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<section name="GAIN_SETS">
|
||||
<define name="NUMBER_OF_GAINSETS" value="2"/>
|
||||
<define name="SCHEDULING_VARIABLE" value="guidance_hybrid_norm_ref_airspeed"/>
|
||||
<define name="SCHEDULING_POINTS" value="{4, 10}"/>
|
||||
<define name="SCHEDULING_VARIABLE_FRAC" value="8"/>
|
||||
|
||||
<define name="PHI_P" value="{800, 600}"/>
|
||||
<define name="PHI_D" value="{350, 250}"/>
|
||||
<define name="PHI_I" value="{40, 20}"/>
|
||||
<define name="PHI_DD" value="{0, 0}"/>
|
||||
|
||||
<define name="THETA_P" value="{800, 400}"/>
|
||||
<define name="THETA_D" value="{350, 150}"/>
|
||||
<define name="THETA_I" value="{40, 100}"/>
|
||||
<define name="THETA_DD" value="{0, 0}"/>
|
||||
|
||||
<define name="PSI_P" value="{1250, 1250}"/>
|
||||
<define name="PSI_D" value="{320, 320}"/>
|
||||
<define name="PSI_I" value="{10, 10}"/>
|
||||
<define name="PSI_DD" value="{0, 0}"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="310"/>
|
||||
<define name="HOVER_KD" value="130"/>
|
||||
|
||||
@@ -27,8 +27,7 @@
|
||||
<define name="MS45XX_I2C_DEV" value="i2c4"/>
|
||||
</module>
|
||||
|
||||
<!-- TODO: add this module -->
|
||||
<!--module name="scheduling_indi_simple"/-->
|
||||
<module name="scheduling_indi_simple"/>
|
||||
|
||||
<!-- Forward FuelCell data back to the GCS -->
|
||||
<!--module name="generic_uart_sensor"/-->
|
||||
@@ -51,7 +50,7 @@
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
|
||||
<!--module name="scheduling_indi_simple"/-->
|
||||
<module name="scheduling_indi_simple"/>
|
||||
|
||||
<module name="logger_file">
|
||||
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
|
||||
@@ -75,8 +74,6 @@
|
||||
<module name="imu" type="mpu6000"/>
|
||||
<module name="gps" type="ublox"/>
|
||||
<module name="gps" type="ubx_ucenter"/>
|
||||
<!--<module name="stabilization" type="int_quat"/>-->
|
||||
<!--<module name="gain_scheduling"/>-->
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="stabilization" type="rate_indi"/>
|
||||
<module name="ins" type="ekf2" />
|
||||
@@ -93,13 +90,14 @@
|
||||
<module name="mag_lis3mdl">
|
||||
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
|
||||
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
|
||||
<define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
|
||||
<define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
|
||||
</module>
|
||||
<!--module name="lidar" type="tfmini">
|
||||
<configure name="TFMINI_PORT" value="UART4"/>
|
||||
<configure name="USE_TFMINI_AGL" value="FALSE"/>
|
||||
</module-->
|
||||
|
||||
<!--<module name="guidance" type="hybrid"/>-->
|
||||
<module name="guidance" type="indi_hybrid">
|
||||
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
|
||||
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
|
||||
@@ -146,8 +144,8 @@
|
||||
<servo name="MOTOR_10" no="3" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_11" no="4" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_12" no="5" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="AIL_3" no="6" min="6000" neutral="0" max="6000"/>
|
||||
<servo name="FLAP_3" no="7" min="6000" neutral="0" max="6000"/>
|
||||
<servo name="AIL_3" no="6" min="6000" neutral="0" max="-6000"/>
|
||||
<servo name="FLAP_3" no="7" min="6000" neutral="0" max="-6000"/>
|
||||
<servo name="FLAP_4" no="8" min="-6000" neutral="0" max="6000"/>
|
||||
<servo name="AIL_4" no="9" min="-6000" neutral="0" max="6000"/>
|
||||
</servos>
|
||||
@@ -207,11 +205,12 @@
|
||||
</command_laws>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="WINDTUNNEL_TO_BODY_THETA" value="-90." unit="deg"/>
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 18.9040120162 * adc)"/><!-- TODO: verify/calibrate -->
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
|
||||
<!-- Basic navigation settings -->
|
||||
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
|
||||
<define name="NAV_DESCEND_VSPEED" value="-0.5"/>
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 18.9040120162 * adc)"/>
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
|
||||
|
||||
<!-- Avoid GPS loss behavior when having RC or datalink -->
|
||||
@@ -220,15 +219,15 @@
|
||||
</section>
|
||||
|
||||
<section name="FORWARD">
|
||||
<!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
|
||||
<!--The Nederdrone 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-->
|
||||
<!-- This is the pitch angle that the Nederdrone will have in forward flight, where 0 degrees is hover-->
|
||||
<define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
|
||||
<!-- For RC coordinated turns, lower because the yawing was too slow -->
|
||||
<define name="MAX_FWD_SPEED" value="20.0"/>
|
||||
<!-- For hybrid guidance -->
|
||||
<define name="MAX_AIRSPEED" value="20.0"/>
|
||||
|
||||
<!-- Enable airspeed measurements -->
|
||||
<define name="USE_AIRSPEED" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
@@ -237,7 +236,6 @@
|
||||
<define name="MPU_CHAN_X" value="1"/>
|
||||
<define name="MPU_CHAN_Y" value="0"/>
|
||||
<define name="MPU_CHAN_Z" value="2"/>
|
||||
|
||||
<define name="MPU_X_SIGN" value="1"/>
|
||||
<define name="MPU_Y_SIGN" value="1"/>
|
||||
<define name="MPU_Z_SIGN" value="-1"/>
|
||||
@@ -272,33 +270,6 @@
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
|
||||
<define name="H_X" value="0.3770441"/>
|
||||
<define name="H_Y" value="0.0193986"/>
|
||||
<define name="H_Z" value="0.9259921"/>
|
||||
<define name="USE_GPS_HEADING" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_P" value="140" unit="deg/s"/>
|
||||
<define name="SP_MAX_Q" value="140" unit="deg/s"/>
|
||||
<define name="SP_MAX_R" value="140" unit="deg/s"/>
|
||||
<define name="DEADBAND_P" value="20"/>
|
||||
<define name="DEADBAND_Q" value="20"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="GAIN_P" value="400"/>
|
||||
<define name="GAIN_Q" value="400"/>
|
||||
<define name="GAIN_R" value="350"/>
|
||||
|
||||
<define name="IGAIN_P" value="75"/>
|
||||
<define name="IGAIN_Q" value="75"/>
|
||||
<define name="IGAIN_R" value="50"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="60." unit="deg"/>
|
||||
@@ -306,55 +277,20 @@
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.85"/>
|
||||
<define name="REF_MAX_P" value="100." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(3000.)"/>
|
||||
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.85"/>
|
||||
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
|
||||
<define name="REF_OMEGA_R" value="300" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.85"/>
|
||||
<define name="REF_MAX_R" value="180." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="800"/>
|
||||
<define name="PHI_DGAIN" value="350"/>
|
||||
<define name="PHI_IGAIN" value="40"/>
|
||||
<define name="THETA_PGAIN" value="800"/>
|
||||
<define name="THETA_DGAIN" value="350"/>
|
||||
<define name="THETA_IGAIN" value="40"/>
|
||||
<define name="PSI_PGAIN" value="1250"/>
|
||||
<define name="PSI_DGAIN" value="320"/>
|
||||
<define name="PSI_IGAIN" value="10"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="0"/>
|
||||
<define name="THETA_DDGAIN" value="0"/>
|
||||
<define name="PSI_DDGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<!-- control effectiveness (hover) -->
|
||||
<define name="G1_P" value="0.0030"/>
|
||||
<define name="G1_Q" value="0.0035"/>
|
||||
<define name="G1_R" value="0.0004"/>
|
||||
<define name="G2_R" value="0.00015"/>
|
||||
|
||||
<!-- control effectiveness (forward) -->
|
||||
<define name="FORWARD_G1_P" value="0.0020"/>
|
||||
<define name="FORWARD_G1_Q" value="0.0077"/>
|
||||
<define name="FORWARD_G1_R" value="0.004"/>
|
||||
|
||||
<!--P effectiveness if center props are switched off-->
|
||||
<define name="FORWARD_G1_P_FEW_PROPS" value="0.0020"/>
|
||||
<!--Part of yaw eff that comes from motors-->
|
||||
<define name="MOT_YAW_EFF" value="0.00"/>
|
||||
<define name="MOT_PITCH_EFF" value="0.003"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="30.0"/>
|
||||
<define name="REF_ERR_Q" value="30.0"/>
|
||||
@@ -385,28 +321,6 @@
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<section name="GAIN_SETS">
|
||||
<define name="NUMBER_OF_GAINSETS" value="2"/>
|
||||
<define name="SCHEDULING_VARIABLE" value="guidance_hybrid_norm_ref_airspeed"/>
|
||||
<define name="SCHEDULING_POINTS" value="{4, 10}"/>
|
||||
<define name="SCHEDULING_VARIABLE_FRAC" value="8"/>
|
||||
|
||||
<define name="PHI_P" value="{800, 600}"/>
|
||||
<define name="PHI_D" value="{350, 250}"/>
|
||||
<define name="PHI_I" value="{40, 20}"/>
|
||||
<define name="PHI_DD" value="{0, 0}"/>
|
||||
|
||||
<define name="THETA_P" value="{800, 400}"/>
|
||||
<define name="THETA_D" value="{350, 150}"/>
|
||||
<define name="THETA_I" value="{40, 100}"/>
|
||||
<define name="THETA_DD" value="{0, 0}"/>
|
||||
|
||||
<define name="PSI_P" value="{1250, 1250}"/>
|
||||
<define name="PSI_D" value="{320, 320}"/>
|
||||
<define name="PSI_I" value="{10, 10}"/>
|
||||
<define name="PSI_DD" value="{0, 0}"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="310"/>
|
||||
<define name="HOVER_KD" value="130"/>
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="scheduling_indi_simple" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Interpolation of control effectivenss matrix.
|
||||
This is necessary if the vehicle has different operating points,
|
||||
with significantly different control effectivenss.
|
||||
|
||||
If instead using online adaptation is an option, be sure to
|
||||
not use this module at the same time!
|
||||
</description>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="scheduling">
|
||||
<dl_setting var="use_scheduling" min="0" step="1" max="1" shortname="enable" persistent="true" module="ctrl/scheduling_indi_simple"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="scheduling_indi_simple.h"/>
|
||||
</header>
|
||||
<init fun="ctrl_eff_scheduling_init()"/>
|
||||
<periodic fun="ctrl_eff_scheduling_periodic()" freq="20"/>
|
||||
<makefile>
|
||||
<file name="scheduling_indi_simple.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -293,7 +293,7 @@
|
||||
telemetry="telemetry/highspeed_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.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/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi_simple.xml"
|
||||
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.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/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
@@ -304,7 +304,7 @@
|
||||
telemetry="telemetry/highspeed_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.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/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi_simple.xml"
|
||||
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.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/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
|
||||
@@ -80,7 +80,7 @@ float nav_max_speed = NAV_MAX_SPEED;
|
||||
|
||||
struct FloatVect3 sp_accel = {0.0,0.0,0.0};
|
||||
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
|
||||
float thrust_in_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
|
||||
float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
|
||||
static void guidance_indi_filter_thrust(void);
|
||||
|
||||
#ifndef GUIDANCE_INDI_THRUST_DYNAMICS
|
||||
@@ -377,7 +377,7 @@ void guidance_indi_run(float *heading_sp) {
|
||||
guidance_indi_filter_thrust();
|
||||
|
||||
//Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
|
||||
thrust_in = thrust_filt.o[0] + euler_cmd.z*thrust_in_specific_force_gain;
|
||||
thrust_in = thrust_filt.o[0] + euler_cmd.z*guidance_indi_specific_force_gain;
|
||||
Bound(thrust_in, 0, 9600);
|
||||
|
||||
#if GUIDANCE_INDI_RC_DEBUG
|
||||
|
||||
@@ -49,5 +49,6 @@ struct guidance_indi_hybrid_params {
|
||||
};
|
||||
|
||||
extern struct guidance_indi_hybrid_params gih_params;
|
||||
extern float guidance_indi_specific_force_gain;
|
||||
|
||||
#endif /* GUIDANCE_INDI_HYBRID_H */
|
||||
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Ewoud Smeur <ewoud_smeur@msn.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file modules/ctrl/indi_simple_scheduling.c
|
||||
* Module that interpolates gainsets in flight based on the transition percentage
|
||||
*/
|
||||
|
||||
#include "modules/ctrl/scheduling_indi_simple.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "state.h"
|
||||
#define INDI_SCHEDULING_LOWER_BOUND_G1 0.0001
|
||||
|
||||
int32_t use_scheduling = 1;
|
||||
|
||||
static float g_forward[3] = {STABILIZATION_INDI_FORWARD_G1_P, STABILIZATION_INDI_FORWARD_G1_Q, STABILIZATION_INDI_FORWARD_G1_R};
|
||||
|
||||
static float g_hover[3] = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R};
|
||||
|
||||
//Get the specified gains in the gainlibrary
|
||||
void ctrl_eff_scheduling_init(void)
|
||||
{
|
||||
}
|
||||
|
||||
void ctrl_eff_scheduling_periodic(void)
|
||||
{
|
||||
// Go from transition percentage to ratio
|
||||
/*float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;*/
|
||||
float ratio = 0.0;
|
||||
|
||||
// Ratio is only based on pitch now, as the pitot tube is often not mounted.
|
||||
if (use_scheduling == 1) {
|
||||
ratio = fabs(stateGetNedToBodyEulers_f()->theta) / M_PI_2;
|
||||
} else {
|
||||
ratio = 0.0;
|
||||
}
|
||||
|
||||
//g_forward[0] = STABILIZATION_INDI_FORWARD_G1_P;
|
||||
// When using the pitch slider to take the props out of the mix, adjust the pitch effectiveness
|
||||
//g_forward[1] = STABILIZATION_INDI_FORWARD_G1_Q - (1.0 - pitch_slider) * STABILIZATION_INDI_MOT_PITCH_EFF;
|
||||
// When using the yaw slider to take the props out of the mix, adjust the yaw effectiveness
|
||||
//g_forward[2] = STABILIZATION_INDI_FORWARD_G1_R - (1.0 - yaw_slider) * STABILIZATION_INDI_MOT_YAW_EFF;
|
||||
|
||||
indi.g1.p = g_hover[0] * (1.0 - ratio) + g_forward[0] * ratio;
|
||||
indi.g1.q = g_hover[1] * (1.0 - ratio) + g_forward[1] * ratio;
|
||||
indi.g1.r = g_hover[2] * (1.0 - ratio) + g_forward[2] * ratio;
|
||||
|
||||
// Make sure g1 can never be negative, as the inverse is taken!
|
||||
if (indi.g1.p < INDI_SCHEDULING_LOWER_BOUND_G1) {
|
||||
indi.g1.p = INDI_SCHEDULING_LOWER_BOUND_G1;
|
||||
}
|
||||
if (indi.g1.q < INDI_SCHEDULING_LOWER_BOUND_G1) {
|
||||
indi.g1.q = INDI_SCHEDULING_LOWER_BOUND_G1;
|
||||
}
|
||||
if (indi.g1.r < INDI_SCHEDULING_LOWER_BOUND_G1) {
|
||||
indi.g1.r = INDI_SCHEDULING_LOWER_BOUND_G1;
|
||||
}
|
||||
|
||||
float ratio_spec_force = 0.0;
|
||||
float airspeed = stateGetAirspeed_f();
|
||||
Bound(airspeed, 8.0, 20.0);
|
||||
ratio_spec_force = (airspeed-8.0) / 12.0;
|
||||
guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN * (1.0 - ratio_spec_force)
|
||||
+ GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD * ratio_spec_force;
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Ewoud Smeur <ewoud_smeur@msn.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ctrl/scheduling_indi_simple.h
|
||||
*/
|
||||
|
||||
#ifndef SCHEDULING_INDI_SIMPLE_H
|
||||
#define SCHEDULING_INDI_SIMPLE_H
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h"
|
||||
|
||||
extern int32_t use_scheduling;
|
||||
|
||||
/**
|
||||
* Initialises periodic loop;
|
||||
*/
|
||||
extern void ctrl_eff_scheduling_init(void);
|
||||
|
||||
/**
|
||||
* Periodic function that interpolates between gain sets depending on the scheduling variable.
|
||||
*/
|
||||
extern void ctrl_eff_scheduling_periodic(void);
|
||||
|
||||
#endif /* SCHEDULING_INDI_SIMPLE_H */
|
||||
|
||||
Reference in New Issue
Block a user