mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-28 01:53:48 +08:00
[xsens] new imu xsens subsystem
* use the Xsens to only provide imu measurements * use an external AHRS algorithm with it
This commit is contained in:
Felix Ruess
@@ -0,0 +1,205 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!-- Microjet Multiplex (http://www.multiplex-rc.de/)
|
||||
Lisa/M v1.0 board (http://paparazzi.enac.fr/wiki/Lisa/M_v10)
|
||||
Xsens running as IMU
|
||||
Xbee modem in transparent mode
|
||||
-->
|
||||
|
||||
<airframe name="Microjet Lisa/M 1.0">
|
||||
|
||||
<firmware name="fixedwing">
|
||||
<target name="sim" board="pc"/>
|
||||
<target name="ap" board="lisa_m_1.0"/>
|
||||
|
||||
<define name="AGR_CLIMB" />
|
||||
<define name="LOITER_TRIM" />
|
||||
<define name="ALT_KALMAN" />
|
||||
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
|
||||
<!-- Communication -->
|
||||
<subsystem name="telemetry" type="transparent"/>
|
||||
|
||||
<!-- Sensors -->
|
||||
<subsystem name="imu" type="xsens">
|
||||
<configure name="XSENS_UART_NR" value="3"/>
|
||||
<configure name="XSENS_UART_BAUD" value="B115200"/>
|
||||
</subsystem>
|
||||
<subsystem name="ahrs" type="int_cmpl_quat"/>
|
||||
<subsystem name="gps" type="ublox"/>
|
||||
<subsystem name="control"/>
|
||||
<subsystem name="navigation"/>
|
||||
</firmware>
|
||||
|
||||
<modules>
|
||||
<load name="sys_mon.xml"/>
|
||||
</modules>
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR" no="0" min="1290" neutral="1290" max="1810"/>
|
||||
<servo name="AILEVON_LEFT" no="1" min="2000" neutral="1510" max="1000"/>
|
||||
<servo name="AILEVON_RIGHT" no="3" min="1000" neutral="1535" max="2000"/>
|
||||
</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="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="GYRO_P_NEUTRAL" value="0"/>
|
||||
<define name="GYRO_R_NEUTRAL" value="0"/>
|
||||
<define name="GYRO_Q_NEUTRAL" value="0"/>
|
||||
|
||||
<define name="GYRO_P_SENS" value="1" integer="16"/>
|
||||
<define name="GYRO_R_SENS" value="1" integer="16"/>
|
||||
<define name="GYRO_Q_SENS" value="1" integer="16"/>
|
||||
|
||||
<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_SENS" value="1" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="1" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="1" integer="16"/>
|
||||
|
||||
<define name="ACCEL_X_NEUTRAL" value="0"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="0"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="0"/>
|
||||
|
||||
<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="-45"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="334"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="7"/>
|
||||
|
||||
<define name="MAG_X_SENS" value="4.47647816128" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="4.71085671542" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="4.41585354498" integer="16"/>
|
||||
|
||||
<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>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- replace this with your local magnetic field -->
|
||||
<define name="H_X" value="0.3770441"/>
|
||||
<define name="H_Y" value="0.0193986"/>
|
||||
<define name="H_Z" value="0.9259921"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
|
||||
<!-- outer loop proportional gain -->
|
||||
<define name="ALTITUDE_PGAIN" value="0.03"/>
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
|
||||
|
||||
<!-- auto throttle inner loop -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
|
||||
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
|
||||
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="0.01"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||
|
||||
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="1.0"/>
|
||||
<define name="COURSE_DGAIN" value="0.3"/>
|
||||
|
||||
<define name="ROLL_MAX_SETPOINT" value="35" unit="deg"/>
|
||||
<define name="PITCH_MAX_SETPOINT" value="30" unit="deg"/>
|
||||
<define name="PITCH_MIN_SETPOINT" value="-30" unit="deg"/>
|
||||
|
||||
<define name="PITCH_PGAIN" value="12000."/>
|
||||
<define name="PITCH_DGAIN" value="1.5"/>
|
||||
|
||||
<define name="ELEVATOR_OF_ROLL" value="1250"/>
|
||||
|
||||
<define name="ROLL_SLEW" value="0.1"/>
|
||||
|
||||
<define name="ROLL_ATTITUDE_GAIN" value="7500"/>
|
||||
<define name="ROLL_RATE_GAIN" value="1500"/>
|
||||
</section>
|
||||
|
||||
<section name="AGGRESSIVE" prefix="AGR_">
|
||||
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
|
||||
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
|
||||
<define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
|
||||
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
|
||||
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
|
||||
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
|
||||
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
|
||||
<define name="DESCENT_NAV_RATIO" value="1.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"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="40." unit="deg"/>
|
||||
<define name="MAX_PITCH" value="35." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
|
||||
<define name="CARROT" value="5." unit="s"/>
|
||||
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||
<define name="CONTROL_RATE" value="60" unit="Hz"/>
|
||||
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,160 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!-- Microjet Multiplex (http://www.multiplex-rc.de/)
|
||||
Lisa/M v1.0 board (http://paparazzi.enac.fr/wiki/Lisa/M_v10)
|
||||
Xsens as full INS
|
||||
Xbee modem in transparent mode
|
||||
-->
|
||||
|
||||
<airframe name="Microjet Lisa/M 1.0">
|
||||
|
||||
<firmware name="fixedwing">
|
||||
<target name="sim" board="pc"/>
|
||||
<target name="ap" board="lisa_m_1.0"/>
|
||||
|
||||
<define name="AGR_CLIMB" />
|
||||
<define name="LOITER_TRIM" />
|
||||
<define name="ALT_KALMAN" />
|
||||
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
|
||||
<!-- Communication -->
|
||||
<subsystem name="telemetry" type="transparent"/>
|
||||
|
||||
<!-- Sensors -->
|
||||
<subsystem name="ins" type="xsens">
|
||||
<configure name="XSENS_UART_NR" value="3"/>
|
||||
<configure name="XSENS_UART_BAUD" value="B115200"/>
|
||||
</subsystem>
|
||||
|
||||
<subsystem name="control"/>
|
||||
<subsystem name="navigation"/>
|
||||
</firmware>
|
||||
|
||||
<modules>
|
||||
<load name="sys_mon.xml"/>
|
||||
</modules>
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR" no="0" min="1290" neutral="1290" max="1810"/>
|
||||
<servo name="AILEVON_LEFT" no="1" min="2000" neutral="1510" max="1000"/>
|
||||
<servo name="AILEVON_RIGHT" no="3" min="1000" neutral="1535" max="2000"/>
|
||||
</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="IMU" prefix="IMU_">
|
||||
<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>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
|
||||
<!-- outer loop proportional gain -->
|
||||
<define name="ALTITUDE_PGAIN" value="0.03"/>
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
|
||||
|
||||
<!-- auto throttle inner loop -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
|
||||
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
|
||||
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="0.01"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||
|
||||
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="1.0"/>
|
||||
<define name="COURSE_DGAIN" value="0.3"/>
|
||||
|
||||
<define name="ROLL_MAX_SETPOINT" value="35" unit="deg"/>
|
||||
<define name="PITCH_MAX_SETPOINT" value="30" unit="deg"/>
|
||||
<define name="PITCH_MIN_SETPOINT" value="-30" unit="deg"/>
|
||||
|
||||
<define name="PITCH_PGAIN" value="12000."/>
|
||||
<define name="PITCH_DGAIN" value="1.5"/>
|
||||
|
||||
<define name="ELEVATOR_OF_ROLL" value="1250"/>
|
||||
|
||||
<define name="ROLL_SLEW" value="0.1"/>
|
||||
|
||||
<define name="ROLL_ATTITUDE_GAIN" value="7500"/>
|
||||
<define name="ROLL_RATE_GAIN" value="1500"/>
|
||||
</section>
|
||||
|
||||
<section name="AGGRESSIVE" prefix="AGR_">
|
||||
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
|
||||
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
|
||||
<define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
|
||||
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
|
||||
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
|
||||
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
|
||||
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
|
||||
<define name="DESCENT_NAV_RATIO" value="1.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"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="40." unit="deg"/>
|
||||
<define name="MAX_PITCH" value="35." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
|
||||
<define name="CARROT" value="5." unit="s"/>
|
||||
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||
<define name="CONTROL_RATE" value="60" unit="Hz"/>
|
||||
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,68 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
|
||||
# XSens Mti just providing IMU measurements
|
||||
|
||||
# <subsystem name="imu" type="xsens">
|
||||
# <configure name="XSENS_UART_NR" value="0"/>
|
||||
# <configure name="XSENS_UART_BAUD" value="B115200"/>
|
||||
# </subsystem>
|
||||
#
|
||||
# <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="GYRO_P_NEUTRAL" value="0"/>
|
||||
# <define name="GYRO_R_NEUTRAL" value="0"/>
|
||||
# <define name="GYRO_Q_NEUTRAL" value="0"/>
|
||||
#
|
||||
# <define name="GYRO_P_SENS" value="1" integer="16"/>
|
||||
# <define name="GYRO_R_SENS" value="1" integer="16"/>
|
||||
# <define name="GYRO_Q_SENS" value="1" integer="16"/>
|
||||
#
|
||||
# <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_SENS" value="1" integer="16"/>
|
||||
# <define name="ACCEL_Z_SENS" value="1" integer="16"/>
|
||||
# <define name="ACCEL_Y_SENS" value="1" integer="16"/>
|
||||
#
|
||||
# <define name="ACCEL_X_NEUTRAL" value="0"/>
|
||||
# <define name="ACCEL_Z_NEUTRAL" value="0"/>
|
||||
# <define name="ACCEL_Y_NEUTRAL" value="0"/>
|
||||
#
|
||||
# <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="-45"/>
|
||||
# <define name="MAG_Y_NEUTRAL" value="334"/>
|
||||
# <define name="MAG_Z_NEUTRAL" value="7"/>
|
||||
#
|
||||
# <define name="MAG_X_SENS" value="4.47647816128" integer="16"/>
|
||||
# <define name="MAG_Y_SENS" value="4.71085671542" integer="16"/>
|
||||
# <define name="MAG_Z_SENS" value="4.41585354498" integer="16"/>
|
||||
#
|
||||
# <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>
|
||||
|
||||
|
||||
#########################################
|
||||
## IMU
|
||||
|
||||
ap.CFLAGS += -DUSE_IMU
|
||||
ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_xsens.h\"
|
||||
ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/imu.c
|
||||
|
||||
ifndef XSENS_UART_BAUD
|
||||
XSENS_UART_BAUD = B115200
|
||||
endif
|
||||
|
||||
ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
|
||||
ap.CFLAGS += -DINS_LINK=Uart$(XSENS_UART_NR)
|
||||
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
|
||||
ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
|
||||
@@ -26,9 +26,6 @@ endif
|
||||
ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
|
||||
ap.CFLAGS += -DINS_LINK=Uart$(XSENS_UART_NR)
|
||||
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
|
||||
ap.CFLAGS += -DUSE_GPS_XSENS
|
||||
ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
|
||||
ap.CFLAGS += -DGPS_NB_CHANNELS=16
|
||||
ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
|
||||
ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c
|
||||
ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP
|
||||
@@ -50,6 +47,9 @@ endif
|
||||
#########################################
|
||||
## GPS
|
||||
|
||||
ap.CFLAGS += -DUSE_GPS_XSENS
|
||||
ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
|
||||
ap.CFLAGS += -DGPS_NB_CHANNELS=16
|
||||
ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
|
||||
ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
|
||||
|
||||
@@ -61,9 +61,10 @@ extern INS_FORMAT ins_mx;
|
||||
extern INS_FORMAT ins_my;
|
||||
extern INS_FORMAT ins_mz;
|
||||
|
||||
#if USE_INS
|
||||
extern INS_FORMAT ins_roll_neutral;
|
||||
extern INS_FORMAT ins_pitch_neutral;
|
||||
|
||||
#endif
|
||||
|
||||
extern volatile uint8_t ins_msg_received;
|
||||
extern volatile uint8_t new_ins_attitude;
|
||||
|
||||
@@ -70,8 +70,10 @@ INS_FORMAT ins_mx;
|
||||
INS_FORMAT ins_my;
|
||||
INS_FORMAT ins_mz;
|
||||
|
||||
#if USE_INS
|
||||
float ins_pitch_neutral;
|
||||
float ins_roll_neutral;
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -181,11 +183,17 @@ uint8_t xsens_msg_status;
|
||||
uint16_t xsens_time_stamp;
|
||||
uint16_t xsens_output_mode;
|
||||
uint32_t xsens_output_settings;
|
||||
|
||||
|
||||
float xsens_declination = 0;
|
||||
float xsens_gps_arm_x = 0;
|
||||
float xsens_gps_arm_y = 0;
|
||||
float xsens_gps_arm_z = 0;
|
||||
|
||||
#if USE_GPS_XSENS
|
||||
struct LlaCoor_f lla_f;
|
||||
struct UtmCoor_f utm_f;
|
||||
#endif
|
||||
|
||||
struct XsensTime xsens_time;
|
||||
|
||||
@@ -196,9 +204,6 @@ static uint8_t xsens_msg_idx;
|
||||
static uint8_t ck;
|
||||
uint8_t send_ck;
|
||||
|
||||
struct LlaCoor_f lla_f;
|
||||
struct UtmCoor_f utm_f;
|
||||
|
||||
volatile int xsens_configured = 0;
|
||||
|
||||
void ins_init( void ) {
|
||||
@@ -206,8 +211,10 @@ void ins_init( void ) {
|
||||
xsens_status = UNINIT;
|
||||
xsens_configured = 20;
|
||||
|
||||
#if USE_INS
|
||||
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
|
||||
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
|
||||
#endif
|
||||
|
||||
xsens_msg_status = 0;
|
||||
xsens_time_stamp = 0;
|
||||
@@ -215,6 +222,21 @@ void ins_init( void ) {
|
||||
xsens_output_settings = XSENS_OUTPUT_SETTINGS;
|
||||
}
|
||||
|
||||
#if USE_IMU
|
||||
struct ImuXsens imu_xsens;
|
||||
|
||||
void imu_impl_init(void) {
|
||||
ins_init();
|
||||
imu_xsens.gyro_available = FALSE;
|
||||
imu_xsens.accel_available = FALSE;
|
||||
imu_xsens.mag_available = FALSE;
|
||||
}
|
||||
|
||||
void imu_periodic(void) {
|
||||
ins_periodic_task();
|
||||
}
|
||||
#endif /* USE_IMU */
|
||||
|
||||
#if USE_GPS_XSENS
|
||||
void gps_impl_init(void) {
|
||||
gps.nb_channels = 0;
|
||||
@@ -283,11 +305,10 @@ void ins_periodic_task( void ) {
|
||||
RunOnceEvery(100,XSENS_ReqGPSStatus());
|
||||
}
|
||||
|
||||
#if USE_INS
|
||||
#include "estimator.h"
|
||||
|
||||
void handle_ins_msg(void) {
|
||||
|
||||
|
||||
static inline void update_fw_estimator(void) {
|
||||
// Send to Estimator (Control)
|
||||
#ifdef XSENS_BACKWARDS
|
||||
EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral));
|
||||
@@ -296,6 +317,38 @@ void handle_ins_msg(void) {
|
||||
EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
|
||||
EstimatorSetRate(ins_p, ins_q, ins_r);
|
||||
#endif
|
||||
}
|
||||
#endif /* USE_INS */
|
||||
|
||||
void handle_ins_msg(void) {
|
||||
|
||||
#if USE_INS
|
||||
update_fw_estimator();
|
||||
#endif
|
||||
|
||||
#if USE_IMU
|
||||
#ifdef XSENS_BACKWARDS
|
||||
if (imu_xsens.gyro_available) {
|
||||
RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
|
||||
}
|
||||
if (imu_xsens.accel_available) {
|
||||
VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
|
||||
}
|
||||
if (imu_xsens.mag_available) {
|
||||
VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
|
||||
}
|
||||
#else
|
||||
if (imu_xsens.gyro_available) {
|
||||
RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
|
||||
}
|
||||
if (imu_xsens.accel_available) {
|
||||
VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
|
||||
}
|
||||
if (imu_xsens.mag_available) {
|
||||
VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
|
||||
}
|
||||
#endif /* XSENS_BACKWARDS */
|
||||
#endif /* USE_IMU */
|
||||
|
||||
#if USE_GPS_XSENS
|
||||
#ifndef ALT_KALMAN
|
||||
@@ -366,6 +419,9 @@ void parse_ins_msg( void ) {
|
||||
ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
|
||||
ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
|
||||
ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
|
||||
#if USE_IMU
|
||||
imu_xsens.gyro_available = TRUE;
|
||||
#endif
|
||||
offset += XSENS_DATA_RAWInertial_LENGTH;
|
||||
}
|
||||
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
|
||||
@@ -424,18 +480,27 @@ void parse_ins_msg( void ) {
|
||||
ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
|
||||
ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
|
||||
ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
|
||||
#if USE_IMU
|
||||
imu_xsens.accel_available = TRUE;
|
||||
#endif
|
||||
l++;
|
||||
}
|
||||
if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
|
||||
ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
|
||||
ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
|
||||
ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
|
||||
#if USE_IMU
|
||||
imu_xsens.gyro_available = TRUE;
|
||||
#endif
|
||||
l++;
|
||||
}
|
||||
if (!XSENS_MASK_MagOut(xsens_output_settings)) {
|
||||
ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
|
||||
ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
|
||||
ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
|
||||
#if USE_IMU
|
||||
imu_xsens.mag_available = TRUE;
|
||||
#endif
|
||||
l++;
|
||||
}
|
||||
offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
|
||||
|
||||
@@ -48,9 +48,44 @@ extern struct XsensTime xsens_time;
|
||||
extern uint8_t xsens_msg_status;
|
||||
extern uint16_t xsens_time_stamp;
|
||||
|
||||
|
||||
/* To use Xsens to just provide IMU measurements
|
||||
* for use with an external AHRS algorithm
|
||||
*/
|
||||
#if USE_IMU
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
struct ImuXsens {
|
||||
bool_t gyro_available;
|
||||
bool_t accel_available;
|
||||
bool_t mag_available;
|
||||
};
|
||||
extern struct ImuXsens imu_xsens;
|
||||
|
||||
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
|
||||
if (imu_xsens.accel_available) { \
|
||||
imu_xsens.accel_available = FALSE; \
|
||||
_accel_handler(); \
|
||||
} \
|
||||
if (imu_xsens.gyro_available) { \
|
||||
imu_xsens.gyro_available = FALSE; \
|
||||
_gyro_handler(); \
|
||||
} \
|
||||
if (imu_xsens.mag_available) { \
|
||||
imu_xsens.mag_available = FALSE; \
|
||||
_mag_handler(); \
|
||||
} \
|
||||
}
|
||||
#endif /* USE_IMU */
|
||||
|
||||
|
||||
/* use Xsens as a full INS solution */
|
||||
#if USE_INS
|
||||
#define InsEvent(_ins_handler) { \
|
||||
InsEventCheckAndHandle(handle_ins_msg()) \
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#if USE_GPS_XSENS
|
||||
extern bool_t gps_xsens_msg_available;
|
||||
|
||||
Reference in New Issue
Block a user