mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-28 01:53:48 +08:00
Merge pull request #209 'fixedwing_ins'
tried to clean ins xsens subsystem up a little for fixedwings
* removed the AhrsEvent
* removed all ahrs things from ins_xsens
* added an InsEvent for fixedwings
* removed some gps stuff from ins_xsens and used the normal gps callbacks instead
* made baud rate for xsens configurable
To use the XSens MTi-G as a full INS (with the xsens GPS):
```xml
<subsystem name="ins" type="xsens">
<configure name="XSENS_UART_NR" value="3"/>
<configure name="XSENS_UART_BAUD" value="B115200"/>
</subsystem>
```
to use it only as an IMU (still need to add an AHRS algorithm and other GPS as usual)
```xml
<subsystem name="imu" type="xsens">
<configure name="XSENS_UART_NR" value="3"/>
<configure name="XSENS_UART_BAUD" value="B115200"/>
</subsystem>
```
This commit is contained in:
Felix Ruess
@@ -209,7 +209,6 @@
|
||||
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
|
||||
|
||||
<configure name="AHRS_ALIGNER_LED" value="1"/>
|
||||
<configure name="CPU_LED" value="1"/>
|
||||
</target>
|
||||
<target name="sim" board="pc"/>
|
||||
|
||||
|
||||
@@ -233,7 +233,6 @@
|
||||
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
|
||||
|
||||
<configure name="AHRS_ALIGNER_LED" value="3"/>
|
||||
<configure name="CPU_LED" value="3"/>
|
||||
</target>
|
||||
<target name="sim" board="pc"/>
|
||||
|
||||
|
||||
@@ -225,7 +225,6 @@
|
||||
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
|
||||
|
||||
<configure name="AHRS_ALIGNER_LED" value="3"/>
|
||||
<configure name="CPU_LED" value="3"/>
|
||||
</target>
|
||||
<target name="sim" board="pc"/>
|
||||
|
||||
|
||||
@@ -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>
|
||||
@@ -13,7 +13,6 @@
|
||||
<configure name="AHRS_PROPAGATE_FREQUENCY" value="120"/>
|
||||
<configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
|
||||
<configure name="AHRS_ALIGNER_LED" value="2"/>
|
||||
<!--configure name="CPU_LED" value="2"/-->
|
||||
<configure name="SYS_TIME_LED" value="1"/>
|
||||
<configure name="GPS_LED" value="none"/>
|
||||
<!-- for airspeed sensor
|
||||
|
||||
@@ -34,10 +34,6 @@ ap.srcs += $(AHRS_SRCS)
|
||||
|
||||
# Extra stuff for fixedwings
|
||||
|
||||
ifdef CPU_LED
|
||||
ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
|
||||
endif
|
||||
|
||||
ifdef AHRS_PROPAGATE_FREQUENCY
|
||||
else
|
||||
AHRS_PROPAGATE_FREQUENCY = 60
|
||||
|
||||
@@ -34,10 +34,6 @@ ap.srcs += $(AHRS_SRCS)
|
||||
|
||||
# Extra stuff for fixedwings
|
||||
|
||||
ifdef CPU_LED
|
||||
ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
|
||||
endif
|
||||
|
||||
ifdef AHRS_PROPAGATE_FREQUENCY
|
||||
else
|
||||
AHRS_PROPAGATE_FREQUENCY = 60
|
||||
|
||||
@@ -25,10 +25,6 @@ ifneq ($(AHRS_ALIGNER_LED),none)
|
||||
ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
|
||||
endif
|
||||
|
||||
ifdef CPU_LED
|
||||
ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
|
||||
endif
|
||||
|
||||
ifdef AHRS_PROPAGATE_FREQUENCY
|
||||
else
|
||||
AHRS_PROPAGATE_FREQUENCY = 60
|
||||
|
||||
@@ -30,10 +30,6 @@ ap.srcs += $(AHRS_SRCS)
|
||||
|
||||
# Extra stuff for fixedwings
|
||||
|
||||
ifdef CPU_LED
|
||||
ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
|
||||
endif
|
||||
|
||||
ifdef AHRS_PROPAGATE_FREQUENCY
|
||||
else
|
||||
AHRS_PROPAGATE_FREQUENCY = 60
|
||||
|
||||
@@ -33,10 +33,6 @@ ap.srcs += $(AHRS_SRCS)
|
||||
|
||||
# Extra stuff for fixedwings
|
||||
|
||||
ifdef CPU_LED
|
||||
ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
|
||||
endif
|
||||
|
||||
ifndef AHRS_PROPAGATE_FREQUENCY
|
||||
AHRS_PROPAGATE_FREQUENCY = 60
|
||||
endif
|
||||
|
||||
@@ -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
|
||||
@@ -2,9 +2,10 @@
|
||||
|
||||
# XSens Mti-G
|
||||
|
||||
# <load name="ins_xsens_MTiG_fixedwing.xml">
|
||||
# <subsystem name="ins" type="xsens">
|
||||
# <configure name="XSENS_UART_NR" value="0"/>
|
||||
# </load>
|
||||
# <configure name="XSENS_UART_BAUD" value="B115200"/>
|
||||
# </subsystem>
|
||||
|
||||
|
||||
|
||||
@@ -13,41 +14,21 @@
|
||||
|
||||
ifeq ($(TARGET), ap)
|
||||
|
||||
# <init fun="ins_init()"/>
|
||||
# <periodic fun="ins_periodic_task()" freq="60"/>
|
||||
# <event fun="InsEventCheckAndHandle(handle_ins_msg())"/>
|
||||
# <makefile target="ap">
|
||||
# <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
|
||||
# <define name="INS_MODULE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
|
||||
# <define name="USE_UART$(XSENS_UART_NR)"/>
|
||||
# <define name="INS_LINK" value="Uart$(XSENS_UART_NR)"/>
|
||||
# <define name="UART$(XSENS_UART_NR)_BAUD" value="B230400"/>
|
||||
# <define name="USE_GPS_XSENS"/>
|
||||
# <define name="USE_GPS_XSENS_RAW_DATA" />
|
||||
# <define name="GPS_NB_CHANNELS" value="16" />
|
||||
# <define name="XSENS_OUTPUT_MODE" value="0x1836" />
|
||||
# <file name="ins_xsens.c"/>
|
||||
# <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
|
||||
# </makefile>
|
||||
|
||||
# ImuEvent -> XSensEvent
|
||||
ap.CFLAGS += -DUSE_AHRS -DUSE_INS
|
||||
ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_xsens.h\"
|
||||
ap.CFLAGS += -DUSE_INS
|
||||
|
||||
# AHRS Results
|
||||
ap.CFLAGS += -DINS_MODULE_H=\"modules/ins/ins_xsens.h\"
|
||||
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\"
|
||||
ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
|
||||
|
||||
ifndef XSENS_UART_BAUD
|
||||
XSENS_UART_BAUD = B115200
|
||||
endif
|
||||
|
||||
#B230400
|
||||
#B115200
|
||||
|
||||
ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
|
||||
ap.CFLAGS += -DINS_LINK=Uart$(XSENS_UART_NR)
|
||||
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=B230400
|
||||
ap.CFLAGS += -DUSE_GPS_XSENS
|
||||
ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
|
||||
ap.CFLAGS += -DGPS_NB_CHANNELS=16
|
||||
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
|
||||
ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
|
||||
ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c
|
||||
ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP
|
||||
@@ -69,13 +50,17 @@ endif
|
||||
#########################################
|
||||
## GPS
|
||||
|
||||
# ap.CFLAGS += -DGPS
|
||||
|
||||
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
|
||||
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
|
||||
|
||||
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
|
||||
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
|
||||
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
|
||||
sim.srcs += $(SRC_SUBSYSTEMS)/gps.c
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -207,8 +207,10 @@
|
||||
|
||||
#define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint);
|
||||
|
||||
#if USE_GPS || USE_GPS_XSENS || defined SITL
|
||||
#if USE_GPS || defined SITL
|
||||
#define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
|
||||
#else
|
||||
#define PERIODIC_SEND_GPS_SOL(_trans, _dev) {}
|
||||
#endif
|
||||
|
||||
#define PERIODIC_SEND_GPS(_trans, _dev) { \
|
||||
|
||||
@@ -93,8 +93,6 @@ static inline void on_gyro_event( void );
|
||||
static inline void on_accel_event( void );
|
||||
static inline void on_mag_event( void );
|
||||
volatile uint8_t ahrs_timeout_counter = 0;
|
||||
#else
|
||||
static inline void on_ahrs_event(void);
|
||||
#endif // USE_IMU
|
||||
#endif // USE_AHRS
|
||||
|
||||
@@ -174,6 +172,10 @@ void init_ap( void ) {
|
||||
ahrs_init();
|
||||
#endif
|
||||
|
||||
#if USE_INS
|
||||
ins_init();
|
||||
#endif
|
||||
|
||||
/************* Links initialization ***************/
|
||||
#if defined MCU_SPI_LINK
|
||||
link_mcu_init();
|
||||
@@ -479,11 +481,9 @@ void navigation_task( void ) {
|
||||
}
|
||||
|
||||
|
||||
#if USE_AHRS
|
||||
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
|
||||
volatile uint8_t new_ins_attitude = 0;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
void attitude_loop( void ) {
|
||||
|
||||
@@ -518,6 +518,10 @@ void sensors_task( void ) {
|
||||
ahrs_timeout_counter ++;
|
||||
#endif // USE_AHRS
|
||||
#endif // USE_IMU
|
||||
|
||||
#if USE_INS
|
||||
ins_periodic_task();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@@ -571,17 +575,17 @@ void event_task_ap( void ) {
|
||||
i2c_event();
|
||||
#endif
|
||||
|
||||
#if USE_AHRS
|
||||
#if USE_IMU
|
||||
#if USE_AHRS && USE_IMU
|
||||
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
|
||||
#else
|
||||
AhrsEvent(on_ahrs_event);
|
||||
#endif // USE_IMU
|
||||
#endif // USE_AHRS
|
||||
#endif
|
||||
|
||||
#if USE_INS
|
||||
InsEvent(NULL);
|
||||
#endif
|
||||
|
||||
#if USE_GPS
|
||||
GpsEvent(on_gps_solution);
|
||||
#endif /** USE_GPS */
|
||||
#endif /* USE_GPS */
|
||||
|
||||
|
||||
DatalinkEvent();
|
||||
@@ -614,7 +618,9 @@ void event_task_ap( void ) {
|
||||
#if USE_GPS
|
||||
static inline void on_gps_solution( void ) {
|
||||
estimator_update_state_gps();
|
||||
#if USE_AHRS
|
||||
ahrs_update_gps();
|
||||
#endif
|
||||
#ifdef GPS_TRIGGERED_FUNCTION
|
||||
GPS_TRIGGERED_FUNCTION();
|
||||
#endif
|
||||
@@ -631,10 +637,6 @@ static inline void on_gyro_event( void ) {
|
||||
|
||||
ahrs_timeout_counter = 0;
|
||||
|
||||
#ifdef AHRS_CPU_LED
|
||||
LED_ON(AHRS_CPU_LED);
|
||||
#endif
|
||||
|
||||
#if USE_AHRS_ALIGNER
|
||||
// Run aligner on raw data as it also makes averages.
|
||||
if (ahrs.status == AHRS_UNINIT) {
|
||||
@@ -701,10 +703,6 @@ static inline void on_gyro_event( void ) {
|
||||
}
|
||||
#endif //PERIODIC_FREQUENCY
|
||||
|
||||
#ifdef AHRS_CPU_LED
|
||||
LED_OFF(AHRS_CPU_LED);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
static inline void on_mag_event(void)
|
||||
@@ -717,13 +715,6 @@ static inline void on_mag_event(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
#else // USE_IMU not defined
|
||||
static inline void on_ahrs_event(void)
|
||||
{
|
||||
#ifdef AHRS_UPDATE_FW_ESTIMATOR
|
||||
ahrs_update_fw_estimator();
|
||||
#endif
|
||||
}
|
||||
#endif // USE_IMU
|
||||
|
||||
#endif // USE_AHRS
|
||||
|
||||
@@ -32,7 +32,6 @@
|
||||
#define INS_H
|
||||
|
||||
#include "std.h"
|
||||
#include "led.h"
|
||||
|
||||
#ifndef INS_FORMAT
|
||||
#define INS_FORMAT float
|
||||
@@ -62,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;
|
||||
@@ -97,7 +97,6 @@ void parse_ins_buffer( uint8_t );
|
||||
ReadInsBuffer(); \
|
||||
} \
|
||||
if (ins_msg_received) { \
|
||||
LED_TOGGLE(2); \
|
||||
parse_ins_msg(); \
|
||||
handler; \
|
||||
ins_msg_received = FALSE; \
|
||||
|
||||
+165
-127
@@ -1,6 +1,4 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
@@ -22,8 +20,8 @@
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file xsens.c
|
||||
* \brief Parser for the Xsens protocol
|
||||
/** @file xsens.c
|
||||
* Parser for the Xsens protocol.
|
||||
*/
|
||||
|
||||
#include "ins_module.h"
|
||||
@@ -38,10 +36,14 @@
|
||||
#include "messages.h"
|
||||
|
||||
#if USE_GPS_XSENS
|
||||
#if !USE_GPS
|
||||
#error "USE_GPS needs to be 1 to use the Xsens GPS!"
|
||||
#endif
|
||||
#include "subsystems/gps.h"
|
||||
#include "math/pprz_geodetic_wgs84.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
#include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
|
||||
bool_t gps_xsens_msg_available;
|
||||
#endif
|
||||
|
||||
INS_FORMAT ins_x;
|
||||
@@ -68,32 +70,12 @@ INS_FORMAT ins_mx;
|
||||
INS_FORMAT ins_my;
|
||||
INS_FORMAT ins_mz;
|
||||
|
||||
#if USE_INS
|
||||
float ins_pitch_neutral;
|
||||
float ins_roll_neutral;
|
||||
|
||||
|
||||
void ahrs_init(void)
|
||||
{
|
||||
ins_init();
|
||||
}
|
||||
|
||||
|
||||
#ifdef USE_IMU
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
void imu_init(void)
|
||||
{
|
||||
ins_init();
|
||||
}
|
||||
|
||||
void imu_periodic(void)
|
||||
{
|
||||
ins_periodic_task();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// XSens Specific
|
||||
@@ -201,19 +183,19 @@ 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
|
||||
|
||||
int8_t xsens_hour;
|
||||
int8_t xsens_min;
|
||||
int8_t xsens_sec;
|
||||
int32_t xsens_nanosec;
|
||||
int16_t xsens_year;
|
||||
int8_t xsens_month;
|
||||
int8_t xsens_day;
|
||||
struct XsensTime xsens_time;
|
||||
|
||||
static uint8_t xsens_id;
|
||||
static uint8_t xsens_status;
|
||||
@@ -222,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 ) {
|
||||
@@ -232,80 +211,104 @@ 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;
|
||||
xsens_output_mode = XSENS_OUTPUT_MODE;
|
||||
xsens_output_settings = XSENS_OUTPUT_SETTINGS;
|
||||
|
||||
gps.nb_channels = 0;
|
||||
}
|
||||
|
||||
#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;
|
||||
gps_xsens_msg_available = FALSE;
|
||||
}
|
||||
#endif
|
||||
|
||||
void ins_periodic_task( void ) {
|
||||
if (xsens_configured > 0)
|
||||
{
|
||||
switch (xsens_configured)
|
||||
{
|
||||
case 20:
|
||||
/* send mode and settings to MT */
|
||||
XSENS_GoToConfig();
|
||||
XSENS_SetOutputMode(xsens_output_mode);
|
||||
XSENS_SetOutputSettings(xsens_output_settings);
|
||||
break;
|
||||
case 18:
|
||||
// Give pulses on SyncOut
|
||||
XSENS_SetSyncOutSettings(0,0x0002);
|
||||
break;
|
||||
case 17:
|
||||
// 1 pulse every 100 samples
|
||||
XSENS_SetSyncOutSettings(1,100);
|
||||
break;
|
||||
case 2:
|
||||
XSENS_ReqLeverArmGps();
|
||||
break;
|
||||
{
|
||||
switch (xsens_configured)
|
||||
{
|
||||
case 20:
|
||||
/* send mode and settings to MT */
|
||||
XSENS_GoToConfig();
|
||||
XSENS_SetOutputMode(xsens_output_mode);
|
||||
XSENS_SetOutputSettings(xsens_output_settings);
|
||||
break;
|
||||
case 18:
|
||||
// Give pulses on SyncOut
|
||||
XSENS_SetSyncOutSettings(0,0x0002);
|
||||
break;
|
||||
case 17:
|
||||
// 1 pulse every 100 samples
|
||||
XSENS_SetSyncOutSettings(1,100);
|
||||
break;
|
||||
case 2:
|
||||
XSENS_ReqLeverArmGps();
|
||||
break;
|
||||
|
||||
case 6:
|
||||
XSENS_ReqMagneticDeclination();
|
||||
break;
|
||||
case 6:
|
||||
XSENS_ReqMagneticDeclination();
|
||||
break;
|
||||
|
||||
case 13:
|
||||
#ifdef AHRS_H_X
|
||||
#pragma message "Sending XSens Magnetic Declination."
|
||||
xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
|
||||
XSENS_SetMagneticDeclination(xsens_declination);
|
||||
#endif
|
||||
break;
|
||||
case 12:
|
||||
#ifdef GPS_IMU_LEVER_ARM_X
|
||||
#pragma message "Sending XSens GPS Arm."
|
||||
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
|
||||
#endif
|
||||
break;
|
||||
case 10:
|
||||
{
|
||||
uint8_t baud = 1;
|
||||
XSENS_SetBaudrate(baud);
|
||||
}
|
||||
break;
|
||||
case 13:
|
||||
#ifdef AHRS_H_X
|
||||
#pragma message "Sending XSens Magnetic Declination."
|
||||
xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
|
||||
XSENS_SetMagneticDeclination(xsens_declination);
|
||||
#endif
|
||||
break;
|
||||
case 12:
|
||||
#ifdef GPS_IMU_LEVER_ARM_X
|
||||
#pragma message "Sending XSens GPS Arm."
|
||||
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
|
||||
#endif
|
||||
break;
|
||||
case 10:
|
||||
{
|
||||
uint8_t baud = 1;
|
||||
XSENS_SetBaudrate(baud);
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
XSENS_GoToMeasurment();
|
||||
break;
|
||||
}
|
||||
xsens_configured--;
|
||||
return;
|
||||
}
|
||||
case 1:
|
||||
XSENS_GoToMeasurment();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
xsens_configured--;
|
||||
return;
|
||||
}
|
||||
|
||||
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));
|
||||
@@ -314,38 +317,55 @@ 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 */
|
||||
|
||||
// Position
|
||||
float gps_east = gps.utm_pos.east / 100.;
|
||||
float gps_north = gps.utm_pos.north / 100.;
|
||||
gps_east -= nav_utm_east0;
|
||||
gps_north -= nav_utm_north0;
|
||||
EstimatorSetPosXY(gps_east, gps_north);
|
||||
void handle_ins_msg(void) {
|
||||
|
||||
// Altitude and vertical speed
|
||||
float hmsl = gps.hmsl;
|
||||
hmsl /= 1000.0f;
|
||||
EstimatorSetAlt(hmsl);
|
||||
#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
|
||||
#warning NO_VZ
|
||||
#endif
|
||||
|
||||
// Horizontal speed
|
||||
float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
|
||||
if (gps.fix != GPS_FIX_3D)
|
||||
{
|
||||
if (gps.fix != GPS_FIX_3D) {
|
||||
fspeed = 0;
|
||||
}
|
||||
float fclimb = -ins_vz;
|
||||
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
|
||||
EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
|
||||
|
||||
// Now also finish filling the gps struct for telemetry purposes
|
||||
gps.gspeed = fspeed * 100.;
|
||||
gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
|
||||
gps.course = fcourse * 1e7;
|
||||
|
||||
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
|
||||
gps.course = fcourse * 1e7;
|
||||
#endif // USE_GPS_XSENS
|
||||
}
|
||||
|
||||
void parse_ins_msg( void ) {
|
||||
@@ -376,8 +396,6 @@ void parse_ins_msg( void ) {
|
||||
gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
|
||||
gps.num_sv = 0;
|
||||
|
||||
gps.last_fix_time = sys_time.nb_sec;
|
||||
|
||||
uint8_t i;
|
||||
// Do not write outside buffer
|
||||
for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
|
||||
@@ -401,21 +419,20 @@ 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)) {
|
||||
#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
|
||||
#ifdef GPS_LED
|
||||
LED_TOGGLE(GPS_LED);
|
||||
#endif
|
||||
gps.last_fix_time = sys_time.nb_sec;
|
||||
|
||||
gps.week = 0; // FIXME
|
||||
gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
|
||||
gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
|
||||
gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
|
||||
gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
|
||||
|
||||
|
||||
/* Set the real UTM zone */
|
||||
gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
|
||||
|
||||
@@ -434,9 +451,9 @@ void parse_ins_msg( void ) {
|
||||
// Altitude: Xsens LLH gives ellipsoid height
|
||||
ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
|
||||
|
||||
// Compute geoid (MSL) height
|
||||
// Compute geoid (MSL) height
|
||||
float hmsl;
|
||||
WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
|
||||
WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
|
||||
gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
|
||||
|
||||
ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
|
||||
@@ -448,6 +465,8 @@ void parse_ins_msg( void ) {
|
||||
gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
|
||||
gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
|
||||
gps.pdop = 5; // FIXME Not output by XSens
|
||||
|
||||
gps_xsens_msg_available = TRUE;
|
||||
#endif
|
||||
offset += XSENS_DATA_RAWGPS_LENGTH;
|
||||
}
|
||||
@@ -461,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;
|
||||
@@ -516,8 +544,6 @@ void parse_ins_msg( void ) {
|
||||
}
|
||||
if (XSENS_MASK_Position(xsens_output_mode)) {
|
||||
#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
|
||||
gps.last_fix_time = sys_time.nb_sec;
|
||||
|
||||
lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
|
||||
lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
|
||||
gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
|
||||
@@ -533,6 +559,8 @@ void parse_ins_msg( void ) {
|
||||
ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid?
|
||||
gps.hmsl = ins_z * 1000;
|
||||
// what about gps.lla_pos.alt and gps.utm_pos.alt ?
|
||||
|
||||
gps_xsens_msg_available = TRUE;
|
||||
#endif
|
||||
offset += XSENS_DATA_Position_LENGTH;
|
||||
}
|
||||
@@ -550,7 +578,15 @@ void parse_ins_msg( void ) {
|
||||
if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
|
||||
else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
|
||||
else gps.fix = GPS_FIX_NONE;
|
||||
#endif
|
||||
#ifdef GPS_LED
|
||||
if (gps.fix == GPS_FIX_3D) {
|
||||
LED_ON(GPS_LED);
|
||||
}
|
||||
else {
|
||||
LED_TOGGLE(GPS_LED);
|
||||
}
|
||||
#endif // GPS_LED
|
||||
#endif // USE_GPS_XSENS
|
||||
offset += XSENS_DATA_Status_LENGTH;
|
||||
}
|
||||
if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
|
||||
@@ -561,13 +597,13 @@ void parse_ins_msg( void ) {
|
||||
offset += XSENS_DATA_TimeStamp_LENGTH;
|
||||
}
|
||||
if (XSENS_MASK_UTC(xsens_output_settings)) {
|
||||
xsens_hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
|
||||
xsens_min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
|
||||
xsens_sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
|
||||
xsens_nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
|
||||
xsens_year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
|
||||
xsens_month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
|
||||
xsens_day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
|
||||
xsens_time.hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
|
||||
xsens_time.min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
|
||||
xsens_time.sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
|
||||
xsens_time.nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
|
||||
xsens_time.year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
|
||||
xsens_time.month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
|
||||
xsens_time.day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
|
||||
|
||||
offset += XSENS_DATA_UTC_LENGTH;
|
||||
}
|
||||
@@ -614,6 +650,8 @@ void parse_ins_buffer( uint8_t c ) {
|
||||
ins_msg_received = TRUE;
|
||||
goto restart;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return;
|
||||
error:
|
||||
|
||||
@@ -33,23 +33,72 @@
|
||||
|
||||
#include "ins_module.h"
|
||||
|
||||
#include "subsystems/ahrs.h"
|
||||
struct XsensTime {
|
||||
int8_t hour;
|
||||
int8_t min;
|
||||
int8_t sec;
|
||||
int32_t nanosec;
|
||||
int16_t year;
|
||||
int8_t month;
|
||||
int8_t day;
|
||||
};
|
||||
|
||||
extern struct XsensTime xsens_time;
|
||||
|
||||
extern int8_t xsens_hour;
|
||||
extern int8_t xsens_min;
|
||||
extern int8_t xsens_sec;
|
||||
extern int32_t xsens_nanosec;
|
||||
extern int16_t xsens_year;
|
||||
extern int8_t xsens_month;
|
||||
extern int8_t xsens_day;
|
||||
extern uint8_t xsens_msg_status;
|
||||
extern uint16_t xsens_time_stamp;
|
||||
|
||||
#define AhrsEvent(_ahrs_handler) { \
|
||||
LED_TOGGLE(3); \
|
||||
|
||||
/* 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;
|
||||
#define GpsEvent(_sol_available_callback) { \
|
||||
if (gps_xsens_msg_available) { \
|
||||
if (gps.fix == GPS_FIX_3D) { \
|
||||
gps.last_fix_ticks = sys_time.nb_sec_rem; \
|
||||
gps.last_fix_time = sys_time.nb_sec; \
|
||||
} \
|
||||
_sol_available_callback(); \
|
||||
gps_xsens_msg_available = FALSE; \
|
||||
} \
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
@@ -36,7 +36,4 @@ extern void ahrs_update_infrared(void);
|
||||
// TODO copy ahrs to state instead of estimator
|
||||
extern void ahrs_update_fw_estimator(void);
|
||||
|
||||
#define AhrsEvent(_available_callback) { \
|
||||
}
|
||||
|
||||
#endif /* AHRS_INFRARED_H */
|
||||
|
||||
@@ -53,6 +53,10 @@ void update_ahrs_from_sim(void) {
|
||||
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
|
||||
|
||||
compute_body_orientation_and_rates();
|
||||
|
||||
#ifdef AHRS_UPDATE_FW_ESTIMATOR
|
||||
ahrs_update_fw_estimator();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@@ -92,6 +96,10 @@ void ahrs_align(void)
|
||||
|
||||
|
||||
void ahrs_propagate(void) {
|
||||
if (ahrs_sim_available) {
|
||||
update_ahrs_from_sim();
|
||||
ahrs_sim_available = FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
void ahrs_update_accel(void) {
|
||||
|
||||
@@ -37,14 +37,4 @@ extern float ins_pitch_neutral;
|
||||
|
||||
extern void update_ahrs_from_sim(void);
|
||||
|
||||
#define AhrsEvent(_available_callback) { \
|
||||
if (ahrs_sim_available) { \
|
||||
update_ahrs_from_sim(); \
|
||||
_available_callback(); \
|
||||
ahrs_sim_available = FALSE; \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* AHRS_SIM_H */
|
||||
|
||||
Reference in New Issue
Block a user