Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Merge branch 'aspirin2' into dev

Browse Source
This commit is contained in:
Piotr Esden-Tempski
2012-02-10 23:00:16 -08:00
parent 5dbcaa69ac 16c2bbf176
commit 1a017eed9a
14 changed files with 1907 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/CDW/LisaAspirin2.xml
+249
View File
@@ -0,0 +1,249 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
Lisa + Aspirin v2 using SPI only
-->
<airframe name="LisaAspirin2">
<servos>
<servo name="THROTTLE" no="2" min="1000" neutral="1000" max="2000"/>
<servo name="AILEVON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
<servo name="AILEVON_RIGHT" no="0" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="BRAKE" failsafe_value="0"/> <!-- both elerons up as butterfly brake ? -->
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="BRAKE" value="@YAW"/>
</rc_commands>
<command_laws>
<set servo="AILEVON_LEFT" value="@ROLL + @PITCH"/>
<set servo="AILEVON_RIGHT" value="@ROLL - @PITCH"/>
<set servo="THROTTLE" value="@THROTTLE"/>
<set servo="ELEVATOR" value="@PITCH"/>
</command_laws>
<!-- Local magnetic field -->
<section name="AHRS" prefix="AHRS_" >
<define name="H_X" value="0.51562740288882" />
<define name="H_Y" value="-0.05707735220832" />
<define name="H_Z" value="0.85490967783446" />
</section>
<section name="IMU" prefix="IMU_">
<!-- Calibration Neutral -->
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<!-- SENS = 16.4 LSB/(deg/sec) * 57.6 deg/rad = 939.650 LSB/rad/sec / 12bit FRAC: 4096 / 939.65 -->
<define name="GYRO_P_SENS" value="4.359" integer="16"/>
<define name="GYRO_Q_SENS" value="4.359" integer="16"/>
<define name="GYRO_R_SENS" value="4.359" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!-- SENS = 2048 LSB/g / 9.81 ms2/g = 208.766564729 LSB/ms2 / 10bit FRAC: 1024 / 208.7665 -->
<define name="ACCEL_X_SENS" value="9.81" integer="16"/>
<define name="ACCEL_Y_SENS" value="9.81" integer="16"/>
<define name="ACCEL_Z_SENS" value="9.81" integer="16"/>
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<define name="MAG_X_SENS" value="1" integer="16"/>
<define name="MAG_Y_SENS" value="1" integer="16"/>
<define name="MAG_Z_SENS" value="1" 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" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.7"/>
<define name="MAX_PITCH" value="0.7"/>
</section>
<section name="BAT">
<define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
<define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
<define name="GLIDE_AIRSPEED" value="10"/>
<define name="GLIDE_VSPEED" value="3."/>
<define name="GLIDE_PITCH" value="45" 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.20000004768"/>
<define name="COURSE_DGAIN" value="0.3"/>
<define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
<define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="PITCH_PGAIN" value="-12000."/>
<define name="PITCH_DGAIN" value="1.5"/>
<define name="ELEVATOR_OF_ROLL" value="1000."/>
<define name="ROLL_SLEW" value="1."/>
<define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
<define name="ROLL_RATE_GAIN" value="0."/>
</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="DEFAULT_THROTTLE" value="0.35" unit="%"/>
<define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
<define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
<define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
<define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
</section>
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
</section>
<modules>
<!--
<load name="light.xml">
<define name="LIGHT_LED_STROBE" value="2"/>
<define name="LIGHT_LED_NAV" value="3"/>
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
<define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
</load>
<load name="digital_cam_i2c.xml"/> -->
<!-- <load name="ins_ppzuavimu.xml" /> -->
<!--
<load name="digital_cam.xml" >
<define name="DC_SHUTTER_LED" value="3"/>
</load>
-->
</modules>
<firmware name="fixedwing">
<target name="ap" board="lisa_m_1.0">
<define name="STRONG_WIND"/>
<define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/>
<configure name="PERIODIC_FREQUENCY" value="120"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
<configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
<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"/>
<define name="AGR_CLIMB"/>
<define name="LOITER_TRIM"/>
<define name="ALT_KALMAN"/>
<!-- Sensors -->
<!--
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" />
</subsystem>
-->
<subsystem name="imu" type="aspirin_v2.0"/>
<subsystem name="ahrs" type="float_dcm">
<!-- <define name="USE_MAGNETOMETER" /> -->
</subsystem>
<subsystem name="radio_control" type="ppm"/>
<!-- Communication -->
<subsystem name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B57600"/>
</subsystem>
<!-- Actuators -->
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="navigation"/>
<subsystem name="gps" type="ublox_utm"/>
</firmware>
</airframe>
conf/airframes/CDW/yapa3_aspirin2.xml
+274
View File
@@ -0,0 +1,274 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
YAPA + XSens + XBee
-->
<airframe name="Yapa v1">
<servos>
<servo name="THROTTLE" no="9" min="1000" neutral="1000" max="2000"/>
<servo name="AILERON_LEFT" no="8" min="1000" neutral="1500" max="2000"/>
<servo name="AILERON_RIGHT" no="7" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="6" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="5" min="1100" neutral="1500" max="1900"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="BRAKE" failsafe_value="0"/> <!-- both elerons up as butterfly brake ? -->
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="BRAKE" value="@YAW"/>
</rc_commands>
<command_laws>
<set servo="AILERON_LEFT" value="@ROLL"/>
<set servo="AILERON_RIGHT" value="@ROLL"/>
<set servo="THROTTLE" value="@THROTTLE"/>
<set servo="ELEVATOR" value="@PITCH"/>
</command_laws>
<!-- Local magnetic field -->
<section name="AHRS" prefix="AHRS_" >
<define name="H_X" value="0.51562740288882" />
<define name="H_Y" value="-0.05707735220832" />
<define name="H_Z" value="0.85490967783446" />
</section>
<section name="IMU" prefix="IMU_">
<!-- Calibration Neutral -->
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<!-- SENS = 16.4 LSB/(deg/sec) * 57.6 deg/rad = 939.650 LSB/rad/sec / 12bit FRAC: 4096 / 939.65 -->
<define name="GYRO_P_SENS" value="4.359" integer="16"/>
<define name="GYRO_Q_SENS" value="4.359" integer="16"/>
<define name="GYRO_R_SENS" value="4.359" integer="16"/>
<define name="GYRO_P_Q" value="0."/>
<define name="GYRO_P_R" value="0"/>
<define name="GYRO_Q_P" value="0."/>
<define name="GYRO_Q_R" value="0."/>
<define name="GYRO_R_P" value="0."/>
<define name="GYRO_R_Q" value="0."/>
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_Q_SIGN" value="1"/>
<define name="GYRO_R_SIGN" value="1"/>
<define name="ACCEL_X_NEUTRAL" value="-40"/>
<define name="ACCEL_Y_NEUTRAL" value="-15"/>
<define name="ACCEL_Z_NEUTRAL" value="375"/>
<!-- SENS = 2048 LSB/g / 9.81 ms2/g = 208.766564729 LSB/ms2 / 10bit FRAC: 1024 / 208.7665 -->
<define name="ACCEL_X_SENS" value="9.81" integer="16"/>
<define name="ACCEL_Y_SENS" value="9.81" integer="16"/>
<define name="ACCEL_Z_SENS" value="9.81" integer="16"/>
<!--
<define name="ACCEL_X_SENS" value="4.905" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.905" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.905" 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="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<define name="MAG_X_SENS" value="1" integer="16"/>
<define name="MAG_Y_SENS" value="1" integer="16"/>
<define name="MAG_Z_SENS" value="1" 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="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" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.7"/>
<define name="MAX_PITCH" value="0.7"/>
</section>
<section name="BAT">
<define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
<define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
<define name="GLIDE_AIRSPEED" value="10"/>
<define name="GLIDE_VSPEED" value="3."/>
<define name="GLIDE_PITCH" value="45" 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.20000004768"/>
<define name="COURSE_DGAIN" value="0.3"/>
<define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
<define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="PITCH_PGAIN" value="-12000."/>
<define name="PITCH_DGAIN" value="1.5"/>
<define name="ELEVATOR_OF_ROLL" value="1000."/>
<define name="ROLL_SLEW" value="1."/>
<define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
<define name="ROLL_RATE_GAIN" value="0."/>
</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="DEFAULT_THROTTLE" value="0.35" unit="%"/>
<define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
<define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
<define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
<define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
</section>
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
</section>
<modules>
<!--
<load name="light.xml">
<define name="LIGHT_LED_STROBE" value="3"/>
<define name="LIGHT_LED_NAV" value="2"/>
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
<define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
</load>
-->
<!-- <load name="digital_cam_i2c.xml"/> -->
<!-- <load name="ins_ppzuavimu.xml" /> -->
<!--
<load name="digital_cam.xml" >
<define name="DC_SHUTTER_LED" value="3"/>
</load>
-->
</modules>
<firmware name="fixedwing">
<target name="ap" board="tiny_2.11">
<define name="STRONG_WIND"/>
<define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/>
<configure name="PERIODIC_FREQUENCY" value="120"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
<configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
<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"/>
<define name="AGR_CLIMB"/>
<define name="LOITER_TRIM"/>
<define name="ALT_KALMAN"/>
<!-- Sensors -->
<!--
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" />
</subsystem>
<subsystem name="imu" type="aspirin_i2c"/>
-->
<subsystem name="imu" type="aspirin2_i2c"/>
<subsystem name="ahrs" type="float_dcm">
<!-- <define name="USE_MAGNETOMETER" /> -->
</subsystem>
<subsystem name="radio_control" type="ppm"/>
<!-- Communication -->
<subsystem name="telemetry" type="xbee_api">
<configure name="MODEM_BAUD" value="B9600"/>
</subsystem>
<!-- Actuators -->
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="navigation"/>
<subsystem name="gps" type="ublox_utm"/>
</firmware>
</airframe>
conf/autopilot/subsystems/fixedwing/imu_aspirin2_i2c.makefile
+23
View File
@@ -0,0 +1,23 @@
# Hey Emacs, this is a -*- makefile -*-
IMU_ASPIRIN2_CFLAGS = -DUSE_IMU
IMU_ASPIRIN2_CFLAGS += -DIMU_TYPE_H=\"modules/sensors/imu_aspirin2.h\"
IMU_ASPIRIN2_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_MODULES)/sensors/imu_aspirin2.c
IMU_ASPIRIN2_CFLAGS += -DUSE_I2C
ifeq ($(ARCH), stm32)
IMU_ASPIRIN2_CFLAGS += -DUSE_I2C2
IMU_ASPIRIN2_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c2
else ifeq ($(ARCH), lpc21)
IMU_ASPIRIN2_CFLAGS += -DUSE_I2C0
IMU_ASPIRIN2_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c0
endif
ap.CFLAGS += $(IMU_ASPIRIN2_CFLAGS)
ap.srcs += $(IMU_ASPIRIN2_SRCS)
ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
conf/autopilot/subsystems/shared/imu_aspirin_v2.0.makefile
+63
View File
@@ -0,0 +1,63 @@
# Hey Emacs, this is a -*- makefile -*-
#
# Aspirin IMU v2.0
#
#
# required xml:
# <section name="IMU" prefix="IMU_">
#
# <define name="GYRO_X_NEUTRAL" value="33924"/>
# <define name="GYRO_Y_NEUTRAL" value="33417"/>
# <define name="GYRO_Z_NEUTRAL" value="32809"/>
#
# <define name="GYRO_X_SENS" value="1.01" integer="16"/>
# <define name="GYRO_Y_SENS" value="1.01" integer="16"/>
# <define name="GYRO_Z_SENS" value="1.01" integer="16"/>
#
# <define name="ACCEL_X_NEUTRAL" value="32081"/>
# <define name="ACCEL_Y_NEUTRAL" value="33738"/>
# <define name="ACCEL_Z_NEUTRAL" value="32441"/>
#
# <define name="ACCEL_X_SENS" value="2.50411474" integer="16"/>
# <define name="ACCEL_Y_SENS" value="2.48126183" integer="16"/>
# <define name="ACCEL_Z_SENS" value="2.51396167" integer="16"/>
#
# <define name="MAG_X_NEUTRAL" value="2358"/>
# <define name="MAG_Y_NEUTRAL" value="2362"/>
# <define name="MAG_Z_NEUTRAL" value="2119"/>
#
# <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
# <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
# <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
#
# </section>
#
#
# imu aspirin
IMU_ASPIRIN_CFLAGS = -DUSE_IMU
IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\" -DIMU_OVERRIDE_CHANNELS
IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c \
$(SRC_ARCH)/subsystems/imu/imu_aspirin2_arch.c \
$(SRC_ARCH)/mcu_periph/spi_arch.c \
mcu_periph/spi.c
IMU_ASPIRIN_CFLAGS += -DUSE_SPI
ifeq ($(ARCH), lpc21)
#TODO
else ifeq ($(ARCH), stm32)
# IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
endif
IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_0
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
ap.srcs += $(IMU_ASPIRIN_SRCS)
sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+191
View File
@@ -0,0 +1,191 @@
#include "subsystems/imu.h"
#include <stm32/gpio.h>
#include <stm32/misc.h>
#include <stm32/rcc.h>
#include <stm32/exti.h>
#include <stm32/spi.h>
#include <stm32/dma.h>
#include "mcu_periph/spi.h"
// SPI2 Slave Selection
#define Spi2Slave0Unselect() GPIOB->BSRR = GPIO_Pin_12
#define Spi2Slave0Select() GPIOB->BRR = GPIO_Pin_12
// spi dma end of rx handler
void dma1_c4_irq_handler(void);
void spi_arch_int_enable(void) {
// Enable DMA1 channel4 IRQ Channel ( SPI RX)
NVIC_InitTypeDef NVIC_init_struct = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = 0,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE
};
NVIC_Init(&NVIC_init_struct);
}
void spi_arch_int_disable(void) {
// Enable DMA1 channel4 IRQ Channel ( SPI RX)
NVIC_InitTypeDef NVIC_init_struct = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = 0,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = DISABLE
};
NVIC_Init(&NVIC_init_struct);
}
void spi_init(void) {
GPIO_InitTypeDef GPIO_InitStructure;
SPI_InitTypeDef SPI_InitStructure;
// Enable SPI2 Periph clock -------------------------------------------------
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
// Configure GPIOs: SCK, MISO and MOSI --------------------------------
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
SPI_Cmd(SPI2, ENABLE);
// configure SPI
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_64;
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
SPI_InitStructure.SPI_CRCPolynomial = 7;
SPI_Init(SPI2, &SPI_InitStructure);
// Enable SPI_2 DMA clock ---------------------------------------------------
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
// SLAVE 0
// set accel slave select as output and assert it ( on PB12)
Spi2Slave0Unselect();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
spi_arch_int_enable();
}
/*
void adxl345_write_to_reg(uint8_t addr, uint8_t val) {
Adxl345Select();
SPI_I2S_SendData(SPI2, addr);
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
SPI_I2S_SendData(SPI2, val);
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
Adxl345Unselect();
}
void spi_clear_rx_buf(void) {
uint8_t __attribute__ ((unused)) ret = SPI_I2S_ReceiveData(SPI2);
}
*/
struct spi_transaction* slave0;
void spi_rw(struct spi_transaction * _trans)
{
// Store local copy to notify of the results
slave0 = _trans;
slave0->status = SPITransRunning;
Spi2Slave0Select();
// SPI2_Rx_DMA_Channel configuration ------------------------------------
DMA_DeInit(DMA1_Channel4);
DMA_InitTypeDef DMA_initStructure_4 = {
.DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
.DMA_MemoryBaseAddr = (uint32_t) slave0->miso_buf,
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_BufferSize = slave0->length,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_VeryHigh,
.DMA_M2M = DMA_M2M_Disable
};
DMA_Init(DMA1_Channel4, &DMA_initStructure_4);
// SPI2_Tx_DMA_Channel configuration ------------------------------------
DMA_DeInit(DMA1_Channel5);
DMA_InitTypeDef DMA_initStructure_5 = {
.DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
.DMA_MemoryBaseAddr = (uint32_t) slave0->mosi_buf,
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_BufferSize = slave0->length,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable
};
DMA_Init(DMA1_Channel5, &DMA_initStructure_5);
// Enable SPI_2 Rx request
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, ENABLE);
// Enable DMA1 Channel4
DMA_Cmd(DMA1_Channel4, ENABLE);
// Enable SPI_2 Tx request
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, ENABLE);
// Enable DMA1 Channel5
DMA_Cmd(DMA1_Channel5, ENABLE);
// Enable DMA1 Channel4 Transfer Complete interrupt
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
}
// Accel end of DMA transfert
void dma1_c4_irq_handler(void)
{
Spi2Slave0Unselect();
if (DMA_GetITStatus(DMA1_IT_TC4)) {
// clear int pending bit
DMA_ClearITPendingBit(DMA1_IT_GL4);
// mark as available
spi_message_received = TRUE;
}
// disable DMA Channel
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
// Disable SPI_2 Rx and TX request
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE);
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE);
// Disable DMA1 Channel4 and 5
DMA_Cmd(DMA1_Channel4, DISABLE);
DMA_Cmd(DMA1_Channel5, DISABLE);
slave0->status = SPITransSuccess;
*(slave0->ready) = 1;
}
sw/airborne/arch/stm32/mcu_periph/spi_arch.h
+166
View File
@@ -0,0 +1,166 @@
/* $Id$
*
* Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin
*
* 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.
*
*/
/** \brief handling of stm32 SPI hardware
*/
#ifndef SPI_ARCH_H
#define SPI_ARCH_H
#include "mcu_periph/spi.h"
#include <stm32/gpio.h>
extern void spi_arch_int_enable(void);
extern void spi_arch_int_disable(void);
extern void spi_clear_rx_buf(void);
void spi_rw(struct spi_transaction * _trans);
/*
//////////
// from aspirin_arch.h
extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
extern void adxl345_start_reading_data(void);
*/
//////////
// from lpc spi_arch
/*
#define SpiTransmit() { \
while (spi_tx_idx < spi_buffer_length \
&& bit_is_set(SSPSR, TNF)) { \
SpiSend(spi_buffer_output[spi_tx_idx]); \
spi_tx_idx++; \
} \
if (spi_tx_idx == spi_buffer_length) \
SpiDisableTxi(); \
}
#define SpiReceive() { \
while (bit_is_set(SSPSR, RNE)) { \
if (spi_rx_idx < spi_buffer_length) { \
SpiRead(spi_buffer_input[spi_rx_idx]) \
spi_rx_idx++; \
} \
else { \
uint8_t foo; \
SpiRead(foo); \
} \
} \
}
#ifdef SPI_MASTER
// !!!!!!!!!!!!! Code for one single slave at a time !!!!!!!!!!!!!!!!!
#if defined SPI_SELECT_SLAVE1_PIN && defined SPI_SELECT_SLAVE0_PIN
#error "SPI: one single slave, please"
#endif
#define SpiStart() { \
SpiEnable(); \
SpiInitBuf(); \
SpiEnableTxi(); // enable tx fifo half empty interrupt \
}
*/
/*
* Slave0 select : P0.20 PINSEL1 00 << 8
* Slave1 select : P1.20
*
*/
/*
#define SPI_SELECT_SLAVE_IO__(port, reg) IO ## port ## reg
#define SPI_SELECT_SLAVE_IO_(port, reg) SPI_SELECT_SLAVE_IO__(port, reg)
#define SPI_SELECT_SLAVE0_IODIR SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE0_PORT, DIR)
#define SPI_SELECT_SLAVE0_IOCLR SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE0_PORT, CLR)
#define SPI_SELECT_SLAVE0_IOSET SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE0_PORT, SET)
#define SPI_SELECT_SLAVE1_IODIR SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE1_PORT, DIR)
#define SPI_SELECT_SLAVE1_IOCLR SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE1_PORT, CLR)
#define SPI_SELECT_SLAVE1_IOSET SPI_SELECT_SLAVE_IO_(SPI_SELECT_SLAVE1_PORT, SET)
#define SpiSelectSlave0() { \
spi_cur_slave = SPI_SLAVE0; \
SetBit(SPI_SELECT_SLAVE0_IOCLR, SPI_SELECT_SLAVE0_PIN); \
}
#define SpiUnselectSlave0() { \
spi_cur_slave = SPI_NONE; \
SetBit(SPI_SELECT_SLAVE0_IOSET, SPI_SELECT_SLAVE0_PIN); \
}
#define SpiSelectSlave1() { \
spi_cur_slave = SPI_SLAVE1; \
SetBit(SPI_SELECT_SLAVE1_IOCLR, SPI_SELECT_SLAVE1_PIN); \
}
#define SpiUnselectSlave1() { \
spi_cur_slave = SPI_NONE; \
SetBit(SPI_SELECT_SLAVE1_IOSET, SPI_SELECT_SLAVE1_PIN); \
}
#ifdef SPI_SELECT_SLAVE0_PIN
#define SpiUnselectCurrentSlave() SpiUnselectSlave0()
#endif
#ifdef SPI_SELECT_SLAVE1_PIN
#define SpiUnselectCurrentSlave() SpiUnselectSlave1()
#endif
#endif // SPI_MASTER
#define SpiSetCPOL() (SSPCR0 |= _BV(6))
#define SpiClrCPOL() (SSPCR0 &= ~(_BV(6)))
#define SpiSetCPHA() (SSPCR0 |= _BV(7))
#define SpiClrCPHA() (SSPCR0 &= ~(_BV(7)))
*/
#endif // SPI_ARCH_H
sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.c
+92
View File
@@ -0,0 +1,92 @@
#include "subsystems/imu.h"
#include <stm32/gpio.h>
#include <stm32/misc.h>
#include <stm32/rcc.h>
#include <stm32/exti.h>
#include <stm32/spi.h>
#include <stm32/dma.h>
// gyro interupt handler
void exti15_10_irq_handler(void);
void imu_aspirin2_arch_int_enable(void) {
/*
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
*/
}
void imu_aspirin2_arch_int_disable(void) {
/*
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
*/
}
void imu_aspirin2_arch_init(void) {
/*
GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
SPI_InitTypeDef SPI_InitStructure;
// Set "mag ss" and "mag reset" as floating inputs ------------------------
// "mag ss" (PC12) is shorted to I2C2 SDA
// "mag reset" (PC13) is shorted to I2C2 SCL
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Gyro --------------------------------------------------------------------
// set "eeprom ss" as floating input (on PC14) = gyro int ---------
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// configure external interrupt exti15_10 on PC14( gyro int )
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// ifdef ASPIRIN_USE_GYRO_INT
GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
EXTI_InitStructure.EXTI_Line = EXTI_Line14;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
*/
}
// Gyro data ready
void exti15_10_irq_handler(void) {
// clear EXTI
if(EXTI_GetITStatus(EXTI_Line14) != RESET)
EXTI_ClearITPendingBit(EXTI_Line14);
/*
imu_aspirin.gyro_eoc = TRUE;
imu_aspirin.status = AspirinStatusReadingGyro;
*/
}
sw/airborne/arch/stm32/subsystems/imu/imu_aspirin2_arch.h
+20
View File
@@ -0,0 +1,20 @@
#ifndef IMU_ASPIRIN2_ARCH_H
#define IMU_ASPIRIN2_ARCH_H
#include "subsystems/imu.h"
#include <stm32/gpio.h>
extern void imu_aspirin2_arch_int_enable(void);
extern void imu_aspirin2_arch_int_disable(void);
/*
static inline int imu_aspirin2_eoc(void)
{
return !GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_14);
}
*/
#endif
sw/airborne/mcu_periph/spi.h
+17
View File
@@ -32,8 +32,25 @@
#ifdef USE_SPI
#include "std.h"
enum SPITransactionStatus {
SPITransPending,
SPITransRunning,
SPITransSuccess,
SPITransFailed
};
struct spi_transaction {
volatile uint8_t* mosi_buf;
volatile uint8_t* miso_buf;
volatile uint8_t* ready;
uint8_t length;
volatile enum SPITransactionStatus status;
};
#include "mcu_periph/spi_arch.h"
extern uint8_t* spi_buffer_input;
extern uint8_t* spi_buffer_output;
extern uint8_t spi_buffer_length;
sw/airborne/modules/sensors/imu_aspirin2.c
+272
View File
@@ -0,0 +1,272 @@
/*
* 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.
*
*/
#include <math.h>
#include "imu_aspirin2.h"
#include "mcu_periph/i2c.h"
#include "led.h"
// Downlink
#include "mcu_periph/uart.h"
#include "messages.h"
#include "downlink.h"
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
// Peripherials
#include "../../peripherals/mpu60X0.h"
// #include "../../peripherals/hmc5843.h"
// Results
volatile bool_t mag_valid;
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
// Communication
struct i2c_transaction aspirin2_mpu60x0;
// Standalone option: run module only
#ifndef IMU_TYPE_H
struct Imu imu;
#endif
#ifndef PERIODIC_FREQUENCY
#define PERIODIC_FREQUENCY 60
#endif
void imu_impl_init(void)
{
/////////////////////////////////////////////////////////////////////
// MPU60X0
aspirin2_mpu60x0.type = I2CTransTx;
aspirin2_mpu60x0.slave_addr = MPU60X0_ADDR;
aspirin2_mpu60x0.len_r = 0;
aspirin2_mpu60x0.len_w = 2;
///////////////////
// Configure power:
// MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
// MPU60X0_REG_USER_CTRL:
// -Enable Aux I2C Master Mode
// -Enable SPI
// MPU60X0_REG_PWR_MGMT_1
// -switch to gyroX clock
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_PWR_MGMT_1;
aspirin2_mpu60x0.buf[1] = 0x01;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status == I2CTransPending);
// MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
/////////////////////////
// Measurement Settings
// MPU60X0_REG_CONFIG
// -ext sync on gyro X (bit 3->6)
// -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
#if PERIODIC_FREQUENCY == 60
#else
# if PERIODIC_FREQUENCY == 120
# else
# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
# endif
#endif
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_CONFIG;
aspirin2_mpu60x0.buf[1] = (2 << 3) | (3 << 0);
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status == I2CTransPending);
// MPU60X0_REG_SMPLRT_DIV
// -100Hz output = 1kHz / (9 + 1)
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_SMPLRT_DIV;
aspirin2_mpu60x0.buf[1] = 9;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status == I2CTransPending);
// MPU60X0_REG_GYRO_CONFIG
// -2000deg/sec
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_GYRO_CONFIG;
aspirin2_mpu60x0.buf[1] = (3<<3);
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status == I2CTransPending);
// MPU60X0_REG_ACCEL_CONFIG
// 16g, no HPFL
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_ACCEL_CONFIG;
aspirin2_mpu60x0.buf[1] = (3<<3);
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status == I2CTransPending);
/*
// no interrupts for now, but set data ready interrupt to enable reading status bits
aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG;
aspirin2_mpu60x0.buf[1] = 0x01;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status == I2CTransPending);
*/
/*
/////////////////////////////////////////////////////////////////////
// HMC5843
ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
ppzuavimu_hmc5843.buf[1] = 0x01<<5;
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
ppzuavimu_hmc5843.buf[1] = 0x00;
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
*/
}
void imu_periodic( void )
{
// Start reading the latest gyroscope data
aspirin2_mpu60x0.type = I2CTransTxRx;
aspirin2_mpu60x0.len_r = 21;
aspirin2_mpu60x0.len_w = 1;
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_INT_STATUS;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &aspirin2_mpu60x0);
/*
// Start reading the latest accelerometer data
ppzuavimu_adxl345.type = I2CTransTxRx;
ppzuavimu_adxl345.len_r = 6;
ppzuavimu_adxl345.len_w = 1;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
*/
// Start reading the latest magnetometer data
#if PERIODIC_FREQUENCY > 60
RunOnceEvery(2,{
#endif
/* ppzuavimu_hmc5843.type = I2CTransTxRx;
ppzuavimu_hmc5843.len_r = 6;
ppzuavimu_hmc5843.len_w = 1;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
*/
#if PERIODIC_FREQUENCY > 60
});
#endif
//RunOnceEvery(10,aspirin2_subsystem_downlink_raw());
}
void aspirin2_subsystem_downlink_raw( void )
{
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
}
void aspirin2_subsystem_event( void )
{
int32_t x, y, z;
// If the itg3200 I2C transaction has succeeded: convert the data
if (aspirin2_mpu60x0.status == I2CTransSuccess)
{
#define MPU_OFFSET_GYRO 9
x = (int16_t) ((aspirin2_mpu60x0.buf[0+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[1+MPU_OFFSET_GYRO]);
y = (int16_t) ((aspirin2_mpu60x0.buf[2+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[3+MPU_OFFSET_GYRO]);
z = (int16_t) ((aspirin2_mpu60x0.buf[4+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[5+MPU_OFFSET_GYRO]);
RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
#define MPU_OFFSET_ACC 1
x = (int16_t) ((aspirin2_mpu60x0.buf[0+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[1+MPU_OFFSET_ACC]);
y = (int16_t) ((aspirin2_mpu60x0.buf[2+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[3+MPU_OFFSET_ACC]);
z = (int16_t) ((aspirin2_mpu60x0.buf[4+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[5+MPU_OFFSET_ACC]);
VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
// Is this is new data
if (aspirin2_mpu60x0.buf[0] & 0x01)
{
gyr_valid = TRUE;
acc_valid = TRUE;
}
else
{
}
aspirin2_mpu60x0.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
}
/*
// If the adxl345 I2C transaction has succeeded: convert the data
if (ppzuavimu_adxl345.status == I2CTransSuccess)
{
x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
#ifdef ASPIRIN_IMU
VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
#else // PPZIMU
VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
#endif
acc_valid = TRUE;
ppzuavimu_adxl345.status = I2CTransDone;
}
// If the hmc5843 I2C transaction has succeeded: convert the data
if (ppzuavimu_hmc5843.status == I2CTransSuccess)
{
x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
#ifdef ASPIRIN_IMU
VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
#else // PPZIMU
VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
#endif
mag_valid = TRUE;
ppzuavimu_hmc5843.status = I2CTransDone;
}
*/
}
sw/airborne/modules/sensors/imu_aspirin2.h
+57
View File
@@ -0,0 +1,57 @@
/*
* 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 PPZUAVIMU_H
#define PPZUAVIMU_H
#include "std.h"
#include "subsystems/imu.h"
extern volatile bool_t gyr_valid;
extern volatile bool_t acc_valid;
extern volatile bool_t mag_valid;
/* must be defined in order to be IMU code: declared in imu.h
extern void imu_impl_init(void);
extern void imu_periodic(void);
*/
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
aspirin2_subsystem_event(); \
if (gyr_valid) { \
gyr_valid = FALSE; \
_gyro_handler(); \
} \
if (acc_valid) { \
acc_valid = FALSE; \
_accel_handler(); \
} \
if (mag_valid) { \
mag_valid = FALSE; \
_mag_handler(); \
} \
}
/* Own Extra Functions */
extern void aspirin2_subsystem_event( void );
extern void aspirin2_subsystem_downlink_raw( void );
#endif // PPZUAVIMU_H
sw/airborne/peripherals/mpu60X0.h
+97
View File
@@ -0,0 +1,97 @@
#ifndef MPU60X0
#define MPU60X0
/* default I2C address */
#define MPU60X0_ADDR 0xD0
#define MPU60X0_ADDR_ALT 0xD2
#define MPU60X0_SPI_READ 0x80
// Power and Interface
#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
#define MPU60X0_REG_USER_CTRL 0x6A
#define MPU60X0_REG_PWR_MGMT_1 0x6B
#define MPU60X0_REG_PWR_MGMT_2 0x6C
// FIFO
#define MPU60X0_REG_FIFO_EN 0X23
#define MPU60X0_REG_FIFO_COUNT_H 0x72
#define MPU60X0_REG_FIFO_COUNT_L 0x73
#define MPU60X0_REG_FIFO_R_W 0x74
// Measurement Settings
#define MPU60X0_REG_SMPLRT_DIV 0X19
#define MPU60X0_REG_CONFIG 0X1A
#define MPU60X0_REG_GYRO_CONFIG 0X1B
#define MPU60X0_REG_ACCEL_CONFIG 0X1C
// I2C Slave settings
#define MPU60X0_REG_I2C_MST_CTRL 0X24
#define MPU60X0_REG_I2C_MST_STATUS 0X36
#define MPU60X0_REG_I2C_MST_DELAY 0X67
// Slave 0
#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
// Slave 1
#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
// Slave 2
#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
// Slave 3
#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
// Slave 4 - special
#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
// Interrupt
#define MPU60X0_REG_INT_PIN_CFG 0X37
#define MPU60X0_REG_INT_ENABLE 0X38
#define MPU60X0_REG_INT_STATUS 0X3A
// Accelero
#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
// Temperature
#define MPU60X0_REG_TEMP_OUT_H 0X41
#define MPU60X0_REG_TEMP_OUT_L 0X42
// Gyro
#define MPU60X0_REG_GYRO_XOUT_H 0X43
#define MPU60X0_REG_GYRO_XOUT_L 0X44
#define MPU60X0_REG_GYRO_YOUT_H 0X45
#define MPU60X0_REG_GYRO_YOUT_L 0X46
#define MPU60X0_REG_GYRO_ZOUT_H 0X47
#define MPU60X0_REG_GYRO_ZOUT_L 0X48
// External Sensor Data
#define MPU60X0_EXT_SENS_DATA 0X49
#define MPU60X0_EXT_SENS_DATA_SIZE 24
/////////////////////////////////////////////////
// MPU60X0 Definitions
#define MPU60X0_REG_WHO_AM_I 0X75
#define MPU60X0_WHOAMI_REPLY 0x68
#endif /* MPU60X0 */
sw/airborne/subsystems/imu/imu_aspirin2.c
+181
View File
@@ -0,0 +1,181 @@
#include "subsystems/imu.h"
#include "led.h"
#include "mcu_periph/spi.h"
#include "mcu_periph/spi_arch.h"
// Peripherials
#include "../../peripherals/mpu60X0.h"
struct ImuAspirin2 imu_aspirin2;
struct spi_transaction aspirin2_mpu60x0;
// initialize peripherals
static void configure(void);
/*
static void configure_accel(void);
//static void configure_mag(void);
*/
void imu_impl_init(void) {
imu_aspirin2.status = Aspirin2StatusUninit;
imu_aspirin2.imu_available = FALSE;
aspirin2_mpu60x0.mosi_buf = imu_aspirin2.imu_tx_buf;
aspirin2_mpu60x0.miso_buf = imu_aspirin2.imu_rx_buf;
aspirin2_mpu60x0.ready = &(imu_aspirin2.imu_available);
aspirin2_mpu60x0.length = 2;
// imu_aspirin2_arch_init();
}
void imu_periodic(void)
{
if (imu_aspirin2.status == Aspirin2StatusUninit)
{
configure();
// imu_aspirin_arch_int_enable();
imu_aspirin2.status = Aspirin2StatusIdle;
aspirin2_mpu60x0.length = 22;
aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
{
for (int i=1;i<aspirin2_mpu60x0.length;i++)
aspirin2_mpu60x0.mosi_buf[i] = 0;
}
}
else
{
// imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + MPU60X0_SPI_READ;
// imu_aspirin2.imu_tx_buf[1] = 0x00;
spi_rw(&aspirin2_mpu60x0);
/*
imu_aspirin.time_since_last_reading++;
imu_aspirin.time_since_last_accel_reading++;
if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT)
{
configure_accel();
imu_aspirin.time_since_last_accel_reading=0;
}
*/
}
}
static void configure(void)
{
aspirin2_mpu60x0.length = 2;
///////////////////
// Configure power:
// MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
// MPU60X0_REG_USER_CTRL:
// -Enable Aux I2C Master Mode
// -Enable SPI
// MPU60X0_REG_PWR_MGMT_1
// -switch to gyroX clock
aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_PWR_MGMT_1;
aspirin2_mpu60x0.mosi_buf[1] = 0x01;
spi_rw(&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status != SPITransSuccess);
// MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
/////////////////////////
// Measurement Settings
// MPU60X0_REG_CONFIG
// -ext sync on gyro X (bit 3->6)
// -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
#if PERIODIC_FREQUENCY == 60
#else
# if PERIODIC_FREQUENCY == 120
# else
# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
# endif
#endif
aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_CONFIG;
aspirin2_mpu60x0.mosi_buf[1] = (2 << 3) | (3 << 0);
spi_rw(&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status != SPITransSuccess);
// MPU60X0_REG_SMPLRT_DIV
// -100Hz output = 1kHz / (9 + 1)
aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_SMPLRT_DIV;
aspirin2_mpu60x0.mosi_buf[1] = 9;
spi_rw(&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status != SPITransSuccess);
// MPU60X0_REG_GYRO_CONFIG
// -2000deg/sec
aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_GYRO_CONFIG;
aspirin2_mpu60x0.mosi_buf[1] = (3<<3);
spi_rw(&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status != SPITransSuccess);
// MPU60X0_REG_ACCEL_CONFIG
// 16g, no HPFL
aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_ACCEL_CONFIG;
aspirin2_mpu60x0.mosi_buf[1] = (3<<3);
spi_rw(&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status != SPITransSuccess);
/*
struct i2c_transaction t;
t.type = I2CTransTx;
t.slave_addr = ITG3200_ADDR;
// set gyro range to 2000deg/s and low pass at 256Hz
t.mosi_buf[0] = ITG3200_REG_DLPF_FS;
t.mosi_buf[1] = (0x03<<3);
t.len_w = 2;
send_i2c_msg_with_retry(&t);
// set sample rate to 533Hz
t.mosi_buf[0] = ITG3200_REG_SMPLRT_DIV;
t.mosi_buf[1] = 0x0E;
send_i2c_msg_with_retry(&t);
// switch to gyroX clock
t.mosi_buf[0] = ITG3200_REG_PWR_MGM;
t.mosi_buf[1] = 0x01;
send_i2c_msg_with_retry(&t);
// enable interrupt on data ready, idle high, latch until read any register
t.mosi_buf[0] = ITG3200_REG_INT_CFG;
t.mosi_buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
send_i2c_msg_with_retry(&t);
*/
}
/*
static void configure_accel(void)
{
// set data rate to 800Hz
adxl345_write_to_reg(ADXL345_REG_BW_RATE, 0x0D);
// switch to measurememnt mode
adxl345_write_to_reg(ADXL345_REG_POWER_CTL, 1<<3);
// enable data ready interrupt
adxl345_write_to_reg(ADXL345_REG_INT_ENABLE, 1<<7);
// Enable full res and interrupt active low
adxl345_write_to_reg(ADXL345_REG_DATA_FORMAT, 1<<3|1<<5);
// clear spi rx reg to make DMA happy
adxl345_clear_rx_buf();
// reads data once to bring interrupt line up
adxl345_start_reading_data();
}
*/
sw/airborne/subsystems/imu/imu_aspirin2.h
+205
View File
@@ -0,0 +1,205 @@
/*
* $Id$
*
* Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef IMU_ASPIRIN_H
#define IMU_ASPIRIN_H
#include "generated/airframe.h"
#include "subsystems/imu.h"
#ifdef IMU_ASPIRIN_VERSION_2_0
#define IMU_MAG_X_CHAN 2
#define IMU_MAG_Y_CHAN 0
#define IMU_MAG_Z_CHAN 1
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#endif
#endif
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range
* sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/16.4 * pi/180 * 4096 = 4.359066229
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS 4.359
#define IMU_GYRO_P_SENS_NUM 4359
#define IMU_GYRO_P_SENS_DEN 1000
#define IMU_GYRO_Q_SENS 4.359
#define IMU_GYRO_Q_SENS_NUM 4359
#define IMU_GYRO_Q_SENS_DEN 1000
#define IMU_GYRO_R_SENS 4.359
#define IMU_GYRO_R_SENS_NUM 4359
#define IMU_GYRO_R_SENS_DEN 1000
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#endif
/** default accel sensitivy from the datasheet
* MPU60X0 has 2048 LSB/g
* fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC
* sens = 9.81 / 2048 * 1024 = 4.905
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS 4.905
#define IMU_ACCEL_X_SENS_NUM 4905
#define IMU_ACCEL_X_SENS_DEN 1000
#define IMU_ACCEL_Y_SENS 4.905
#define IMU_ACCEL_Y_SENS_NUM 4905
#define IMU_ACCEL_Y_SENS_DEN 1000
#define IMU_ACCEL_Z_SENS 4.905
#define IMU_ACCEL_Z_SENS_NUM 4905
#define IMU_ACCEL_Z_SENS_DEN 1000
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 0
#define IMU_ACCEL_Y_NEUTRAL 0
#define IMU_ACCEL_Z_NEUTRAL 0
#endif
enum Aspirin2Status
{ Aspirin2StatusUninit,
Aspirin2StatusIdle,
Aspirin2StatusReading
};
struct ImuAspirin2 {
volatile enum Aspirin2Status status;
volatile uint8_t imu_available;
volatile uint8_t imu_tx_buf[64];
volatile uint8_t imu_rx_buf[64];
uint32_t time_since_last_reading;
};
extern struct ImuAspirin2 imu_aspirin2;
#define ASPIRIN2_TIMEOUT 3
/*
#define foo_handler() {}
#define ImuMagEvent(_mag_handler) { \
MagEvent(foo_handler); \
}
if (hmc5843.data_available) { \
imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; \
imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN]; \
imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN]; \
_mag_handler(); \
hmc5843.data_available = FALSE; \
} \
*/
/* underlying architecture */
#include "subsystems/imu/imu_aspirin2_arch.h"
/* must be implemented by underlying architecture */
extern void imu_aspirin2_arch_init(void);
static inline void imu_from_buff(void)
{
int32_t x, y, z, p, q, r;
// If the itg3200 I2C transaction has succeeded: convert the data
#define MPU_OFFSET_GYRO 10
p = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_GYRO]);
q = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_GYRO]);
r = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_GYRO]);
#define MPU_OFFSET_ACC 2
x = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_ACC]);
y = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_ACC]);
z = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_ACC]);
#ifdef LISA_M_LONGITUDINAL_X
RATES_ASSIGN(imu.gyro_unscaled, q, -p, r);
VECT3_ASSIGN(imu.accel_unscaled, y, -x, z);
#else
RATES_ASSIGN(imu.gyro_unscaled, p, q, r);
VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
#endif
// Is this is new data
#define MPU_OFFSET_STATUS 1
if (imu_aspirin2.imu_rx_buf[MPU_OFFSET_STATUS] & 0x01)
{
//gyr_valid = TRUE;
//acc_valid = TRUE;
}
else
{
}
}
static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
if (imu_aspirin2.status == Aspirin2StatusUninit) return;
// imu_aspirin2_arch_int_disable();
if (imu_aspirin2.imu_available)
{
imu_aspirin2.time_since_last_reading = 0;
imu_aspirin2.imu_available = FALSE;
imu_from_buff();
_gyro_handler();
_accel_handler();
}
// imu_aspirin2_arch_int_enable();
// Reset everything if we've been waiting too long
//if (imu_aspirin2.time_since_last_reading > ASPIRIN2_TIMEOUT) {
// imu_aspirin2.time_since_last_reading = 0;
// return;
//}
}
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \
}
#endif /* IMU_ASPIRIN_H */