mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
[ins] add new ins based on invariant filter
- experimental invariant filter for complet ins - tested on fixed-wing only, but should work on rotorcraft as well - generic vector and rung-kutta library - fix alt init in utm local frame - settable channel order on mag hmc58xx module
This commit is contained in:
Gautier Hattenberger
@@ -0,0 +1,274 @@
|
|||||||
|
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||||
|
|
||||||
|
<airframe name="JP">
|
||||||
|
|
||||||
|
<modules>
|
||||||
|
<load name="mag_hmc58xx.xml">
|
||||||
|
<define name="MODULE_HMC58XX_UPDATE_AHRS"/>
|
||||||
|
<define name="MAG_HMC58XX_I2C_DEV" value="i2c2"/>
|
||||||
|
</load>
|
||||||
|
<load name="airspeed_adc.xml">
|
||||||
|
<configure name="ADC_AIRSPEED" value="ADC_2"/>
|
||||||
|
<define name="AIRSPEED_QUADRATIC_SCALE" value="0.5"/>
|
||||||
|
<define name="AIRSPEED_BIAS" value="430"/>
|
||||||
|
<define name="MEASURE_AIRSPEED"/>
|
||||||
|
<define name="AIRSPEED_SEND_RAW"/>
|
||||||
|
</load>
|
||||||
|
<!--load name="gps_ubx_ucenter.xml"/-->
|
||||||
|
<load name="sys_mon.xml"/>
|
||||||
|
</modules>
|
||||||
|
|
||||||
|
<firmware name="fixedwing">
|
||||||
|
<define name="USE_I2C1"/>
|
||||||
|
<define name="USE_I2C2"/>
|
||||||
|
<define name="USE_GYRO_PITCH_RATE"/>
|
||||||
|
<configure name="PERIODIC_FREQUENCY" value="125"/>
|
||||||
|
|
||||||
|
<target name="ap" board="apogee_0.99"/>
|
||||||
|
<target name="sim" board="pc"/>
|
||||||
|
<target name="nps" board="pc">
|
||||||
|
<!-- Note NPS needs the ppm type radio_control subsystem -->
|
||||||
|
<subsystem name="fdm" type="jsbsim"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<subsystem name="radio_control" type="ppm"/>
|
||||||
|
|
||||||
|
<!-- Communication -->
|
||||||
|
<subsystem name="telemetry" type="xbee_api"/>
|
||||||
|
|
||||||
|
<!-- Actuators are automatically chosen according to board-->
|
||||||
|
<subsystem name="imu" type="apogee">
|
||||||
|
<define name="USE_MAGNETOMETER"/>
|
||||||
|
<define name="APOGEE_LOWPASS_FILTER" value="MPU60X0_DLPF_20HZ"/>
|
||||||
|
<define name="APOGEE_SMPLRT_DIV" value="7"/>
|
||||||
|
<!--define name="INS_COEF"/-->
|
||||||
|
<!--define name="USE_BAROMETER"/-->
|
||||||
|
<!--define name="BOARD_HAS_BARO"/-->
|
||||||
|
</subsystem>
|
||||||
|
<subsystem name="ins" type="float_invariant">
|
||||||
|
<configure name="AHRS_PROPAGATE_FREQUENCY" value="125"/>
|
||||||
|
<configure name="AHRS_CORRECT_FREQUENCY" value="125"/>
|
||||||
|
<define name="BARO_BOARD_APOGEE_FREQ" value="50"/>
|
||||||
|
<define name="MPL3115_OVERSAMPLING" value="2"/>
|
||||||
|
</subsystem>
|
||||||
|
<subsystem name="control" type="new"/>
|
||||||
|
<subsystem name="navigation"/>
|
||||||
|
<!-- Sensors -->
|
||||||
|
<subsystem name="gps" type="ublox"/>
|
||||||
|
|
||||||
|
<subsystem name="current_sensor">
|
||||||
|
<configure name="ADC_CURRENT_SENSOR" value="ADC_1"/>
|
||||||
|
</subsystem>
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<!-- commands section -->
|
||||||
|
<servos>
|
||||||
|
<servo name="MOTOR" no="0" min="1040" neutral="1040" max="2000"/>
|
||||||
|
<servo name="AILEVON_RIGHT" no="1" max="1100" neutral="1400" min="1900"/>
|
||||||
|
<servo name="AILEVON_LEFT" no="2" max="1900" neutral="1600" min="1100"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<commands>
|
||||||
|
<axis name="THROTTLE" failsafe_value="0"/>
|
||||||
|
<axis name="ROLL" failsafe_value="0"/>
|
||||||
|
<axis name="PITCH" failsafe_value="0"/>
|
||||||
|
</commands>
|
||||||
|
|
||||||
|
<rc_commands>
|
||||||
|
<set command="THROTTLE" value="@THROTTLE"/>
|
||||||
|
<set command="ROLL" value="@ROLL"/>
|
||||||
|
<set command="PITCH" value="@PITCH"/>
|
||||||
|
</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="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
|
||||||
|
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
|
||||||
|
<set servo="MOTOR" value="@THROTTLE"/>
|
||||||
|
<set servo="AILEVON_LEFT" value="$elevator - $aileron"/>
|
||||||
|
<set servo="AILEVON_RIGHT" value="$elevator + $aileron"/>
|
||||||
|
</command_laws>
|
||||||
|
|
||||||
|
<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_">
|
||||||
|
<!-- Calibration Neutral -->
|
||||||
|
<define name="GYRO_P_SIGN" value="1"/>
|
||||||
|
<define name="GYRO_Q_SIGN" value="-1"/>
|
||||||
|
<define name="GYRO_R_SIGN" value="-1"/>
|
||||||
|
<define name="GYRO_P_NEUTRAL" value="-50"/>
|
||||||
|
<define name="GYRO_Q_NEUTRAL" value="20"/>
|
||||||
|
<define name="GYRO_R_NEUTRAL" value="-45"/>
|
||||||
|
<!--define name="GYRO_P_NEUTRAL" value="-66"/>
|
||||||
|
<define name="GYRO_Q_NEUTRAL" value="25"/>
|
||||||
|
<define name="GYRO_R_NEUTRAL" value="55"/-->
|
||||||
|
|
||||||
|
<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="109"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="13"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="-404"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="2.45045342816" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="2.44747844234" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="2.42689216106" integer="16"/>
|
||||||
|
<!--define name="ACCEL_X_NEUTRAL" value="148"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="-8"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="12"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="2.45197002728" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="2.45528611115" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="2.4328991225" 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="27"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="-241"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="139"/>
|
||||||
|
<define name="MAG_X_SENS" value="3.89895537059" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="3.96680514301" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="4.60606895547" integer="16"/>
|
||||||
|
|
||||||
|
<define name="BODY_TO_IMU_PHI" value="0"/>
|
||||||
|
<define name="BODY_TO_IMU_THETA" value="0"/>
|
||||||
|
<define name="BODY_TO_IMU_PSI" value="0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="INS" prefix="INS_">
|
||||||
|
<define name="ROLL_NEUTRAL_DEFAULT" value="-0.990987803591" unit="deg"/>
|
||||||
|
<define name="PITCH_NEUTRAL_DEFAULT" value="11.1209962246" unit="deg"/>
|
||||||
|
<!--muret-->
|
||||||
|
<!--define name="H_X" value="0.5180"/>
|
||||||
|
<define name="H_Y" value="-0.0071"/>
|
||||||
|
<define name="H_Z" value="0.8554"/-->
|
||||||
|
<!--Enac-->
|
||||||
|
<!--define name="H_X" value="0.5138"/>
|
||||||
|
<define name="H_Y" value="0.00019"/>
|
||||||
|
<define name="H_Z" value="0.8578"/-->
|
||||||
|
<!--Ramonville-->
|
||||||
|
<define name="H_X" value="0.5141"/>
|
||||||
|
<define name="H_Y" value="0.0002"/>
|
||||||
|
<define name="H_Z" value="0.8576"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="BAT">
|
||||||
|
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||||
|
<define name="MilliAmpereOfAdc(_adc)" value="(_adc-632)*4.14"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="MISC">
|
||||||
|
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
|
||||||
|
<define name="CARROT" value="5." unit="s"/>
|
||||||
|
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||||
|
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||||
|
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||||
|
|
||||||
|
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||||
|
<!-- outer loop proportional gain -->
|
||||||
|
<define name="ALTITUDE_PGAIN" value="0.12"/>
|
||||||
|
<!-- outer loop saturation -->
|
||||||
|
<define name="ALTITUDE_MAX_CLIMB" value="4."/>
|
||||||
|
<!-- disable climb rate limiter -->
|
||||||
|
<define name="AUTO_CLIMB_LIMIT" value="2*V_CTL_ALTITUDE_MAX_CLIMB"/>
|
||||||
|
|
||||||
|
<!-- Cruise throttle + limits -->
|
||||||
|
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.4"/>
|
||||||
|
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
|
||||||
|
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
|
||||||
|
<define name="AUTO_PITCH_MAX_PITCH" value="20." unit="deg"/>
|
||||||
|
<define name="AUTO_PITCH_MIN_PITCH" value="-20." unit="deg"/>
|
||||||
|
|
||||||
|
<!-- Climb loop (throttle) -->
|
||||||
|
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.088" unit="%/(m/s)"/>
|
||||||
|
<define name="AUTO_THROTTLE_PGAIN" value="0.004"/>
|
||||||
|
<define name="AUTO_THROTTLE_DGAIN" value="0.0"/>
|
||||||
|
<define name="AUTO_THROTTLE_IGAIN" value="0."/>
|
||||||
|
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.077"/>
|
||||||
|
|
||||||
|
<!-- Climb loop (pitch) -->
|
||||||
|
<define name="AUTO_PITCH_PGAIN" value="0.027"/>
|
||||||
|
<define name="AUTO_PITCH_DGAIN" value="0.01"/>
|
||||||
|
<define name="AUTO_PITCH_IGAIN" value="0.0"/>
|
||||||
|
|
||||||
|
<!-- airspeed control -->
|
||||||
|
<define name="AUTO_AIRSPEED_SETPOINT" value="16."/>
|
||||||
|
<define name="AUTO_AIRSPEED_THROTTLE_PGAIN" value="0.1"/>
|
||||||
|
<define name="AUTO_AIRSPEED_THROTTLE_DGAIN" value="0.12"/>
|
||||||
|
<define name="AUTO_AIRSPEED_THROTTLE_IGAIN" value="0.0"/>
|
||||||
|
<define name="AUTO_AIRSPEED_PITCH_PGAIN" value="0.06"/>
|
||||||
|
<define name="AUTO_AIRSPEED_PITCH_DGAIN" value="0.0"/>
|
||||||
|
<define name="AUTO_AIRSPEED_PITCH_IGAIN" value="0.042"/>
|
||||||
|
<define name="AIRSPEED_MAX" value="30" unit="m/s"/>
|
||||||
|
<define name="AIRSPEED_MIN" value="10" unit="m/s"/>
|
||||||
|
|
||||||
|
<!-- groundspeed control -->
|
||||||
|
<define name="AUTO_GROUNDSPEED_SETPOINT" value="15" unit="m/s"/>
|
||||||
|
<define name="AUTO_GROUNDSPEED_PGAIN" value="1."/>
|
||||||
|
<define name="AUTO_GROUNDSPEED_IGAIN" value="0."/>
|
||||||
|
|
||||||
|
<!-- pitch trim -->
|
||||||
|
<define name="PITCH_LOITER_TRIM" value="0." unit="deg"/>
|
||||||
|
<define name="PITCH_DASH_TRIM" value="0." unit="deg"/>
|
||||||
|
|
||||||
|
<define name="THROTTLE_SLEW" value="0.1"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||||
|
<define name="COURSE_PGAIN" value="0.743"/>
|
||||||
|
<define name="ROLL_MAX_SETPOINT" value="0.8" unit="rad"/>
|
||||||
|
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
|
||||||
|
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
|
||||||
|
|
||||||
|
<define name="ROLL_ATTITUDE_GAIN" value="11000."/>
|
||||||
|
<define name="ROLL_RATE_GAIN" value="1000."/>
|
||||||
|
<define name="ROLL_IGAIN" value="100."/>
|
||||||
|
<define name="ROLL_KFFA" value="0"/>
|
||||||
|
<define name="ROLL_KFFD" value="0"/>
|
||||||
|
|
||||||
|
<define name="PITCH_PGAIN" value="17250"/>
|
||||||
|
<define name="PITCH_DGAIN" value="500."/>
|
||||||
|
<define name="PITCH_IGAIN" value="400"/>
|
||||||
|
<define name="PITCH_KFFA" value="0."/>
|
||||||
|
<define name="PITCH_KFFD" value="0."/>
|
||||||
|
|
||||||
|
<define name="PITCH_OF_ROLL" value="1." unit="deg"/>
|
||||||
|
<define name="AILERON_OF_THROTTLE" value="0.0"/>
|
||||||
|
<!--define name="ELEVATOR_OF_ROLL" value="1400"/-->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="NAV">
|
||||||
|
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||||
|
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
|
||||||
|
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
|
||||||
|
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
|
||||||
|
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
|
||||||
|
<define name="HOME_RADIUS" value="100" unit="m"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="SIMU">
|
||||||
|
<!--define name="ROLL_RESPONSE_FACTOR" value="10"/>
|
||||||
|
<define name="MAX_ROLL_DOT" value="20" unit="rad/s"/-->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="SIMULATOR" prefix="NPS_">
|
||||||
|
<!--define name="JSBSIM_MODEL" value=""Malolo1""/-->
|
||||||
|
<define name="JSBSIM_MODEL" value=""LisaAspirin2""/>
|
||||||
|
<define name="ACTUATOR_NAMES" value="{"throttle-cmd-norm", "aileron-cmd-norm", "elevator-cmd-norm", "rudder-cmd-norm"}"/>
|
||||||
|
<define name="SENSORS_PARAMS" value=""nps_sensors_params_invariant.h""/>
|
||||||
|
<define name="JS_AXIS_MODE" value="4"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,57 @@
|
|||||||
|
# Hey Emacs, this is a -*- makefile -*-
|
||||||
|
|
||||||
|
# attitude and speed estimation for fixedwings via invariant filter
|
||||||
|
|
||||||
|
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant.h\"
|
||||||
|
INS_CFLAGS += -DUSE_AHRS_ALIGNER
|
||||||
|
INS_CFLAGS += -DUSE_AHRS
|
||||||
|
INS_CFLAGS += -DINS_UPDATE_FW_ESTIMATOR
|
||||||
|
|
||||||
|
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
|
||||||
|
INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
|
||||||
|
INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c
|
||||||
|
INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c
|
||||||
|
|
||||||
|
|
||||||
|
ifneq ($(AHRS_ALIGNER_LED),none)
|
||||||
|
INS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
|
||||||
|
endif
|
||||||
|
|
||||||
|
ifdef AHRS_PROPAGATE_FREQUENCY
|
||||||
|
else
|
||||||
|
AHRS_PROPAGATE_FREQUENCY = 125
|
||||||
|
endif
|
||||||
|
|
||||||
|
ifdef AHRS_CORRECT_FREQUENCY
|
||||||
|
else
|
||||||
|
AHRS_CORRECT_FREQUENCY = 125
|
||||||
|
endif
|
||||||
|
|
||||||
|
INS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
|
||||||
|
INS_CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
|
||||||
|
|
||||||
|
ap.CFLAGS += $(INS_CFLAGS)
|
||||||
|
ap.srcs += $(INS_SRCS)
|
||||||
|
|
||||||
|
#
|
||||||
|
# NPS uses the real algorithm
|
||||||
|
#
|
||||||
|
nps.CFLAGS += $(INS_CFLAGS)
|
||||||
|
nps.srcs += $(INS_SRCS)
|
||||||
|
|
||||||
|
|
||||||
|
#
|
||||||
|
# Simple simulation of the AHRS result
|
||||||
|
#
|
||||||
|
ahrssim_CFLAGS = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
|
||||||
|
ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
|
||||||
|
|
||||||
|
ahrssim_srcs = $(SRC_SUBSYSTEMS)/ahrs.c
|
||||||
|
ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
|
||||||
|
|
||||||
|
sim.CFLAGS += $(ahrssim_CFLAGS)
|
||||||
|
sim.srcs += $(ahrssim_srcs)
|
||||||
|
|
||||||
|
jsbsim.CFLAGS += $(ahrssim_CFLAGS)
|
||||||
|
jsbsim.srcs += $(ahrssim_srcs)
|
||||||
|
|
||||||
@@ -26,3 +26,7 @@ ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
|
|||||||
ap.CFLAGS += $(IMU_APOGEE_CFLAGS)
|
ap.CFLAGS += $(IMU_APOGEE_CFLAGS)
|
||||||
ap.srcs += $(IMU_APOGEE_SRCS)
|
ap.srcs += $(IMU_APOGEE_SRCS)
|
||||||
|
|
||||||
|
#
|
||||||
|
# Simulator
|
||||||
|
#
|
||||||
|
include $(CFG_SHARED)/imu_nps.makefile
|
||||||
|
|||||||
+20
-1
@@ -653,7 +653,26 @@
|
|||||||
<field name="accel" type="float" unit="m/s/s"/>
|
<field name="accel" type="float" unit="m/s/s"/>
|
||||||
</message>
|
</message>
|
||||||
|
|
||||||
<!-- 78 is free -->
|
<!--message for Invariant filter with position-->
|
||||||
|
<message name="INV_FILTER" id="78">
|
||||||
|
<field name="quat" type="float"/>
|
||||||
|
<field name="phi_inv" type="float" unit="rad" alt_unit="deg"/>
|
||||||
|
<field name="theta_inv" type="float" unit="rad" alt_unit="deg"/>
|
||||||
|
<field name="psi_inv" type="float" unit="rad" alt_unit="deg"/>
|
||||||
|
<field name="Vx_inv" type="float" unit="m/s"/>
|
||||||
|
<field name="Vy_inv" type="float" unit="m/s"/>
|
||||||
|
<field name="Vz_inv" type="float" unit="m/s"/>
|
||||||
|
<field name="Px_inv" type="float" unit="m"/>
|
||||||
|
<field name="Py_inv" type="float" unit="m"/>
|
||||||
|
<field name="Pz_inv" type="float" unit="m"/>
|
||||||
|
<field name="bias_phi" type="float" unit="rad/s" alt_unit="deg/s"/>
|
||||||
|
<field name="bias_theta" type="float" unit="rad/s" alt_unit="deg/s"/>
|
||||||
|
<field name="bias_psi" type="float" unit="rad/s" alt_unit="deg/s"/>
|
||||||
|
<field name="bias_as" type="float"/>
|
||||||
|
<field name="bias_hb" type="float"/>
|
||||||
|
<field name="meas_baro" type="float" unit="m"/>
|
||||||
|
<field name="meas_gps" type="float" unit="m"/>
|
||||||
|
</message>
|
||||||
|
|
||||||
<message name="MISSION_STATUS" id="79">
|
<message name="MISSION_STATUS" id="79">
|
||||||
<field name="remaining_time" type="float"/>
|
<field name="remaining_time" type="float"/>
|
||||||
|
|||||||
@@ -16,7 +16,7 @@
|
|||||||
<periodic fun="mag_hmc58xx_module_periodic()" freq="60"/>
|
<periodic fun="mag_hmc58xx_module_periodic()" freq="60"/>
|
||||||
<periodic fun="mag_hmc58xx_report()" freq="10" autorun="TRUE"/>
|
<periodic fun="mag_hmc58xx_report()" freq="10" autorun="TRUE"/>
|
||||||
<event fun="mag_hmc58xx_module_event()"/>
|
<event fun="mag_hmc58xx_module_event()"/>
|
||||||
<makefile>
|
<makefile target="ap">
|
||||||
<file name="mag_hmc58xx.c"/>
|
<file name="mag_hmc58xx.c"/>
|
||||||
<file name="hmc58xx.c" dir="peripherals"/>
|
<file name="hmc58xx.c" dir="peripherals"/>
|
||||||
<define name="USE_I2C"/>
|
<define name="USE_I2C"/>
|
||||||
|
|||||||
@@ -0,0 +1,21 @@
|
|||||||
|
<!DOCTYPE settings SYSTEM "../settings.dtd">
|
||||||
|
|
||||||
|
<settings>
|
||||||
|
<dl_settings>
|
||||||
|
<dl_settings NAME="invariant">
|
||||||
|
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.lv" shortname="lv" module="subsystems/ins/ins_float_invariant"/>
|
||||||
|
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.lb" shortname="lb"/>
|
||||||
|
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.mv" shortname="mv"/>
|
||||||
|
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.mh" shortname="mh"/>
|
||||||
|
<dl_setting MAX="20" MIN="0." STEP="0.01" VAR="ins_impl.gains.nx" shortname="nx"/>
|
||||||
|
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.nxz" shortname="nxz"/>
|
||||||
|
<dl_setting MAX="30" MIN="0." STEP="0.01" VAR="ins_impl.gains.mvz" shortname="mvz"/>
|
||||||
|
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_impl.gains.nh" shortname="nh"/>
|
||||||
|
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_impl.gains.ov" shortname="ov"/>
|
||||||
|
<dl_setting MAX="3" MIN="0." STEP="0.01" VAR="ins_impl.gains.ob" shortname="ob"/>
|
||||||
|
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.rv" shortname="rv"/>
|
||||||
|
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_impl.gains.rh" shortname="rh"/>
|
||||||
|
<dl_setting MAX="1" MIN="0." STEP="0.001" VAR="ins_impl.gains.sh" shortname="sh"/>
|
||||||
|
</dl_settings>
|
||||||
|
</dl_settings>
|
||||||
|
</settings>
|
||||||
@@ -0,0 +1,177 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013 Felix Ruess, Gautier Hattenberger, JP Condomines
|
||||||
|
*
|
||||||
|
* 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 "subsystems/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
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Accelerometer
|
||||||
|
*/
|
||||||
|
/* MPU6050 has 16bit resolution */
|
||||||
|
#define NPS_ACCEL_MIN -32767
|
||||||
|
#define NPS_ACCEL_MAX 32767
|
||||||
|
/* ms-2 */
|
||||||
|
/* aka 2^10/ACCEL_X_SENS */
|
||||||
|
#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS)
|
||||||
|
#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS)
|
||||||
|
#define NPS_ACCEL_SENSITIVITY_ZZ 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.e-2
|
||||||
|
#define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2
|
||||||
|
#define NPS_ACCEL_NOISE_STD_DEV_Z 5.e-2
|
||||||
|
/* ms-2 */
|
||||||
|
#define NPS_ACCEL_BIAS_X 0
|
||||||
|
#define NPS_ACCEL_BIAS_Y 0
|
||||||
|
#define NPS_ACCEL_BIAS_Z 0
|
||||||
|
/* s */
|
||||||
|
#define NPS_ACCEL_DT (1./125.)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Gyrometer
|
||||||
|
*/
|
||||||
|
/* MPU6050 has 16 bit resolution */
|
||||||
|
#define NPS_GYRO_MIN -32767
|
||||||
|
#define NPS_GYRO_MAX 32767
|
||||||
|
|
||||||
|
/* 2^12/GYRO_X_SENS */
|
||||||
|
#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS)
|
||||||
|
#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS)
|
||||||
|
#define NPS_GYRO_SENSITIVITY_RR 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(0.)
|
||||||
|
#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
|
||||||
|
#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
|
||||||
|
|
||||||
|
#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 */
|
||||||
|
#define NPS_GYRO_DT (1./125.)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Magnetometer
|
||||||
|
*/
|
||||||
|
/* HMC5883 has 12 bit resolution */
|
||||||
|
#define NPS_MAG_MIN -2047
|
||||||
|
#define NPS_MAG_MAX 2047
|
||||||
|
|
||||||
|
#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 MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS)
|
||||||
|
#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS)
|
||||||
|
#define NPS_MAG_SENSITIVITY_ZZ 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-3
|
||||||
|
#define NPS_MAG_NOISE_STD_DEV_Y 2e-3
|
||||||
|
#define NPS_MAG_NOISE_STD_DEV_Z 2e-3
|
||||||
|
|
||||||
|
#define NPS_MAG_DT (1./60.)
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Range meter
|
||||||
|
*/
|
||||||
|
#define BSM_RANGEMETER_RESOLUTION (1024)
|
||||||
|
#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.)
|
||||||
|
#define BSM_RANGEMETER_MAX_RANGE (6. * BSM_RANGEMETER_SENSITIVITY)
|
||||||
|
#define BSM_RANGEMETER_DT (1./20.)
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Barometer
|
||||||
|
*/
|
||||||
|
/* m */
|
||||||
|
/* aka 2^8/INS_BARO_SENS */
|
||||||
|
#define NPS_BARO_QNH 1013.25
|
||||||
|
#define NPS_BARO_SENSITIVITY 4.0
|
||||||
|
#define NPS_BARO_DT (1./5.)
|
||||||
|
#define NPS_BARO_NOISE_STD_DEV 5.e-2
|
||||||
|
|
||||||
|
/*
|
||||||
|
* GPS
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef GPS_PERFECT
|
||||||
|
#define GPS_PERFECT 1
|
||||||
|
#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 */
|
||||||
|
|
||||||
|
#define NPS_GPS_DT (1./5.)
|
||||||
|
|
||||||
|
#endif /* NPS_SENSORS_PARAMS_H */
|
||||||
|
|
||||||
@@ -29,6 +29,13 @@
|
|||||||
|
|
||||||
#define SQUARE(_a) ((_a)*(_a))
|
#define SQUARE(_a) ((_a)*(_a))
|
||||||
|
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// Vector algebra
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Dimension 2 vectors
|
* Dimension 2 vectors
|
||||||
*/
|
*/
|
||||||
@@ -87,7 +94,10 @@
|
|||||||
(_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
|
(_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* _vo=v1*v2 */
|
||||||
|
#define VECT2_DOT_PRODUCT(_so, _v1, _v2) { \
|
||||||
|
(_so) = (_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z; \
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Dimension 3 vectors
|
* Dimension 3 vectors
|
||||||
@@ -100,6 +110,14 @@
|
|||||||
(_a).z = (_z); \
|
(_a).z = (_z); \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* a = a * b */
|
||||||
|
|
||||||
|
#define VECT3_MUL( _v1, _v2){ \
|
||||||
|
(_v1).x = (_v1).x * (_v2).x; \
|
||||||
|
(_v1).y = (_v1).y * (_v2).y; \
|
||||||
|
(_v1).z = (_v1).z * (_v2).z; \
|
||||||
|
}
|
||||||
|
|
||||||
/* a = {abs(x), abs(y), abs(z)} */
|
/* a = {abs(x), abs(y), abs(z)} */
|
||||||
#define VECT3_ASSIGN_ABS(_a, _x, _y, _z) { \
|
#define VECT3_ASSIGN_ABS(_a, _x, _y, _z) { \
|
||||||
(_a).x = ABS(_x); \
|
(_a).x = ABS(_x); \
|
||||||
@@ -218,6 +236,10 @@
|
|||||||
(_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
|
(_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#define VECT3_DOT_PRODUCT(_so, _v1, _v2) { \
|
||||||
|
(_so) = (_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z; \
|
||||||
|
}
|
||||||
|
|
||||||
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) { \
|
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) { \
|
||||||
(_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y; \
|
(_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y; \
|
||||||
(_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z; \
|
(_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z; \
|
||||||
@@ -225,12 +247,13 @@
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// Euler angles
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Euler angles
|
|
||||||
*/
|
|
||||||
|
|
||||||
#define EULERS_COPY(_a, _b) { \
|
#define EULERS_COPY(_a, _b) { \
|
||||||
(_a).phi = (_b).phi; \
|
(_a).phi = (_b).phi; \
|
||||||
(_a).theta = (_b).theta; \
|
(_a).theta = (_b).theta; \
|
||||||
@@ -286,9 +309,11 @@
|
|||||||
(_v).psi = (_v).psi < (_min) ? (_min) : (_v).psi > (_max) ? (_max) : (_v).psi; \
|
(_v).psi = (_v).psi < (_min) ? (_min) : (_v).psi > (_max) ? (_max) : (_v).psi; \
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
//
|
||||||
* Rates
|
//
|
||||||
*/
|
// Rates
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
/* ra = {p, q, r} */
|
/* ra = {p, q, r} */
|
||||||
#define RATES_ASSIGN(_ra, _p, _q, _r) { \
|
#define RATES_ASSIGN(_ra, _p, _q, _r) { \
|
||||||
@@ -376,6 +401,13 @@
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// Matrix
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* 3x3 matrices
|
* 3x3 matrices
|
||||||
*/
|
*/
|
||||||
@@ -456,9 +488,11 @@
|
|||||||
|
|
||||||
//
|
//
|
||||||
//
|
//
|
||||||
// Quaternions
|
// Quaternion algebras
|
||||||
//
|
//
|
||||||
//
|
//
|
||||||
|
|
||||||
|
/* _q = [_i _x _y _z] */
|
||||||
#define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
|
#define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
|
||||||
(_q).qi = (_i); \
|
(_q).qi = (_i); \
|
||||||
(_q).qx = (_x); \
|
(_q).qx = (_x); \
|
||||||
@@ -466,6 +500,7 @@
|
|||||||
(_q).qz = (_z); \
|
(_q).qz = (_z); \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* _qc = _qa - _qc */
|
||||||
#define QUAT_DIFF(_qc, _qa, _qb) { \
|
#define QUAT_DIFF(_qc, _qa, _qb) { \
|
||||||
(_qc).qi = (_qa).qi - (_qb).qi; \
|
(_qc).qi = (_qa).qi - (_qb).qi; \
|
||||||
(_qc).qx = (_qa).qx - (_qb).qx; \
|
(_qc).qx = (_qa).qx - (_qb).qx; \
|
||||||
@@ -473,6 +508,7 @@
|
|||||||
(_qc).qz = (_qa).qz - (_qb).qz; \
|
(_qc).qz = (_qa).qz - (_qb).qz; \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* _qo = _qi */
|
||||||
#define QUAT_COPY(_qo, _qi) { \
|
#define QUAT_COPY(_qo, _qi) { \
|
||||||
(_qo).qi = (_qi).qi; \
|
(_qo).qi = (_qi).qi; \
|
||||||
(_qo).qx = (_qi).qx; \
|
(_qo).qx = (_qi).qx; \
|
||||||
@@ -487,7 +523,7 @@
|
|||||||
(b).qz = -(a).qz; \
|
(b).qz = -(a).qz; \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* _qo = _qi * _s */
|
||||||
#define QUAT_SMUL(_qo, _qi, _s) { \
|
#define QUAT_SMUL(_qo, _qi, _s) { \
|
||||||
(_qo).qi = (_qi).qi * (_s); \
|
(_qo).qi = (_qi).qi * (_s); \
|
||||||
(_qo).qx = (_qi).qx * (_s); \
|
(_qo).qx = (_qi).qx * (_s); \
|
||||||
@@ -495,6 +531,7 @@
|
|||||||
(_qo).qz = (_qi).qz * (_s); \
|
(_qo).qz = (_qi).qz * (_s); \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* _qo = _qo + _qi */
|
||||||
#define QUAT_ADD(_qo, _qi) { \
|
#define QUAT_ADD(_qo, _qi) { \
|
||||||
(_qo).qi += (_qi).qi; \
|
(_qo).qi += (_qi).qi; \
|
||||||
(_qo).qx += (_qi).qx; \
|
(_qo).qx += (_qi).qx; \
|
||||||
@@ -502,6 +539,7 @@
|
|||||||
(_qo).qz += (_qi).qz; \
|
(_qo).qz += (_qi).qz; \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* _qo = [qi -qx -qy -qz] */
|
||||||
#define QUAT_INVERT(_qo, _qi) { \
|
#define QUAT_INVERT(_qo, _qi) { \
|
||||||
(_qo).qi = (_qi).qi; \
|
(_qo).qi = (_qi).qi; \
|
||||||
(_qo).qx = -(_qi).qx; \
|
(_qo).qx = -(_qi).qx; \
|
||||||
@@ -509,10 +547,27 @@
|
|||||||
(_qo).qz = -(_qi).qz; \
|
(_qo).qz = -(_qi).qz; \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* _vo=[ qx qy qz] */
|
||||||
|
#define QUAT_EXTRACT_Q(_vo, _qi) { \
|
||||||
|
(_vo).x=(_qi).qx; \
|
||||||
|
(_vo).y=(_qi).qy; \
|
||||||
|
(_vo).z=(_qi).qz; \
|
||||||
|
}
|
||||||
|
|
||||||
|
/* _qo = _qo / _s */
|
||||||
|
#define QUAT_SDIV(_qo, _qi, _s) { \
|
||||||
|
(_qo).qi = (_qi).qi / _s; \
|
||||||
|
(_qo).qx = (_qi).qx / _s; \
|
||||||
|
(_qo).qy = (_qi).qy / _s; \
|
||||||
|
(_qo).qz = (_qi).qz / _s; \
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// Rotation Matrices
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
/*
|
|
||||||
* Rotation Matrices
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* accessor : row and col range from 0 to 2 */
|
/* accessor : row and col range from 0 to 2 */
|
||||||
#define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
|
#define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
|
||||||
|
|||||||
@@ -95,6 +95,12 @@ struct FloatRates {
|
|||||||
while (_a < -M_PI) _a += (2.*M_PI); \
|
while (_a < -M_PI) _a += (2.*M_PI); \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// Vector algebra
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Dimension 2 Vectors
|
* Dimension 2 Vectors
|
||||||
@@ -137,6 +143,10 @@ struct FloatRates {
|
|||||||
* Dimension 3 Vectors
|
* Dimension 3 Vectors
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#define FLOAT_VECT3_SUM(_a, _b, _c) VECT3_SUM(_a, _b, _c)
|
||||||
|
#define FLOAT_VECT3_SDIV(_a, _b, _s) VECT3_SDIV(_a, _b, _s)
|
||||||
|
#define FLOAT_VECT3_COPY(_a, _b) VECT3_COPY(_a, _b)
|
||||||
|
|
||||||
#define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
|
#define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
|
||||||
|
|
||||||
#define FLOAT_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
|
#define FLOAT_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
|
||||||
@@ -147,9 +157,13 @@ struct FloatRates {
|
|||||||
/* a -= b */
|
/* a -= b */
|
||||||
#define FLOAT_VECT3_SUB(_a, _b) VECT3_SUB(_a, _b)
|
#define FLOAT_VECT3_SUB(_a, _b) VECT3_SUB(_a, _b)
|
||||||
|
|
||||||
|
#define FLOAT_VECT3_ADD(_a, _b) VECT3_ADD(_a, _b)
|
||||||
|
|
||||||
/* _vo = _vi * _s */
|
/* _vo = _vi * _s */
|
||||||
#define FLOAT_VECT3_SMUL(_vo, _vi, _s) VECT3_SMUL(_vo, _vi, _s)
|
#define FLOAT_VECT3_SMUL(_vo, _vi, _s) VECT3_SMUL(_vo, _vi, _s)
|
||||||
|
|
||||||
|
#define FLOAT_VECT3_MUL(_v1, _v2) VECT3_MUL(_v1, _v2)
|
||||||
|
|
||||||
#define FLOAT_VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
|
#define FLOAT_VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
|
||||||
|
|
||||||
#define FLOAT_VECT3_NORM(_v) (sqrtf(FLOAT_VECT3_NORM2(_v)))
|
#define FLOAT_VECT3_NORM(_v) (sqrtf(FLOAT_VECT3_NORM2(_v)))
|
||||||
@@ -243,9 +257,12 @@ struct FloatRates {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
//
|
||||||
* Rotation Matrices
|
//
|
||||||
*/
|
// Rotation Matrices
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
|
|
||||||
/* initialises a matrix to identity */
|
/* initialises a matrix to identity */
|
||||||
#define FLOAT_RMAT_ZERO(_rm) FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)
|
#define FLOAT_RMAT_ZERO(_rm) FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)
|
||||||
@@ -456,36 +473,48 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//
|
||||||
/*
|
//
|
||||||
* Quaternions
|
// Quaternion algebras
|
||||||
*/
|
//
|
||||||
|
//
|
||||||
|
|
||||||
#define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.)
|
#define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.)
|
||||||
|
|
||||||
|
#define FLOAT_QUAT_ASSIGN(_qi, _i, _x, _y, _z) QUAT_ASSIGN(_qi, _i, _x, _y, _z)
|
||||||
|
|
||||||
/* _q += _qa */
|
/* _q += _qa */
|
||||||
#define FLOAT_QUAT_ADD(_qo, _qi) QUAT_ADD(_qo, _qi)
|
#define FLOAT_QUAT_ADD(_qo, _qi) QUAT_ADD(_qo, _qi)
|
||||||
|
|
||||||
/* _qo = _qi * _s */
|
/* _qo = _qi * _s */
|
||||||
#define FLOAT_QUAT_SMUL(_qo, _qi, _s) QUAT_SMUL(_qo, _qi, _s)
|
#define FLOAT_QUAT_SMUL(_qo, _qi, _s) QUAT_SMUL(_qo, _qi, _s)
|
||||||
|
|
||||||
|
/* _qo = _qo / _s */
|
||||||
|
#define FLOAT_QUAT_SDIV( _qo, _qi, _s) QUAT_SDIV(_qo, _qi, _s)
|
||||||
|
|
||||||
|
/* */
|
||||||
#define FLOAT_QUAT_EXPLEMENTARY(b,a) QUAT_EXPLEMENTARY(b,a)
|
#define FLOAT_QUAT_EXPLEMENTARY(b,a) QUAT_EXPLEMENTARY(b,a)
|
||||||
|
|
||||||
|
/* */
|
||||||
#define FLOAT_QUAT_COPY(_qo, _qi) QUAT_COPY(_qo, _qi)
|
#define FLOAT_QUAT_COPY(_qo, _qi) QUAT_COPY(_qo, _qi)
|
||||||
|
|
||||||
#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \
|
#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \
|
||||||
SQUARE((_q).qy) + SQUARE((_q).qz)))
|
SQUARE((_q).qy) + SQUARE((_q).qz)))
|
||||||
|
|
||||||
#define FLOAT_QUAT_NORMALIZE(_q) { \
|
#define FLOAT_QUAT_NORMALIZE(_q) { \
|
||||||
float norm = FLOAT_QUAT_NORM(_q); \
|
float qnorm = FLOAT_QUAT_NORM(_q); \
|
||||||
if (norm > FLT_MIN) { \
|
if (qnorm > FLT_MIN) { \
|
||||||
(_q).qi = (_q).qi / norm; \
|
(_q).qi = (_q).qi / qnorm; \
|
||||||
(_q).qx = (_q).qx / norm; \
|
(_q).qx = (_q).qx / qnorm; \
|
||||||
(_q).qy = (_q).qy / norm; \
|
(_q).qy = (_q).qy / qnorm; \
|
||||||
(_q).qz = (_q).qz / norm; \
|
(_q).qz = (_q).qz / qnorm; \
|
||||||
} \
|
} \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* */
|
||||||
|
#define FLOAT_QUAT_EXTRACT(_vo, _qi) QUAT_EXTRACT_Q(_vo, _qi)
|
||||||
|
|
||||||
|
/* Be careful : after invert make a normalization */
|
||||||
#define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
|
#define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
|
||||||
|
|
||||||
#define FLOAT_QUAT_WRAP_SHORTEST(_q) { \
|
#define FLOAT_QUAT_WRAP_SHORTEST(_q) { \
|
||||||
@@ -493,6 +522,101 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
|
|||||||
QUAT_EXPLEMENTARY(_q,_q); \
|
QUAT_EXPLEMENTARY(_q,_q); \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Rotation Matrix using quaternions
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* The (non commutative) quaternion product * then reads
|
||||||
|
*
|
||||||
|
* [ p0.q0 - p.q ]
|
||||||
|
* p * q = [ ]
|
||||||
|
* [ p0.q + q0.p + p x q ]
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* (qi)-1 * vi * qi represents R_q of n->b on vectors vi
|
||||||
|
*
|
||||||
|
* "FLOAT_QUAT_EXTRACT : Extracted of the vector part"
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define FLOAT_QUAT_RMAT_N2B(_n2b, _qi, _vi){ \
|
||||||
|
\
|
||||||
|
struct FloatQuat quatinv; \
|
||||||
|
struct FloatVect3 quat3, v1, v2; \
|
||||||
|
float qi; \
|
||||||
|
\
|
||||||
|
FLOAT_QUAT_INVERT(quatinv, _qi); \
|
||||||
|
FLOAT_QUAT_NORMALIZE(quatinv); \
|
||||||
|
\
|
||||||
|
FLOAT_QUAT_EXTRACT(quat3, quatinv); \
|
||||||
|
qi = - FLOAT_VECT3_DOT_PRODUCT(quat3, _vi); \
|
||||||
|
FLOAT_VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
|
||||||
|
FLOAT_VECT3_SMUL(v2, _vi, (quatinv.qi)) ; \
|
||||||
|
FLOAT_VECT3_ADD(v2, v1); \
|
||||||
|
\
|
||||||
|
FLOAT_QUAT_EXTRACT(quat3, _qi); \
|
||||||
|
FLOAT_VECT3_CROSS_PRODUCT(_n2b, v2, quat3); \
|
||||||
|
FLOAT_VECT3_SMUL(v1, v2, (_qi).qi); \
|
||||||
|
FLOAT_VECT3_ADD(_n2b,v1); \
|
||||||
|
FLOAT_VECT3_SMUL(v1, quat3, qi); \
|
||||||
|
FLOAT_VECT3_ADD(_n2b,v1); \
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* qi * vi * (qi)-1 represents R_q of b->n on vectors vi
|
||||||
|
*/
|
||||||
|
#define FLOAT_QUAT_RMAT_B2N(_b2n,_qi,_vi){ \
|
||||||
|
\
|
||||||
|
struct FloatQuat _quatinv; \
|
||||||
|
\
|
||||||
|
\
|
||||||
|
FLOAT_QUAT_INVERT(_quatinv, _qi); \
|
||||||
|
FLOAT_QUAT_NORMALIZE(_quatinv); \
|
||||||
|
\
|
||||||
|
FLOAT_QUAT_RMAT_N2B(_b2n, _quatinv, _vi); \
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Right multiplication by a quaternion
|
||||||
|
*
|
||||||
|
* vi * qi
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
#define FLOAT_QUAT_VMUL_RIGHT(_mright,_qi,_vi){ \
|
||||||
|
\
|
||||||
|
struct FloatVect3 quat3, v1, v2; \
|
||||||
|
float qi; \
|
||||||
|
\
|
||||||
|
FLOAT_QUAT_EXTRACT(quat3, _qi); \
|
||||||
|
qi = - FLOAT_VECT3_DOT_PRODUCT(_vi, quat3); \
|
||||||
|
FLOAT_VECT3_CROSS_PRODUCT(v1, _vi, quat3); \
|
||||||
|
FLOAT_VECT3_SMUL(v2, _vi, (_qi.qi)); \
|
||||||
|
FLOAT_VECT3_ADD(v2, v1); \
|
||||||
|
FLOAT_QUAT_ASSIGN(_mright, qi, v2.x, v2.y, v2.z);\
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/* Left multiplication by a quaternion
|
||||||
|
*
|
||||||
|
* qi * vi
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
#define FLOAT_QUAT_VMUL_LEFT(_mleft,_qi,_vi){ \
|
||||||
|
\
|
||||||
|
struct FloatVect3 quat3, v1, v2; \
|
||||||
|
float qi; \
|
||||||
|
\
|
||||||
|
FLOAT_QUAT_EXTRACT(quat3, _qi); \
|
||||||
|
qi = - FLOAT_VECT3_DOT_PRODUCT(quat3, _vi); \
|
||||||
|
FLOAT_VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
|
||||||
|
FLOAT_VECT3_SMUL(v2, _vi, (_qi.qi)); \
|
||||||
|
FLOAT_VECT3_ADD(v2, v1); \
|
||||||
|
FLOAT_QUAT_ASSIGN(_mleft, qi, v2.x, v2.y, v2.z);\
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
|
||||||
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
|
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
|
||||||
FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
|
FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
|
||||||
@@ -710,9 +834,12 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Euler angles
|
//
|
||||||
*/
|
//
|
||||||
|
// Euler angles
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
#define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
|
#define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
|
||||||
|
|
||||||
@@ -755,6 +882,67 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// Generic vector algebra
|
||||||
|
//
|
||||||
|
//
|
||||||
|
|
||||||
|
/** a = 0 */
|
||||||
|
static inline void float_vect_zero(float * a, const int n) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < n; i++) { a[i] = 0.; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/** a = b */
|
||||||
|
static inline void float_vect_copy(float * a, const float * b, const int n) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < n; i++) { a[i] = b[i]; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/** o = a + b */
|
||||||
|
static inline void float_vect_sum(float * o, const float * a, const float * b, const int n) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/** o = a - b */
|
||||||
|
static inline void float_vect_diff(float * o, const float * a, const float * b, const int n) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/** o = a * b (element wise) */
|
||||||
|
static inline void float_vect_mul(float * o, const float * a, const float * b, const int n) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < n; i++) { o[i] = a[i] * b[i]; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/** a += b */
|
||||||
|
static inline void float_vect_add(float * a, const float * b, const int n) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < n; i++) { a[i] += b[i]; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/** a -= b */
|
||||||
|
static inline void float_vect_sub(float * a, const float * b, const int n) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < n; i++) { a[i] -= b[i]; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/** o = a * s */
|
||||||
|
static inline void float_vect_smul(float * o, const float * a, const float s, const int n) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < n; i++) { o[i] = a[i] * s; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/** o = a / s */
|
||||||
|
static inline void float_vect_sdiv(float * o, const float * a, const float s, const int n) {
|
||||||
|
int i;
|
||||||
|
if ( fabs(s) > 1e-5 ) {
|
||||||
|
for (i = 0; i < n; i++) { o[i] = a[i] / s; }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -47,5 +47,17 @@
|
|||||||
(_pos).z = -(_utm1).alt + (_utm2).alt; \
|
(_pos).z = -(_utm1).alt + (_utm2).alt; \
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0) { \
|
||||||
|
(_utm).east = (_utm0).east + (_pos).x; \
|
||||||
|
(_utm).north = (_utm0).north + (_pos).y; \
|
||||||
|
(_utm).alt = (_utm0).alt + (_pos).z; \
|
||||||
|
}
|
||||||
|
|
||||||
|
#define UTM_OF_NED_ADD(_utm, _pos, _utm0) { \
|
||||||
|
(_utm).east = (_utm0).east + (_pos).y; \
|
||||||
|
(_utm).north = (_utm0).north + (_pos).x; \
|
||||||
|
(_utm).alt = (_utm0).alt - (_pos).z; \
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
#endif /* PPRZ_GEODETIC_H */
|
#endif /* PPRZ_GEODETIC_H */
|
||||||
|
|||||||
@@ -0,0 +1,165 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013 Gautier Hattenberger
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Runge-Kutta library (float version)
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef PPRZ_RK_FLOAT_H
|
||||||
|
#define PPRZ_RK_FLOAT_H
|
||||||
|
|
||||||
|
#include "math/pprz_algebra_float.h"
|
||||||
|
|
||||||
|
/** First-Order Runge-Kutta
|
||||||
|
*
|
||||||
|
* aka RK1, aka the euler method
|
||||||
|
*
|
||||||
|
* considering x' = f(x,u)
|
||||||
|
* with x = x0 the initial state
|
||||||
|
* and u the command vector
|
||||||
|
*
|
||||||
|
* x_new = x + dt * f(x, u)
|
||||||
|
* is the integrated state vector x based on model f under command u
|
||||||
|
*
|
||||||
|
* @param xo output integrated state
|
||||||
|
* @param x initial state
|
||||||
|
* @param n state dimension
|
||||||
|
* @param u command vector
|
||||||
|
* @param m command dimension
|
||||||
|
* @param f model function
|
||||||
|
* @param dt integration step
|
||||||
|
*/
|
||||||
|
static inline void runge_kutta_1_float(
|
||||||
|
float * xo,
|
||||||
|
const float * x, const int n,
|
||||||
|
const float * u, const int m,
|
||||||
|
void (*f)(float * o, const float * x, const int n, const float * u, const int m),
|
||||||
|
const float dt)
|
||||||
|
{
|
||||||
|
float dx[n];
|
||||||
|
|
||||||
|
f(dx, x, n, u, m);
|
||||||
|
float_vect_smul(xo, dx, dt, n);
|
||||||
|
float_vect_add(xo, x, n);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Second-Order Runge-Kutta
|
||||||
|
*
|
||||||
|
* aka RK2, aka the mid-point method
|
||||||
|
*
|
||||||
|
* considering x' = f(x,u)
|
||||||
|
* with x = x0 the initial state
|
||||||
|
* and u the command vector
|
||||||
|
*
|
||||||
|
* mid_point = x + (dt/2)*f(x, u)
|
||||||
|
* x_new = x + dt * f(mid_point, u)
|
||||||
|
* is the integrated state vector x based on model f under command u
|
||||||
|
*
|
||||||
|
* @param xo output integrated state
|
||||||
|
* @param x initial state
|
||||||
|
* @param n state dimension
|
||||||
|
* @param u command vector
|
||||||
|
* @param m command dimension
|
||||||
|
* @param f model function
|
||||||
|
* @param dt integration step
|
||||||
|
*/
|
||||||
|
static inline void runge_kutta_2_float(
|
||||||
|
float * xo,
|
||||||
|
const float * x, const int n,
|
||||||
|
const float * u, const int m,
|
||||||
|
void (*f)(float * o, const float * x, const int n, const float * u, const int m),
|
||||||
|
const float dt)
|
||||||
|
{
|
||||||
|
float mid_point[n];
|
||||||
|
|
||||||
|
// mid_point = x + (dt/2)*f(x, u)
|
||||||
|
f(mid_point, x, n, u, m);
|
||||||
|
float_vect_smul(mid_point, mid_point, dt/2., n);
|
||||||
|
float_vect_add(mid_point, x, n);
|
||||||
|
// xo = x + dt * f(mid_point, u)
|
||||||
|
f(xo, mid_point, n, u, m);
|
||||||
|
float_vect_smul(xo, xo, dt, n);
|
||||||
|
float_vect_add(xo, x, n);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Fourth-Order Runge-Kutta
|
||||||
|
*
|
||||||
|
* aka RK4, aka 'the' Runge-Kutta
|
||||||
|
*
|
||||||
|
* considering x' = f(x,u)
|
||||||
|
* with x = x0 the initial state
|
||||||
|
* and u the command vector
|
||||||
|
*
|
||||||
|
* k1 = f(x, u)
|
||||||
|
* k2 = f(x + dt * (k1 / 2), u)
|
||||||
|
* k3 = f(x + dt * (k2 / 2), u)
|
||||||
|
* k4 = f(x + dt * k3, u)
|
||||||
|
*
|
||||||
|
* x_new = x + (dt / 6) * (k1 + 2 * (k2 + k3) + k4)
|
||||||
|
* is the integrated state vector x based on model f under command u
|
||||||
|
*
|
||||||
|
* @param xo output integrated state
|
||||||
|
* @param x initial state
|
||||||
|
* @param n state dimension
|
||||||
|
* @param u command vector
|
||||||
|
* @param m command dimension
|
||||||
|
* @param f model function
|
||||||
|
* @param dt integration step
|
||||||
|
*/
|
||||||
|
static inline void runge_kutta_4_float(
|
||||||
|
float * xo,
|
||||||
|
const float * x, const int n,
|
||||||
|
const float * u, const int m,
|
||||||
|
void (*f)(float * o, const float * x, const int n, const float * u, const int m),
|
||||||
|
const float dt)
|
||||||
|
{
|
||||||
|
float k1[n], k2[n], k3[n], k4[n], ktmp[n];
|
||||||
|
|
||||||
|
// k1 = f(x, u)
|
||||||
|
f(k1, x, n, u, m);
|
||||||
|
|
||||||
|
// k2 = f(x + dt * (k1 / 2), u)
|
||||||
|
float_vect_smul(ktmp, k1, dt / 2., n);
|
||||||
|
float_vect_add(ktmp, x, n);
|
||||||
|
f(k2, ktmp, n, u, m);
|
||||||
|
|
||||||
|
// k3 = f(x + dt * (k2 / 2), u)
|
||||||
|
float_vect_smul(ktmp, k2, dt / 2., n);
|
||||||
|
float_vect_add(ktmp, x, n);
|
||||||
|
f(k3, ktmp, n, u, m);
|
||||||
|
|
||||||
|
// k4 = f(x + dt * k3, u)
|
||||||
|
float_vect_smul(ktmp, k3, dt, n);
|
||||||
|
float_vect_add(ktmp, x, n);
|
||||||
|
f(k4, ktmp, n, u, m);
|
||||||
|
|
||||||
|
// xo = x + (dt / 6) * (k1 + 2 * (k2 + k3) + k4)
|
||||||
|
float_vect_add(k2, k3, n);
|
||||||
|
float_vect_smul(k2, k2, 2., n);
|
||||||
|
float_vect_add(k1, k2, n);
|
||||||
|
float_vect_add(k1, k4, n);
|
||||||
|
float_vect_smul(k1, k1, dt / 6., n);
|
||||||
|
float_vect_sum(xo, x, k1, n);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
@@ -30,6 +30,25 @@
|
|||||||
#include "messages.h"
|
#include "messages.h"
|
||||||
#include "subsystems/datalink/downlink.h"
|
#include "subsystems/datalink/downlink.h"
|
||||||
|
|
||||||
|
#if MODULE_HMC58XX_UPDATE_AHRS
|
||||||
|
#include "subsystems/imu.h"
|
||||||
|
#include "subsystems/ahrs.h"
|
||||||
|
|
||||||
|
#ifndef HMC58XX_CHAN_X
|
||||||
|
#define HMC58XX_CHAN_X 0
|
||||||
|
#endif
|
||||||
|
#ifndef HMC58XX_CHAN_Y
|
||||||
|
#define HMC58XX_CHAN_Y 1
|
||||||
|
#endif
|
||||||
|
#ifndef HMC58XX_CHAN_Z
|
||||||
|
#define HMC58XX_CHAN_Z 2
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef DOWNLINK_DEVICE
|
||||||
|
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
||||||
|
#endif
|
||||||
|
|
||||||
struct Hmc58xx mag_hmc58xx;
|
struct Hmc58xx mag_hmc58xx;
|
||||||
|
|
||||||
@@ -43,6 +62,25 @@ void mag_hmc58xx_module_periodic(void) {
|
|||||||
|
|
||||||
void mag_hmc58xx_module_event(void) {
|
void mag_hmc58xx_module_event(void) {
|
||||||
hmc58xx_event(&mag_hmc58xx);
|
hmc58xx_event(&mag_hmc58xx);
|
||||||
|
#if MODULE_HMC58XX_UPDATE_AHRS
|
||||||
|
if (mag_hmc58xx.data_available) {
|
||||||
|
// set channel order
|
||||||
|
struct Int32Vect3 mag = {
|
||||||
|
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
|
||||||
|
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
|
||||||
|
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
|
||||||
|
};
|
||||||
|
// unscaled vector
|
||||||
|
VECT3_COPY(imu.mag_unscaled, mag);
|
||||||
|
// scale vector
|
||||||
|
ImuScaleMag(imu);
|
||||||
|
// update ahrs
|
||||||
|
if (ahrs.status == AHRS_RUNNING) {
|
||||||
|
ahrs_update_mag();
|
||||||
|
}
|
||||||
|
mag_hmc58xx.data_available = FALSE;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
#if MODULE_HMC58XX_SYNC_SEND
|
#if MODULE_HMC58XX_SYNC_SEND
|
||||||
if (mag_hmc58xx.data_available) {
|
if (mag_hmc58xx.data_available) {
|
||||||
mag_hmc58xx_report();
|
mag_hmc58xx_report();
|
||||||
|
|||||||
@@ -337,6 +337,24 @@ void stateCalcPositionUtm_f(void) {
|
|||||||
SetBit(state.pos_status, POS_LLA_F);
|
SetBit(state.pos_status, POS_LLA_F);
|
||||||
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
|
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
|
||||||
}
|
}
|
||||||
|
else if (state.utm_initialized_f) {
|
||||||
|
if (bit_is_set(state.pos_status, POS_ENU_F)) {
|
||||||
|
UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
|
||||||
|
}
|
||||||
|
else if (bit_is_set(state.pos_status, POS_ENU_I)) {
|
||||||
|
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
|
||||||
|
SetBit(state.pos_status, POS_ENU_F);
|
||||||
|
UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
|
||||||
|
}
|
||||||
|
else if (bit_is_set(state.pos_status, POS_NED_F)) {
|
||||||
|
UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
|
||||||
|
}
|
||||||
|
else if (bit_is_set(state.pos_status, POS_NED_I)) {
|
||||||
|
NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
|
||||||
|
SetBit(state.pos_status, POS_NED_F);
|
||||||
|
UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
|
||||||
|
}
|
||||||
|
}
|
||||||
else {
|
else {
|
||||||
/* could not get this representation, set errno */
|
/* could not get this representation, set errno */
|
||||||
//struct EcefCoor_f _ecef_zero = {0.0f};
|
//struct EcefCoor_f _ecef_zero = {0.0f};
|
||||||
|
|||||||
@@ -30,6 +30,7 @@
|
|||||||
#ifndef AHRS_FLOAT_UTILS_H
|
#ifndef AHRS_FLOAT_UTILS_H
|
||||||
#define AHRS_FLOAT_UTILS_H
|
#define AHRS_FLOAT_UTILS_H
|
||||||
|
|
||||||
|
#include "math/pprz_algebra_float.h"
|
||||||
#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
|
#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
|
||||||
|
|
||||||
#include "std.h" // for ABS
|
#include "std.h" // for ABS
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,118 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2012-2013 Jean-Philippe Condomines, Gautier Hattenberger
|
||||||
|
*
|
||||||
|
* 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/>.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* For more information, please send an email to "jp.condomines@gmail.com"
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef INS_FLOAT_INVARIANT_H
|
||||||
|
#define INS_FLOAT_INVARIANT_H
|
||||||
|
|
||||||
|
#include "subsystems/ahrs.h"
|
||||||
|
#include "subsystems/ins.h"
|
||||||
|
|
||||||
|
/** Invariant filter state dimension
|
||||||
|
*/
|
||||||
|
#define INV_STATE_DIM 15
|
||||||
|
|
||||||
|
/** Invariant filter state
|
||||||
|
*/
|
||||||
|
struct inv_state {
|
||||||
|
struct FloatQuat quat; ///< Estimated attitude (quaternion)
|
||||||
|
struct FloatRates bias; ///< Estimated gyro biases
|
||||||
|
struct NedCoor_f speed; ///< Estimates speed
|
||||||
|
struct NedCoor_f pos; ///< Estimates position
|
||||||
|
float hb; ///< Estimates barometers bias
|
||||||
|
float as; ///< Estimated accelerometer sensitivity
|
||||||
|
//float cs; ///< Estimated magnetic sensitivity
|
||||||
|
};
|
||||||
|
|
||||||
|
/** Invariant filter measurement vector dimension
|
||||||
|
*/
|
||||||
|
#define INV_MEASURE_DIM 10
|
||||||
|
|
||||||
|
/** Invariant filter measurement vector
|
||||||
|
*/
|
||||||
|
struct inv_measures {
|
||||||
|
struct NedCoor_f pos_gps; ///< Measured gps position
|
||||||
|
struct NedCoor_f speed_gps; ///< Measured gps speed
|
||||||
|
struct FloatVect3 mag; ///< Measured magnetic field
|
||||||
|
float baro_alt; ///< Measured barometric altitude
|
||||||
|
};
|
||||||
|
|
||||||
|
/** Invariant filter command vector dimension
|
||||||
|
*/
|
||||||
|
#define INV_COMMAND_DIM 6
|
||||||
|
|
||||||
|
/** Invariant filter command vector
|
||||||
|
*/
|
||||||
|
struct inv_command {
|
||||||
|
struct FloatRates rates; ///< Input gyro rates
|
||||||
|
struct FloatVect3 accel; ///< Input accelerometers
|
||||||
|
};
|
||||||
|
|
||||||
|
/** Invariant filter correction gains
|
||||||
|
*/
|
||||||
|
struct inv_correction_gains {
|
||||||
|
struct FloatVect3 LE; ///< Correction gains on attitude
|
||||||
|
struct FloatVect3 ME; ///< Correction gains on speed
|
||||||
|
struct FloatVect3 NE; ///< Correction gains on position
|
||||||
|
struct FloatVect3 OE; ///< Correction gains on gyro biases
|
||||||
|
float RE; ///< Correction gains on accel bias
|
||||||
|
float SE; ///< Correction gains on barometer bias
|
||||||
|
};
|
||||||
|
|
||||||
|
/** Invariant filter tuning gains
|
||||||
|
*/
|
||||||
|
struct inv_gains {
|
||||||
|
float lv; ///< Tuning parameter of speed error on attitude
|
||||||
|
float lb; ///< Tuning parameter of mag error on attitude
|
||||||
|
float mv; ///< Tuning parameter of horizontal speed error on speed
|
||||||
|
float mvz; ///< Tuning parameter of vertical speed error on speed
|
||||||
|
float mh; ///< Tuning parameter of baro error on vertical speed
|
||||||
|
float nx; ///< Tuning parameter of horizontal position error on position
|
||||||
|
float nxz; ///< Tuning parameter of vertical position error on position
|
||||||
|
float nh; ///< Tuning parameter of baro error on vertical position
|
||||||
|
float ov; ///< Tuning parameter of speed error on gyro biases
|
||||||
|
float ob; ///< Tuning parameter of mag error on gyro biases
|
||||||
|
float rv; ///< Tuning parameter of speed error on accel biases
|
||||||
|
float rh; ///< Tuning parameter of baro error on accel biases (vertical projection)
|
||||||
|
float sh; ///< Tuning parameter of baro error on baro bias
|
||||||
|
};
|
||||||
|
|
||||||
|
/** Invariant filter structure
|
||||||
|
*/
|
||||||
|
struct InsFloatInv {
|
||||||
|
struct inv_state state; ///< state vector
|
||||||
|
struct inv_measures meas; ///< measurement vector
|
||||||
|
struct inv_command cmd; ///< command vector
|
||||||
|
struct inv_correction_gains corr; ///< correction gains
|
||||||
|
struct inv_gains gains; ///< tuning gains
|
||||||
|
};
|
||||||
|
|
||||||
|
extern struct InsFloatInv ins_impl;
|
||||||
|
|
||||||
|
extern float ins_roll_neutral;
|
||||||
|
extern float ins_pitch_neutral;
|
||||||
|
|
||||||
|
#endif /* INS_FLOAT_INVARIANT_H */
|
||||||
|
|
||||||
@@ -113,16 +113,17 @@ unit_t nav_reset_reference( void ) {
|
|||||||
nav_utm_north0 = gps.utm_pos.north/100;
|
nav_utm_north0 = gps.utm_pos.north/100;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
previous_ground_alt = ground_alt;
|
||||||
|
ground_alt = gps.hmsl/1000.;
|
||||||
|
|
||||||
// reset state UTM ref
|
// reset state UTM ref
|
||||||
struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
|
struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 };
|
||||||
stateSetLocalUtmOrigin_f(&utm0);
|
stateSetLocalUtmOrigin_f(&utm0);
|
||||||
|
|
||||||
// realign INS if needed
|
// realign INS if needed
|
||||||
ins.hf_realign = TRUE;
|
ins.hf_realign = TRUE;
|
||||||
ins.vf_realign = TRUE;
|
ins.vf_realign = TRUE;
|
||||||
|
|
||||||
previous_ground_alt = ground_alt;
|
|
||||||
ground_alt = gps.hmsl/1000.;
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -434,10 +434,15 @@ static void init_ltp(void) {
|
|||||||
fdm.ltp_g.z = 9.81;
|
fdm.ltp_g.z = 9.81;
|
||||||
|
|
||||||
#ifdef AHRS_H_X
|
#ifdef AHRS_H_X
|
||||||
#pragma message "Using magnetic field as defined in airframe file."
|
#pragma message "Using magnetic field as defined in airframe file (AHRS section)."
|
||||||
fdm.ltp_h.x = AHRS_H_X;
|
fdm.ltp_h.x = AHRS_H_X;
|
||||||
fdm.ltp_h.y = AHRS_H_Y;
|
fdm.ltp_h.y = AHRS_H_Y;
|
||||||
fdm.ltp_h.z = AHRS_H_Z;
|
fdm.ltp_h.z = AHRS_H_Z;
|
||||||
|
#elif defined INS_H_X
|
||||||
|
#pragma message "Using magnetic field as defined in airframe file (INS section)."
|
||||||
|
fdm.ltp_h.x = INS_H_X;
|
||||||
|
fdm.ltp_h.y = INS_H_Y;
|
||||||
|
fdm.ltp_h.z = INS_H_Z;
|
||||||
#else
|
#else
|
||||||
fdm.ltp_h.x = 0.4912;
|
fdm.ltp_h.x = 0.4912;
|
||||||
fdm.ltp_h.y = 0.1225;
|
fdm.ltp_h.y = 0.1225;
|
||||||
|
|||||||
Reference in New Issue
Block a user