mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-24 05:45:59 +08:00
[module] wind estimation from quadrotor motion (#2800)
- add a generic linear kalman filter lib - add a quad model with linear drag and simplified for recent jsbsim - add example frame and noisy NPS sensor config see "Estimating wind using a quadrotor" in IMAV2021 proceedings
This commit is contained in:
Gautier Hattenberger
committed by
GitHub
GitHub
parent
fb0a7a0eb1
commit
e075daf06f
@@ -0,0 +1,274 @@
|
||||
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||
|
||||
<airframe name="Quadricopter WIND estimation">
|
||||
|
||||
<description>
|
||||
* Autopilot: Tawaki
|
||||
* Actuators: 4 in 4 Holybro BLHELI ESC
|
||||
* Telemetry: XBee
|
||||
* GPS: datalink
|
||||
* RC: Futaba
|
||||
</description>
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/>
|
||||
<configure name="AHRS_PROPAGATE_FREQUENCY" value="$(PERIODIC_FREQUENCY)"/>
|
||||
|
||||
<target name="ap" board="tawaki_1.0">
|
||||
<module name="radio_control" type="sbus"/>
|
||||
<configure name="BARO_PERIODIC_FREQUENCY" value="50"/>
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
|
||||
|
||||
<!-- indoor optitrack setup -->
|
||||
<module name="gps" type="datalink">
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
<!-- Use GPS heading instead of magneto -->
|
||||
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
|
||||
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
|
||||
<define name="USE_SONAR" value="0"/>
|
||||
</module>
|
||||
<module name="ins" type="gps_passthrough"/>
|
||||
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<module name="radio_control" type="ppm"/>
|
||||
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B115200"/>
|
||||
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
|
||||
</module>
|
||||
<module name="ins"/>
|
||||
|
||||
</target>
|
||||
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
|
||||
<module name="motor_mixing"/>
|
||||
|
||||
<module name="actuators" type="dshot">
|
||||
<define name="DSHOT_SPEED" value="300"/>
|
||||
</module>
|
||||
|
||||
<module name="board" type="tawaki">
|
||||
<configure name="BOARD_TAWAKI_ROTATED" value="TRUE"/>
|
||||
<define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_98HZ"/>
|
||||
<define name="IMU_MPU_ACCEL_LOWPASS_FILTER" value="MPU60X0_DLPF_ACC_44HZ"/>
|
||||
<define name="IMU_MPU_SMPLRT_DIV" value="1"/>
|
||||
<configure name="MAG_LIS3MDL_I2C_DEV" value="i2c2"/>
|
||||
</module>
|
||||
|
||||
<module name="ahrs" type="int_cmpl_quat"/>
|
||||
|
||||
<module name="stabilization" type="int_quat"/>
|
||||
<module name="air_data"/>
|
||||
|
||||
<module name="wind_estimation_quadrotor"/>
|
||||
|
||||
<!--module name="filter" type="1euro_imu">
|
||||
<define name="AHRS_ICQ_IMU_ID" value="IMU_F1E_ID"/>
|
||||
<define name="AHRS_ALIGNER_IMU_ID" value="IMU_F1E_ID"/>
|
||||
</module-->
|
||||
</firmware>
|
||||
|
||||
<servos driver="DShot">
|
||||
<servo name="FRONT" no="2" min="0" neutral="100" max="2000"/>
|
||||
<servo name="RIGHT" no="1" min="0" neutral="100" max="2000"/>
|
||||
<servo name="BACK" no="3" min="0" neutral="100" max="2000"/>
|
||||
<servo name="LEFT" no="4" min="0" neutral="100" max="2000"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||
<define name="TYPE" value="QUAD_PLUS"/>
|
||||
<!--define name="REVERSE" value="TRUE"/-->
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
<set servo="FRONT" value="motor_mixing.commands[MOTOR_FRONT]"/>
|
||||
<set servo="RIGHT" value="motor_mixing.commands[MOTOR_RIGHT]"/>
|
||||
<set servo="BACK" value="motor_mixing.commands[MOTOR_BACK]"/>
|
||||
<set servo="LEFT" value="motor_mixing.commands[MOTOR_LEFT]"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="WIND_ESTIMATION" prefix="WE_QUAD_">
|
||||
<define name="MASS" value="0.896"/>
|
||||
<define name="DRAG" value="0.230"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="GYRO_P_SIGN" value="-1"/>
|
||||
<define name="GYRO_Q_SIGN" value="-1"/>
|
||||
<define name="GYRO_R_SIGN" value="1"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||
<define name="ACCEL_X_NEUTRAL" value="-66"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="134"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="12"/>
|
||||
<define name="ACCEL_X_SENS" value="2.45208261737" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="2.64210954935" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="2.45501830376" integer="16"/>
|
||||
|
||||
<define name="MAG_X_SIGN" value="1"/>
|
||||
<define name="MAG_Y_SIGN" value="1"/>
|
||||
<define name="MAG_Z_SIGN" value="1"/>
|
||||
<define name="MAG_X_NEUTRAL" value="61"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="-643"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="-308"/>
|
||||
<define name="MAG_X_SENS" value="0.677847523102" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="0.69808834351" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="0.692571212902" integer="16"/>
|
||||
|
||||
<!--define name= "MAG_X_CURRENT_COEF" value="118.702992289"/>
|
||||
<define name= "MAG_Y_CURRENT_COEF" value="135.416707118"/>
|
||||
<define name= "MAG_Z_CURRENT_COEF" value="-301.422405936"/-->
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<!-- Magnetic Field Calculator
|
||||
http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
|
||||
http://www.wolframalpha.com
|
||||
|
||||
Enac : Lat 43.564080°N, Lon 1.481289°E
|
||||
Normalize[{23759.6, 138.7, 39666.4}]
|
||||
Result(0.51385, 0.00299, 0.85787)
|
||||
-->
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="H_X" value="0.51385"/>
|
||||
<define name="H_Y" value="0.00299"/>
|
||||
<define name="H_Z" value="0.85787"/>
|
||||
<define name="INV_NXZ" value="0.25"/>
|
||||
<define name="INV_NH" value="2.0"/>
|
||||
<define name="INV_MVZ" value="8."/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||
|
||||
<define name="SP_MAX_P" value="10000"/>
|
||||
<define name="SP_MAX_Q" value="10000"/>
|
||||
<define name="SP_MAX_R" value="10000"/>
|
||||
|
||||
<define name="GAIN_P" value="400"/>
|
||||
<define name="GAIN_Q" value="400"/>
|
||||
<define name="GAIN_R" value="350"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="45." unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45." unit="deg"/>
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="250"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.85"/>
|
||||
<define name="REF_MAX_P" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="800" 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="500" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.85"/>
|
||||
<define name="REF_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="300"/>
|
||||
<define name="PHI_DGAIN" value="300"/>
|
||||
<define name="PHI_IGAIN" value="45"/>
|
||||
|
||||
<define name="THETA_PGAIN" value="300"/>
|
||||
<define name="THETA_DGAIN" value="300"/>
|
||||
<define name="THETA_IGAIN" value="45"/>
|
||||
|
||||
<define name="PSI_PGAIN" value="1000"/>
|
||||
<define name="PSI_DGAIN" value="600"/>
|
||||
<define name="PSI_IGAIN" value="30"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="80"/>
|
||||
<define name="THETA_DDGAIN" value="80"/>
|
||||
<define name="PSI_DDGAIN" value="170"/>
|
||||
|
||||
<define name="PHI_AGAIN" value="0"/>
|
||||
<define name="THETA_AGAIN" value="0"/>
|
||||
<define name="PSI_AGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="REF_MIN_ZDD" value="-0.4*9.81"/>
|
||||
<define name="REF_MAX_ZDD" value=" 0.4*9.81"/>
|
||||
<define name="REF_MIN_ZD" value="-1.5"/>
|
||||
<define name="REF_MAX_ZD" value=" 1."/>
|
||||
<define name="HOVER_KP" value="48"/>
|
||||
<define name="HOVER_KD" value="80"/>
|
||||
<define name="HOVER_KI" value="11"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.61"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
||||
<define name="ADAPT_NOISE_FACTOR" value="1."/>
|
||||
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.6"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||
<define name="PGAIN" value="41"/>
|
||||
<define name="DGAIN" value="108"/>
|
||||
<define name="IGAIN" value="20"/>
|
||||
<define name="NGAIN" value="0"/>
|
||||
<!-- feedforward -->
|
||||
<define name="AGAIN" value="0"/>
|
||||
<define name="REF_MAX_SPEED" value="2.5"/>
|
||||
<define name="REF_MAX_ACCEL" value="2.5"/>
|
||||
</section>
|
||||
|
||||
<section name="NAV">
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="2.0"/>
|
||||
<define name="RECTANGLE_SURVEY_HEADING_WE" value="180."/>
|
||||
<define name="NAV_CLIMB_VSPEED" value="1.0"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.8" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="20000" unit="mA"/>
|
||||
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000" value="mA"/>
|
||||
<define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.0"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<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"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="simple_quad_wind" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,50 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
<module name="wind_estimation_quadrotor" dir="meteo">
|
||||
<doc>
|
||||
<description>
|
||||
Wind estimation from quadrotor motion
|
||||
Estimation using a linear Kalman filter
|
||||
|
||||
For details, see:
|
||||
G. Hattenberger, M. Bronz, and J. Condomines, “Estimating wind using a quadrotor,”
|
||||
in 12th international micro air vehicle conference, Puebla, México, 2021, p. 124–130.
|
||||
</description>
|
||||
<section name="WE_QUAD" prefix="WE_QUAD_">
|
||||
<define name="MASS" value="required" description="mass of the airframe" unit="kg"/>
|
||||
<define name="DRAG" value="required" description="linear drag coefficient modeling the relation between drag and bank angle"/>
|
||||
<define name="P0_VA" value="1." description="initial covariance on airspeed estimate"/>
|
||||
<define name="P0_W" value="1." description="initial covariance on wind estimate"/>
|
||||
<define name="Q_VA" value="0.05" description="process noise on airspeed"/>
|
||||
<define name="Q_W" value="0.001" description="process noise on wind estimate"/>
|
||||
<define name="R" value="0.5" description="measurement noise on ground speed"/>
|
||||
<define name="UPDATE_STATE" value="FALSE|TRUE" description="update directly wind estimation in state interface (default: TRUE)"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="Wind quad">
|
||||
<dl_setting MIN="0.001" MAX="1" STEP="0.01" VAR="we_quad_params.Q_va" shortname="Q_va" module="meteo/wind_estimation_quadrotor" handler="SetQva"/>
|
||||
<dl_setting MIN="0.001" MAX="0.1" STEP="0.001" VAR="we_quad_params.Q_w" shortname="Q_w" module="meteo/wind_estimation_quadrotor" handler="SetQw"/>
|
||||
<dl_setting MIN="0.01" MAX="3" STEP="0.01" VAR="we_quad_params.R" shortname="R" module="meteo/wind_estimation_quadrotor" handler="SetR"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="wind_estimation_quadrotor.h"/>
|
||||
</header>
|
||||
<init fun="wind_estimation_quadrotor_init()"/>
|
||||
<periodic fun="wind_estimation_quadrotor_periodic()" freq="10.0" start="wind_estimation_quadrotor_stop()" stop="wind_estimation_quadrotor_start()" autorun="FALSE"/>
|
||||
<periodic fun="wind_estimation_quadrotor_report()" freq="10.0" autorun="FALSE"/>
|
||||
<makefile>
|
||||
<file name="wind_estimation_quadrotor.c"/>
|
||||
<file name="linear_kalman_filter.c" dir="filters"/>
|
||||
<test>
|
||||
<define name="WE_QUAD_MASS" value="1."/>
|
||||
<define name="WE_QUAD_DRAG" value="1."/>
|
||||
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
|
||||
<define name="DOWNLINK_DEVICE" value="uart0"/>
|
||||
<define name="USE_UART0"/>
|
||||
<define name="WIND_ESTIMATION_QUADROTOR_PERIODIC_PERIOD" value="0.1"/>
|
||||
</test>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,314 @@
|
||||
<?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>Gautier Hattenberger</author>
|
||||
<filecreationdate>26-08-2021</filecreationdate>
|
||||
<version>Version 1.0</version>
|
||||
<description>
|
||||
Simple Quadrotor in + configuration without rotor dynamic (front/back motors turning CW, left/right CCW)
|
||||
|
||||
Drag model is linear with speed and correspond to the quadrotor used in
|
||||
IMAV2021 paper "Estimating wind using a quadrotor" (Hattenberger, Bronz, Condomines)
|
||||
</description>
|
||||
</fileheader>
|
||||
|
||||
<metrics>
|
||||
<wingarea unit="M2"> 1 </wingarea>
|
||||
<wingspan unit="M"> 1 </wingspan>
|
||||
<chord unit="M"> 1 </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.0068 </ixx>
|
||||
<iyy unit="KG*M2"> 0.0068 </iyy>
|
||||
<izz unit="KG*M2"> 0.0136 </izz>
|
||||
<ixy unit="KG*M2"> 0. </ixy>
|
||||
<ixz unit="KG*M2"> 0. </ixz>
|
||||
<iyz unit="KG*M2"> 0. </iyz>
|
||||
<emptywt unit="KG"> 0.896 </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.25 </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.25</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.25</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.25</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>
|
||||
|
||||
<external_reactions>
|
||||
|
||||
<property>fcs/front_motor</property>
|
||||
<property>fcs/back_motor</property>
|
||||
<property>fcs/right_motor</property>
|
||||
<property>fcs/left_motor</property>
|
||||
|
||||
<!-- First the lift forces produced by each propeller -->
|
||||
|
||||
<force name="front_motor" frame="BODY">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/front_motor</property>
|
||||
<value> 3.6 </value>
|
||||
<value>0.224808943</value> <!-- N to LBS -->
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>-0.25</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="back_motor" frame="BODY">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/back_motor</property>
|
||||
<value> 3.6 </value>
|
||||
<value>0.224808943</value> <!-- N to LBS -->
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0.25</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">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/right_motor</property>
|
||||
<value> 3.6 </value>
|
||||
<value>0.224808943</value> <!-- N to LBS -->
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>0.25</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<force name="left_motor" frame="BODY">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/left_motor</property>
|
||||
<value> 3.6 </value>
|
||||
<value>0.224808943</value> <!-- N to LBS -->
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>-0.25</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</force>
|
||||
|
||||
<!-- Then the Moment Couples -->
|
||||
|
||||
<moment name="front_couple" frame="BODY">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/front_motor</property>
|
||||
<value> 0.1 </value>
|
||||
<value> 0.738 </value> <!-- N.m to FT.LBS -->
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>-0.25</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="back_couple" frame="BODY">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/back_motor</property>
|
||||
<value> 0.1 </value>
|
||||
<value> 0.738 </value> <!-- N.m to FT.LBS -->
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0.25</x>
|
||||
<y>0</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>-1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="right_couple" frame="BODY">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/right_motor</property>
|
||||
<value> 0.1 </value>
|
||||
<value> 0.738 </value> <!-- N.m to FT.LBS -->
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>0.25</y>
|
||||
<z>0</z>
|
||||
</location>
|
||||
<direction>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<z>1</z>
|
||||
</direction>
|
||||
</moment>
|
||||
|
||||
<moment name="left_couple" frame="BODY">
|
||||
<function>
|
||||
<product>
|
||||
<property>fcs/left_motor</property>
|
||||
<value> 0.1 </value>
|
||||
<value> 0.738 </value> <!-- N.m to FT.LBS -->
|
||||
</product>
|
||||
</function>
|
||||
<location unit="M">
|
||||
<x>0</x>
|
||||
<y>-0.25</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>velocities/vtrue-fps</property>
|
||||
<value>0.3048</value> <!-- conversion in m/s -->
|
||||
<value>0.230292</value> <!-- drag coef -->
|
||||
<value>0.224808943</value> <!-- N to LBS -->
|
||||
</product>
|
||||
</function>
|
||||
</axis>
|
||||
</aerodynamics>
|
||||
|
||||
</fdm_config>
|
||||
@@ -0,0 +1,194 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Felix Ruess <felix.ruess@gmail.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.
|
||||
*/
|
||||
|
||||
#ifndef NPS_SENSORS_PARAMS_H
|
||||
#define NPS_SENSORS_PARAMS_H
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "modules/imu/imu.h"
|
||||
|
||||
|
||||
#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
|
||||
#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA
|
||||
#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
|
||||
|
||||
// try to determine propagate frequency
|
||||
#if defined AHRS_PROPAGATE_FREQUENCY
|
||||
#define NPS_PROPAGATE AHRS_PROPAGATE_FREQUENCY
|
||||
#elif defined INS_PROPAGATE_FREQUENCY
|
||||
#define NPS_PROPAGATE INS_PROPAGATE_FREQUENCY
|
||||
#elif defined PERIODIC_FREQUENCY
|
||||
#define NPS_PROPAGATE PERIODIC_FREQUENCY
|
||||
#else
|
||||
#define 512. // historical magic number
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Accelerometer
|
||||
*/
|
||||
/* assume resolution is less than 16 bits, so saturation will not occur */
|
||||
#ifndef NPS_ACCEL_MIN
|
||||
#define NPS_ACCEL_MIN -65536
|
||||
#endif
|
||||
#ifndef NPS_ACCEL_MAX
|
||||
#define NPS_ACCEL_MAX 65536
|
||||
#endif
|
||||
/* ms-2 */
|
||||
/* aka 2^10/ACCEL_X_SENS */
|
||||
#define NPS_ACCEL_SENSITIVITY_XX (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS))
|
||||
#define NPS_ACCEL_SENSITIVITY_YY (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS))
|
||||
#define NPS_ACCEL_SENSITIVITY_ZZ (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS))
|
||||
|
||||
#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL
|
||||
#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL
|
||||
#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL
|
||||
/* m2s-4 */
|
||||
#define NPS_ACCEL_NOISE_STD_DEV_X .5
|
||||
#define NPS_ACCEL_NOISE_STD_DEV_Y .5
|
||||
#define NPS_ACCEL_NOISE_STD_DEV_Z .5
|
||||
/* ms-2 */
|
||||
#define NPS_ACCEL_BIAS_X 0
|
||||
#define NPS_ACCEL_BIAS_Y 0
|
||||
#define NPS_ACCEL_BIAS_Z 0
|
||||
/* s */
|
||||
#ifndef NPS_ACCEL_DT
|
||||
#define NPS_ACCEL_DT (1./NPS_PROPAGATE)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Gyrometer
|
||||
*/
|
||||
/* assume resolution is less than 16 bits, so saturation will not occur */
|
||||
#ifndef NPS_GYRO_MIN
|
||||
#define NPS_GYRO_MIN -65536
|
||||
#endif
|
||||
#ifndef NPS_GYRO_MAX
|
||||
#define NPS_GYRO_MAX 65536
|
||||
#endif
|
||||
|
||||
/* 2^12/GYRO_X_SENS */
|
||||
#define NPS_GYRO_SENSITIVITY_PP (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS))
|
||||
#define NPS_GYRO_SENSITIVITY_QQ (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS))
|
||||
#define NPS_GYRO_SENSITIVITY_RR (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS))
|
||||
|
||||
#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL
|
||||
#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL
|
||||
#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL
|
||||
|
||||
#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(9.)
|
||||
#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(9.)
|
||||
#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(9.)
|
||||
|
||||
#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
|
||||
#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
|
||||
#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
|
||||
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
|
||||
/* s */
|
||||
#ifndef NPS_GYRO_DT
|
||||
#define NPS_GYRO_DT (1./NPS_PROPAGATE)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Magnetometer
|
||||
*/
|
||||
/* assume resolution is less than 16 bits, so saturation will not occur */
|
||||
#ifndef NPS_MAG_MIN
|
||||
#define NPS_MAG_MIN -65536
|
||||
#endif
|
||||
#ifndef NPS_MAG_MAX
|
||||
#define NPS_MAG_MAX 65536
|
||||
#endif
|
||||
|
||||
#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
|
||||
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
|
||||
#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
|
||||
|
||||
#define NPS_MAG_SENSITIVITY_XX (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS))
|
||||
#define NPS_MAG_SENSITIVITY_YY (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS))
|
||||
#define NPS_MAG_SENSITIVITY_ZZ (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS))
|
||||
|
||||
#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
|
||||
#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
|
||||
#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
|
||||
|
||||
#define NPS_MAG_NOISE_STD_DEV_X 2e-1
|
||||
#define NPS_MAG_NOISE_STD_DEV_Y 2e-1
|
||||
#define NPS_MAG_NOISE_STD_DEV_Z 2e-1
|
||||
|
||||
#ifndef NPS_MAG_DT
|
||||
#define NPS_MAG_DT (1./100.)
|
||||
#endif
|
||||
|
||||
|
||||
/*
|
||||
* Barometer (pressure and std dev in Pascal)
|
||||
*/
|
||||
#define NPS_BARO_DT (1./50.)
|
||||
#define NPS_BARO_NOISE_STD_DEV 2
|
||||
|
||||
/*
|
||||
* GPS
|
||||
*/
|
||||
|
||||
#ifndef GPS_PERFECT
|
||||
#define GPS_PERFECT 0
|
||||
#endif
|
||||
|
||||
#if GPS_PERFECT
|
||||
|
||||
#define NPS_GPS_SPEED_NOISE_STD_DEV 0.
|
||||
#define NPS_GPS_SPEED_LATENCY 0.
|
||||
#define NPS_GPS_POS_NOISE_STD_DEV 0.001
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_X 0.
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_Y 0.
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_Z 0.
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
|
||||
#define NPS_GPS_POS_LATENCY 0.
|
||||
|
||||
#else
|
||||
|
||||
#define NPS_GPS_SPEED_NOISE_STD_DEV 0.5
|
||||
#define NPS_GPS_SPEED_LATENCY 0.2
|
||||
#define NPS_GPS_POS_NOISE_STD_DEV 2
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
|
||||
#define NPS_GPS_POS_LATENCY 0.2
|
||||
|
||||
#endif /* GPS_PERFECT */
|
||||
|
||||
#ifndef NPS_GPS_DT
|
||||
#define NPS_GPS_DT (1./10.)
|
||||
#endif
|
||||
|
||||
#endif /* NPS_SENSORS_PARAMS_H */
|
||||
@@ -35,6 +35,7 @@
|
||||
<message name="LOGGER_STATUS" period="5.1"/>
|
||||
<message name="LIDAR" period="1.2"/>
|
||||
<message name="INS_EKF2" period=".25"/>
|
||||
<message name="WIND_INFO_RET" period="1."/>
|
||||
</mode>
|
||||
|
||||
<mode name="ppm">
|
||||
|
||||
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "filters/linear_kalman_filter.c"
|
||||
*
|
||||
* Generic discrete Linear Kalman Filter
|
||||
*/
|
||||
|
||||
#include "filters/linear_kalman_filter.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
|
||||
/** Init all matrix and vectors to zero
|
||||
*
|
||||
* @param filter pointer to a filter structure
|
||||
* @param n size of the state vector
|
||||
* @param c size of the command vector
|
||||
* @param m size of the measurement vector
|
||||
* @return false if n, c or m are larger than the maximum value
|
||||
*/
|
||||
bool linear_kalman_filter_init(struct linear_kalman_filter *filter, uint8_t n, uint8_t c, uint8_t m)
|
||||
{
|
||||
if (n > KF_MAX_STATE_SIZE || c > KF_MAX_CMD_SIZE || m > KF_MAX_MEAS_SIZE) {
|
||||
filter->n = 0;
|
||||
filter->c = 0;
|
||||
filter->m = 0;
|
||||
return false; // invalide sizes;
|
||||
}
|
||||
filter->n = n;
|
||||
filter->c = c;
|
||||
filter->m = m;
|
||||
|
||||
// Matrix
|
||||
MAKE_MATRIX_PTR(_A, filter->A, n);
|
||||
float_mat_zero(_A, n, n);
|
||||
MAKE_MATRIX_PTR(_B, filter->B, n);
|
||||
float_mat_zero(_B, n, c);
|
||||
MAKE_MATRIX_PTR(_C, filter->C, m);
|
||||
float_mat_zero(_C, m, n);
|
||||
MAKE_MATRIX_PTR(_P, filter->P, n);
|
||||
float_mat_zero(_P, n, n);
|
||||
MAKE_MATRIX_PTR(_Q, filter->Q, n);
|
||||
float_mat_zero(_Q, n, n);
|
||||
MAKE_MATRIX_PTR(_R, filter->R, m);
|
||||
float_mat_zero(_R, m, m);
|
||||
|
||||
// Vector
|
||||
float_vect_zero(filter->X, n);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/** Prediction step
|
||||
*
|
||||
* X = Ad * X + Bd * U
|
||||
* P = Ad * P * Ad' + Q
|
||||
*
|
||||
* @param filter pointer to the filter structure
|
||||
* @param U command vector
|
||||
*/
|
||||
void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U)
|
||||
{
|
||||
float AX[filter->n];
|
||||
float BU[filter->n];
|
||||
float tmp[filter->n][filter->n];
|
||||
|
||||
MAKE_MATRIX_PTR(_A, filter->A, filter->n);
|
||||
MAKE_MATRIX_PTR(_B, filter->B, filter->n);
|
||||
MAKE_MATRIX_PTR(_P, filter->P, filter->n);
|
||||
MAKE_MATRIX_PTR(_Q, filter->Q, filter->n);
|
||||
MAKE_MATRIX_PTR(_tmp, tmp, filter->n);
|
||||
|
||||
// X = A * X + B * U
|
||||
float_mat_vect_mul(AX, _A, filter->X, filter->n, filter->n);
|
||||
float_mat_vect_mul(BU, _B, U, filter->n, filter->c);
|
||||
float_vect_sum(filter->X, AX, BU, filter->n);
|
||||
|
||||
// P = A * P * A' + Q
|
||||
float_mat_mul(_tmp, _A, _P, filter->n, filter->n, filter->n); // A * P
|
||||
float_mat_mul_transpose(_P, _tmp, _A, filter->n, filter->n, filter->n); // * A'
|
||||
float_mat_sum(_P, _P, _Q, filter->n, filter->n); // + Q
|
||||
}
|
||||
|
||||
/** Update step
|
||||
*
|
||||
* S = Cd * P * Cd' + R
|
||||
* K = P * Cd' / S
|
||||
* X = X + K * (Y - Cd * X)
|
||||
* P = P - K * Cd * P
|
||||
*
|
||||
* @param filter pointer to the filter structure
|
||||
* @param Y measurement vector
|
||||
*/
|
||||
extern void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y)
|
||||
{
|
||||
float S[filter->m][filter->m];
|
||||
float K[filter->n][filter->m];
|
||||
float tmp1[filter->n][filter->m];
|
||||
float tmp2[filter->n][filter->n];
|
||||
|
||||
MAKE_MATRIX_PTR(_P, filter->P, filter->n);
|
||||
MAKE_MATRIX_PTR(_C, filter->C, filter->m);
|
||||
MAKE_MATRIX_PTR(_R, filter->R, filter->m);
|
||||
MAKE_MATRIX_PTR(_S, S, filter->m);
|
||||
MAKE_MATRIX_PTR(_K, K, filter->n);
|
||||
MAKE_MATRIX_PTR(_tmp1, tmp1, filter->n);
|
||||
MAKE_MATRIX_PTR(_tmp2, tmp2, filter->n);
|
||||
|
||||
// S = Cd * P * Cd' + R
|
||||
float_mat_mul_transpose(_tmp1, _P, _C, filter->n, filter->n, filter->m); // P * C'
|
||||
float_mat_mul(_S, _C, _tmp1, filter->m, filter->n, filter->m); // C *
|
||||
float_mat_sum(_S, _S, _R, filter->m, filter->m); // + R
|
||||
|
||||
// K = P * Cd' * inv(S)
|
||||
float_mat_invert(_S, _S, filter->m); // inv(S) in place
|
||||
float_mat_mul(_K, _tmp1, _S, filter->n, filter->m, filter->m); // tmp1 {P*C'} * inv(S)
|
||||
|
||||
// P = P - K * C * P
|
||||
float_mat_mul(_tmp2, _K, _C, filter->n, filter->m, filter->n); // K * C
|
||||
float_mat_mul_copy(_tmp2, _tmp2, _P, filter->n, filter->n, filter->n); // * P
|
||||
float_mat_diff(_P, _P, _tmp2, filter->n, filter->n); // P - K*H*P
|
||||
|
||||
// X = X + K * err
|
||||
float err[filter->n];
|
||||
float dx_err[filter->n];
|
||||
|
||||
float_mat_vect_mul(err, _C, filter->X, filter->m, filter->n); // C * X
|
||||
float_vect_diff(err, Y, err, filter->m); // err = Y - C * X
|
||||
float_mat_vect_mul(dx_err, _K, err, filter->n, filter->m); // K * err
|
||||
float_vect_sum(filter->X, filter->X, dx_err, filter->n); // X + dx_err
|
||||
}
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "filters/linear_kalman_filter.c"
|
||||
*
|
||||
* Generic discrete Linear Kalman Filter
|
||||
*/
|
||||
|
||||
#ifndef LINEAR_KALMAN_FILTER_H
|
||||
#define LINEAR_KALMAN_FILTER_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
// maximum size for the state vector
|
||||
#ifndef KF_MAX_STATE_SIZE
|
||||
#define KF_MAX_STATE_SIZE 6
|
||||
#endif
|
||||
|
||||
// maximum size for the command vector
|
||||
#ifndef KF_MAX_CMD_SIZE
|
||||
#define KF_MAX_CMD_SIZE 2
|
||||
#endif
|
||||
|
||||
// maximum size for the measurement vector
|
||||
#ifndef KF_MAX_MEAS_SIZE
|
||||
#define KF_MAX_MEAS_SIZE 6
|
||||
#endif
|
||||
|
||||
struct linear_kalman_filter {
|
||||
// filled by user after calling init function
|
||||
float A[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]; ///< dynamic matrix
|
||||
float B[KF_MAX_STATE_SIZE][KF_MAX_CMD_SIZE]; ///< command matrix
|
||||
float C[KF_MAX_MEAS_SIZE][KF_MAX_STATE_SIZE]; ///< observation matrix
|
||||
float P[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]; ///< state covariance matrix
|
||||
float Q[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]; ///< proces covariance noise
|
||||
float R[KF_MAX_MEAS_SIZE][KF_MAX_MEAS_SIZE]; ///< measurement covariance noise
|
||||
|
||||
float X[KF_MAX_STATE_SIZE]; ///< estimated state X
|
||||
|
||||
uint8_t n; ///< state vector size (<= KF_MAX_STATE_SIZE)
|
||||
uint8_t c; ///< command vector size (<= KF_MAX_CMD_SIZE)
|
||||
uint8_t m; ///< measurement vector size (<= KF_MAX_MEAS_SIZE)
|
||||
};
|
||||
|
||||
/** Init all matrix and vectors to zero
|
||||
*
|
||||
* @param filter pointer to a filter structure
|
||||
* @param n size of the state vector
|
||||
* @param c size of the command vector
|
||||
* @param m size of the measurement vector
|
||||
* @return false if n, c or m are larger than the maximum value
|
||||
*/
|
||||
extern bool linear_kalman_filter_init(struct linear_kalman_filter *filter, uint8_t n, uint8_t c, uint8_t m);
|
||||
|
||||
/** Prediction step
|
||||
*
|
||||
* X = Ad * X + Bd * U
|
||||
* P = Ad * P * Ad' + Q
|
||||
*
|
||||
* @param filter pointer to the filter structure
|
||||
* @param U command vector
|
||||
*/
|
||||
extern void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U);
|
||||
|
||||
/** Update step
|
||||
*
|
||||
* S = Cd * P * Cd' + R
|
||||
* K = P * Cd' / S
|
||||
* X = X + K * (Y - Cd * X)
|
||||
* P = P - K * Cd * P
|
||||
*
|
||||
* @param filter pointer to the filter structure
|
||||
* @param Y measurement vector
|
||||
*/
|
||||
extern void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y);
|
||||
|
||||
#endif /* DISCRETE_EKF_H */
|
||||
@@ -728,6 +728,25 @@ static inline void float_mat_mul(float **o, float **a, float **b, int m, int n,
|
||||
}
|
||||
}
|
||||
|
||||
/** o = a * b'
|
||||
*
|
||||
* a: [m x n]
|
||||
* b: [l x n]
|
||||
* o: [m x l]
|
||||
*/
|
||||
static inline void float_mat_mul_transpose(float **o, float **a, float **b, int m, int n, int l)
|
||||
{
|
||||
int i, j, k;
|
||||
for (i = 0; i < m; i++) {
|
||||
for (j = 0; j < l; j++) {
|
||||
o[i][j] = 0.;
|
||||
for (k = 0; k < n; k++) {
|
||||
o[i][j] += a[i][k] * b[j][k];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** o = a * b
|
||||
*
|
||||
* a: [m x n]
|
||||
|
||||
@@ -327,15 +327,9 @@ float get_tas_factor(float p, float t)
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from equivalent airspeed.
|
||||
*
|
||||
* True airspeed (TAS) from EAS:
|
||||
* TAS = air_data.tas_factor * EAS
|
||||
*
|
||||
* @param eas equivalent airspeed (EAS) in m/s
|
||||
* @return true airspeed in m/s
|
||||
* Internal utility function to compute current tas factor if needed
|
||||
*/
|
||||
float tas_from_eas(float eas)
|
||||
static void compute_tas_factor(void)
|
||||
{
|
||||
// update tas factor if requested
|
||||
if (air_data.calc_tas_factor) {
|
||||
@@ -351,9 +345,38 @@ float tas_from_eas(float eas)
|
||||
air_data.tas_factor = get_tas_factor(p, CelsiusOfKelvin(t));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from equivalent airspeed.
|
||||
*
|
||||
* True airspeed (TAS) from EAS:
|
||||
* TAS = air_data.tas_factor * EAS
|
||||
*
|
||||
* @param eas equivalent airspeed (EAS) in m/s
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
float tas_from_eas(float eas)
|
||||
{
|
||||
compute_tas_factor();
|
||||
return air_data.tas_factor * eas;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate equivalent airspeed from true airspeed.
|
||||
*
|
||||
* EAS from True airspeed (TAS):
|
||||
* EAS = TAS / air_data.tas_factor
|
||||
*
|
||||
* @param tas true airspeed (TAS) in m/s
|
||||
* @return equivalent airspeed in m/s
|
||||
*/
|
||||
float eas_from_tas(float tas)
|
||||
{
|
||||
compute_tas_factor();
|
||||
return tas / air_data.tas_factor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from dynamic pressure.
|
||||
* Dynamic pressure @f$q@f$ (also called impact pressure) is the
|
||||
|
||||
@@ -101,6 +101,14 @@ extern float get_tas_factor(float p, float t);
|
||||
*/
|
||||
extern float tas_from_eas(float eas);
|
||||
|
||||
/**
|
||||
* Calculate equivalent airspeed from true airspeed.
|
||||
*
|
||||
* @param tas true airspeed (TAS) in m/s
|
||||
* @return equivalent airspeed in m/s
|
||||
*/
|
||||
extern float eas_from_tas(float tas);
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from dynamic pressure.
|
||||
* Dynamic pressure @f$q@f$ (also called impact pressure) is the
|
||||
|
||||
@@ -111,6 +111,10 @@
|
||||
#define AIRSPEED_ETS_ID 4
|
||||
#endif
|
||||
|
||||
#ifndef AIRSPEED_WE_QUAD_ID
|
||||
#define AIRSPEED_WE_QUAD_ID 5
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of Incidence angles (message 24)
|
||||
*/
|
||||
|
||||
@@ -0,0 +1,226 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/** @file "modules/meteo/wind_estimation_quadrotor.c"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Wind estimation from quadrotor motion
|
||||
*/
|
||||
|
||||
#include "modules/meteo/wind_estimation_quadrotor.h"
|
||||
#include "filters/linear_kalman_filter.h"
|
||||
#include "math/pprz_isa.h"
|
||||
#include "state.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "modules/datalink/downlink.h"
|
||||
#include "generated/modules.h"
|
||||
#include "modules/core/abi.h"
|
||||
#include "modules/air_data/air_data.h"
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
#include "modules/datalink/telemetry.h"
|
||||
#endif
|
||||
|
||||
#define WE_QUAD_STATUS_IDLE 0
|
||||
#define WE_QUAD_STATUS_RUN 1
|
||||
|
||||
#define WE_QUAD_STATE_SIZE 4
|
||||
#define WE_QUAD_CMD_SIZE 2
|
||||
#define WE_QUAD_MEAS_SIZE 2
|
||||
|
||||
#define WE_QUAD_VA_X 0
|
||||
#define WE_QUAD_W_X 1
|
||||
#define WE_QUAD_VA_Y 2
|
||||
#define WE_QUAD_W_Y 3
|
||||
|
||||
#ifndef WE_QUAD_P0_VA
|
||||
#define WE_QUAD_P0_VA 1.f
|
||||
#endif
|
||||
|
||||
#ifndef WE_QUAD_P0_W
|
||||
#define WE_QUAD_P0_W 1.f
|
||||
#endif
|
||||
|
||||
#ifndef WE_QUAD_Q_VA
|
||||
#define WE_QUAD_Q_VA .05f
|
||||
#endif
|
||||
|
||||
#ifndef WE_QUAD_Q_W
|
||||
#define WE_QUAD_Q_W .001f
|
||||
#endif
|
||||
|
||||
#ifndef WE_QUAD_R
|
||||
#define WE_QUAD_R .5f
|
||||
#endif
|
||||
|
||||
// update wind in state interface directly
|
||||
#ifndef WE_QUAD_UPDATE_STATE
|
||||
#define WE_QUAD_UPDATE_STATE TRUE
|
||||
#endif
|
||||
|
||||
static const float we_dt = WIND_ESTIMATION_QUADROTOR_PERIODIC_PERIOD;
|
||||
|
||||
static void send_wind(struct transport_tx *trans, struct link_device *dev);
|
||||
|
||||
struct wind_estimation_quadrotor {
|
||||
struct linear_kalman_filter filter;
|
||||
uint8_t status;
|
||||
};
|
||||
|
||||
static struct wind_estimation_quadrotor we_quad;
|
||||
struct wind_estimation_quadrotor_params we_quad_params;
|
||||
|
||||
static void wind_estimation_quadrotor_reset(void)
|
||||
{
|
||||
we_quad.filter.P[0][0] = WE_QUAD_P0_VA;
|
||||
we_quad.filter.P[1][1] = WE_QUAD_P0_W;
|
||||
we_quad.filter.P[2][2] = WE_QUAD_P0_VA;
|
||||
we_quad.filter.P[3][3] = WE_QUAD_P0_W;
|
||||
float_vect_zero(we_quad.filter.X, WE_QUAD_STATE_SIZE);
|
||||
}
|
||||
|
||||
void wind_estimation_quadrotor_init(void)
|
||||
{
|
||||
we_quad.status = WE_QUAD_STATUS_IDLE;
|
||||
|
||||
// init filter and model
|
||||
linear_kalman_filter_init(&we_quad.filter, WE_QUAD_STATE_SIZE, WE_QUAD_CMD_SIZE, WE_QUAD_MEAS_SIZE);
|
||||
we_quad.filter.A[0][0] = 1.f - WE_QUAD_DRAG * we_dt / WE_QUAD_MASS;
|
||||
we_quad.filter.A[1][1] = 1.f;
|
||||
we_quad.filter.A[2][2] = 1.f - WE_QUAD_DRAG * we_dt / WE_QUAD_MASS;
|
||||
we_quad.filter.A[3][3] = 1.f;
|
||||
we_quad.filter.B[0][0] = we_dt / WE_QUAD_MASS;
|
||||
we_quad.filter.B[2][1] = we_dt / WE_QUAD_MASS;
|
||||
we_quad.filter.C[0][0] = 1.f;
|
||||
we_quad.filter.C[0][1] = 1.f;
|
||||
we_quad.filter.C[1][2] = 1.f;
|
||||
we_quad.filter.C[1][3] = 1.f;
|
||||
we_quad.filter.Q[0][0] = WE_QUAD_Q_VA;
|
||||
we_quad.filter.Q[1][1] = WE_QUAD_Q_W;
|
||||
we_quad.filter.Q[2][2] = WE_QUAD_Q_VA;
|
||||
we_quad.filter.Q[3][3] = WE_QUAD_Q_W;
|
||||
we_quad.filter.R[0][0] = WE_QUAD_R;
|
||||
we_quad.filter.R[1][1] = WE_QUAD_R;
|
||||
// reset covariance and state
|
||||
wind_estimation_quadrotor_reset();
|
||||
// external params
|
||||
we_quad_params.Q_va = WE_QUAD_Q_VA;
|
||||
we_quad_params.Q_w = WE_QUAD_Q_W;
|
||||
we_quad_params.R = WE_QUAD_R;
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
// register for periodic telemetry
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind);
|
||||
#endif
|
||||
}
|
||||
|
||||
void wind_estimation_quadrotor_periodic(void)
|
||||
{
|
||||
// compute command from state interface
|
||||
float U[WE_QUAD_CMD_SIZE];
|
||||
struct FloatRMat rot = *stateGetNedToBodyRMat_f();
|
||||
float coef = Max(MAT33_ELMT(rot, 2, 2), 0.5); // limit to 1/2
|
||||
float Tnorm = WE_QUAD_MASS * PPRZ_ISA_GRAVITY / coef;
|
||||
struct FloatVect3 Tbody = { 0.f, 0.f, -Tnorm };
|
||||
struct FloatVect3 Tned;
|
||||
float_rmat_transp_vmult(&Tned, &rot, &Tbody);
|
||||
U[0] = Tned.x;
|
||||
U[1] = Tned.y;
|
||||
|
||||
linear_kalman_filter_predict(&we_quad.filter, U);
|
||||
|
||||
// compute correction from measure
|
||||
float Y[WE_QUAD_MEAS_SIZE];
|
||||
Y[0] = stateGetSpeedNed_f()->x; // north ground speed
|
||||
Y[1] = stateGetSpeedNed_f()->y; // east ground speed
|
||||
|
||||
linear_kalman_filter_update(&we_quad.filter,Y);
|
||||
|
||||
// send airspeed as ABI message
|
||||
struct FloatVect2 tas = { we_quad.filter.X[WE_QUAD_VA_X], we_quad.filter.X[WE_QUAD_VA_Y] };
|
||||
float eas = eas_from_tas(float_vect2_norm(&tas));
|
||||
AbiSendMsgAIRSPEED(AIRSPEED_WE_QUAD_ID, eas);
|
||||
// update wind in state interface only if enabled
|
||||
#if WE_QUAD_UPDATE_STATE
|
||||
struct FloatVect2 wind_ne = {
|
||||
we_quad.filter.X[WE_QUAD_W_X],
|
||||
we_quad.filter.X[WE_QUAD_W_Y]
|
||||
};
|
||||
stateSetHorizontalWindspeed_f(&wind_ne);
|
||||
#endif
|
||||
}
|
||||
|
||||
void wind_estimation_quadrotor_stop(void)
|
||||
{
|
||||
we_quad.status = WE_QUAD_STATUS_IDLE;
|
||||
#if WE_QUAD_UPDATE_STATE
|
||||
struct FloatVect2 zero_wind_ne = { 0.f, 0.f };
|
||||
stateSetHorizontalWindspeed_f(&zero_wind_ne);
|
||||
#endif
|
||||
}
|
||||
|
||||
void wind_estimation_quadrotor_start(void)
|
||||
{
|
||||
wind_estimation_quadrotor_reset();
|
||||
we_quad.status = WE_QUAD_STATUS_RUN;
|
||||
}
|
||||
|
||||
float wind_estimation_quadrotor_SetQva(float Q_va)
|
||||
{
|
||||
we_quad_params.Q_va = Q_va;
|
||||
we_quad.filter.Q[0][0] = Q_va;
|
||||
we_quad.filter.Q[2][2] = Q_va;
|
||||
return Q_va;
|
||||
}
|
||||
|
||||
float wind_estimation_quadrotor_SetQw(float Q_w)
|
||||
{
|
||||
we_quad_params.Q_w = Q_w;
|
||||
we_quad.filter.Q[1][1] = Q_w;
|
||||
we_quad.filter.Q[3][3] = Q_w;
|
||||
return Q_w;
|
||||
}
|
||||
|
||||
float wind_estimation_quadrotor_SetR(float R)
|
||||
{
|
||||
we_quad_params.R = R;
|
||||
we_quad.filter.R[0][0] = R;
|
||||
we_quad.filter.R[1][1] = R;
|
||||
return R;
|
||||
}
|
||||
|
||||
void wind_estimation_quadrotor_report(void)
|
||||
{
|
||||
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, WE_QUAD_STATE_SIZE, we_quad.filter.X);
|
||||
}
|
||||
|
||||
static void send_wind(struct transport_tx *trans, struct link_device *dev)
|
||||
{
|
||||
uint8_t flags = 5; // send horizontal wind and airspeed
|
||||
float upwind = 0.f;
|
||||
struct FloatVect2 as = { we_quad.filter.X[WE_QUAD_VA_X], we_quad.filter.X[WE_QUAD_VA_Y] };
|
||||
float airspeed = float_vect2_norm(&as);
|
||||
pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID,
|
||||
&flags,
|
||||
&we_quad.filter.X[WE_QUAD_W_Y], // east
|
||||
&we_quad.filter.X[WE_QUAD_W_X], // north
|
||||
&upwind,
|
||||
&airspeed);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* Copyright (C) 2021 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/** @file "modules/meteo/wind_estimation_quadrotor.h"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Wind estimation from quadrotor motion
|
||||
*/
|
||||
|
||||
#ifndef WIND_ESTIMATION_QUADROTOR_H
|
||||
#define WIND_ESTIMATION_QUADROTOR_H
|
||||
|
||||
struct wind_estimation_quadrotor_params {
|
||||
float Q_va; ///< model noise on airspeed
|
||||
float Q_w; ///< model noise on wind
|
||||
float R; ///< measurement noise (ground speed)
|
||||
};
|
||||
|
||||
extern struct wind_estimation_quadrotor_params we_quad_params;
|
||||
|
||||
extern void wind_estimation_quadrotor_init(void);
|
||||
extern void wind_estimation_quadrotor_periodic(void);
|
||||
extern void wind_estimation_quadrotor_stop(void);
|
||||
extern void wind_estimation_quadrotor_start(void);
|
||||
extern void wind_estimation_quadrotor_report(void);
|
||||
|
||||
extern float wind_estimation_quadrotor_SetQva(float Q_va);
|
||||
extern float wind_estimation_quadrotor_SetQw(float Q_w);
|
||||
extern float wind_estimation_quadrotor_SetR(float R);
|
||||
|
||||
#endif // WIND_ESTIMATION_QUADROTOR_H
|
||||
@@ -576,6 +576,7 @@ static void init_jsbsim(double dt)
|
||||
delete FDMExec;
|
||||
exit(-1);
|
||||
}
|
||||
cout << "JSBSim model loaded from " << NPS_JSBSIM_MODEL << endl;
|
||||
|
||||
#ifdef DEBUG
|
||||
cerr << "NumEngines: " << FDMExec->GetPropulsion()->GetNumEngines() << endl;
|
||||
|
||||
Reference in New Issue
Block a user