forked from paparazzi/paparazzi
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[apogee] add apogee imu based on mpu60x0
- Loading branch information
1 parent
aaa7c3e
commit 19196c4
Showing
6 changed files
with
331 additions
and
20 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,27 @@ | ||
| # Hey Emacs, this is a -*- makefile -*- | ||
| # | ||
| # Apogee IMU | ||
| # | ||
|
|
||
| IMU_APOGEE_CFLAGS = -DUSE_IMU | ||
| IMU_APOGEE_CFLAGS += -DIMU_TYPE_H=\"boards/apogee/imu_apogee.h\" | ||
|
|
||
| IMU_APOGEE_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ | ||
| $(SRC_BOARD)/imu_apogee.c | ||
|
|
||
| IMU_APOGEE_I2C_DEV=i2c1 | ||
| IMU_APOGEE_CFLAGS += -DUSE_I2C -DUSE_I2C1 | ||
|
|
||
| IMU_APOGEE_CFLAGS += -DIMU_APOGEE_I2C_DEV=$(IMU_APOGEE_I2C_DEV) | ||
| IMU_APOGEE_SRCS += peripherals/mpu60x0_i2c.c | ||
|
|
||
| # with default APOGEE_SMPLRT_DIV (gyro output 100Hz) | ||
| # the AHRS_PROPAGATE_FREQUENCY needs to be adjusted accordingly | ||
| AHRS_PROPAGATE_FREQUENCY ?= 100 | ||
| AHRS_CORRECT_FREQUENCY ?= 100 | ||
| ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) | ||
| ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) | ||
|
|
||
| ap.CFLAGS += $(IMU_APOGEE_CFLAGS) | ||
| ap.srcs += $(IMU_APOGEE_SRCS) | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,106 @@ | ||
| /* | ||
| * Copyright (C) 2013 Gautier Hattenberger | ||
| * | ||
| * This file is part of paparazzi. | ||
| * | ||
| * paparazzi is free software; you can redistribute it and/or modify | ||
| * it under the terms of the GNU General Public License as published by | ||
| * the Free Software Foundation; either version 2, or (at your option) | ||
| * any later version. | ||
| * | ||
| * paparazzi is distributed in the hope that it will be useful, | ||
| * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| * GNU General Public License for more details. | ||
| * | ||
| * You should have received a copy of the GNU General Public License | ||
| * along with paparazzi; see the file COPYING. If not, write to | ||
| * the Free Software Foundation, 59 Temple Place - Suite 330, | ||
| * Boston, MA 02111-1307, USA. | ||
| */ | ||
|
|
||
| /** | ||
| * @file boards/apogee/imu_apogee.c | ||
| * | ||
| * Driver for the IMU on the Apogee board. | ||
| * | ||
| * Invensense MPU-6050 | ||
| */ | ||
|
|
||
| #include <math.h> | ||
| #include "boards/apogee/imu_apogee.h" | ||
| #include "mcu_periph/i2c.h" | ||
| #include "led.h" | ||
|
|
||
| // Downlink | ||
| #include "mcu_periph/uart.h" | ||
| #include "messages.h" | ||
| #include "subsystems/datalink/downlink.h" | ||
|
|
||
| #ifndef DOWNLINK_DEVICE | ||
| #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE | ||
| #endif | ||
|
|
||
| #if !defined APOGEE_LOWPASS_FILTER && !defined APOGEE_SMPLRT_DIV | ||
| #define APOGEE_LOWPASS_FILTER MPU60X0_DLPF_42HZ | ||
| #define APOGEE_SMPLRT_DIV 9 | ||
| PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz") | ||
| #endif | ||
| PRINT_CONFIG_VAR(APOGEE_SMPLRT_DIV) | ||
| PRINT_CONFIG_VAR(APOGEE_LOWPASS_FILTER) | ||
|
|
||
| #ifndef APOGEE_GYRO_RANGE | ||
| #define APOGEE_GYRO_RANGE MPU60X0_GYRO_RANGE_1000 | ||
| #endif | ||
| PRINT_CONFIG_VAR(APOGEE_GYRO_RANGE) | ||
|
|
||
| #ifndef APOGEE_ACCEL_RANGE | ||
| #define APOGEE_ACCEL_RANGE MPU60X0_ACCEL_RANGE_4G | ||
| #endif | ||
| PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE) | ||
|
|
||
| struct ImuApogee imu_apogee; | ||
|
|
||
| void imu_impl_init(void) | ||
| { | ||
| ///////////////////////////////////////////////////////////////////// | ||
| // MPU-60X0 | ||
| mpu60x0_i2c_init(&imu_apogee.mpu, &(IMU_APOGEE_I2C_DEV), MPU60X0_ADDR_ALT); | ||
| // change the default configuration | ||
| imu_apogee.mpu.smplrt_div = APOGEE_SMPLRT_DIV; | ||
| imu_apogee.mpu.dlpf_cfg = APOGEE_LOWPASS_FILTER; | ||
| imu_apogee.mpu.config.gyro_range = APOGEE_GYRO_RANGE; | ||
| imu_apogee.mpu.config.accel_range = APOGEE_ACCEL_RANGE; | ||
|
|
||
| imu_apogee.gyr_valid = FALSE; | ||
| imu_apogee.acc_valid = FALSE; | ||
| } | ||
|
|
||
| void imu_periodic( void ) | ||
| { | ||
| // Start reading the latest gyroscope data | ||
| mpu60x0_i2c_periodic(&imu_apogee.imu); | ||
|
|
||
| //RunOnceEvery(10,imu_apogee_downlink_raw()); | ||
| } | ||
|
|
||
| void imu_apogee_downlink_raw( void ) | ||
| { | ||
| DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); | ||
| DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); | ||
| } | ||
|
|
||
|
|
||
| void imu_umarim_event( void ) | ||
| { | ||
| // If the itg3200 I2C transaction has succeeded: convert the data | ||
| mpu60x0_i2c_event(&imu_apogee.mpu); | ||
| if (imu_apogee.mpu.data_available) { | ||
| RATES_COPY(imu.gyro_unscaled, imu_apogee.mpu.data_rates.rates); | ||
| VECT3_COPY(imu.accel_unscaled, imu_apogee.mpu.data_accel.vect); | ||
| imu_apogee.mpu.data_available = FALSE; | ||
| imu_apogee.gyr_valid = TRUE; | ||
| imu_apogee.acc_valid = TRUE; | ||
| } | ||
| } | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,128 @@ | ||
| /* | ||
| * Copyright (C) 2011 Gautier Hattenberger | ||
| * | ||
| * This file is part of paparazzi. | ||
| * | ||
| * paparazzi is free software; you can redistribute it and/or modify | ||
| * it under the terms of the GNU General Public License as published by | ||
| * the Free Software Foundation; either version 2, or (at your option) | ||
| * any later version. | ||
| * | ||
| * paparazzi is distributed in the hope that it will be useful, | ||
| * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| * GNU General Public License for more details. | ||
| * | ||
| * You should have received a copy of the GNU General Public License | ||
| * along with paparazzi; see the file COPYING. If not, write to | ||
| * the Free Software Foundation, 59 Temple Place - Suite 330, | ||
| * Boston, MA 02111-1307, USA. | ||
| */ | ||
|
|
||
| /** | ||
| * @file boards/apogee/imu_apogee.h | ||
| * | ||
| * Driver for the IMU on the Apogee board. | ||
| * | ||
| * Invensense MPU-6050 | ||
| */ | ||
|
|
||
| #ifndef IMU_APOGEE_H | ||
| #define IMU_APOGEE_H | ||
|
|
||
| #include "std.h" | ||
| #include "generated/airframe.h" | ||
| #include "subsystems/imu.h" | ||
|
|
||
| #include "peripherals/mpu60x0_i2c.h" | ||
|
|
||
| // Default configuration | ||
| #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 | ||
| * MPU with 1000 deg/s has 32.8 LSB/(deg/s) | ||
| * sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC | ||
| * sens = 1/32.8 * pi/180 * 4096 = 2.17953 | ||
| I*/ | ||
| #if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS | ||
| // FIXME | ||
| #define IMU_GYRO_P_SENS 4.973 | ||
| #define IMU_GYRO_P_SENS_NUM 4973 | ||
| #define IMU_GYRO_P_SENS_DEN 1000 | ||
| #define IMU_GYRO_Q_SENS 4.973 | ||
| #define IMU_GYRO_Q_SENS_NUM 4973 | ||
| #define IMU_GYRO_Q_SENS_DEN 1000 | ||
| #define IMU_GYRO_R_SENS 4.973 | ||
| #define IMU_GYRO_R_SENS_NUM 4973 | ||
| #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 | ||
| * MPU with 8g has 4.096 LSB/g | ||
| * sens = 9.81 [m/s^2] / 4.096 [LSB/g] * 2^INT32_ACCEL_FRAC = 306.263 | ||
| */ | ||
| #if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS | ||
| // FIXME | ||
| #define IMU_ACCEL_X_SENS 37.91 | ||
| #define IMU_ACCEL_X_SENS_NUM 3791 | ||
| #define IMU_ACCEL_X_SENS_DEN 100 | ||
| #define IMU_ACCEL_Y_SENS 37.91 | ||
| #define IMU_ACCEL_Y_SENS_NUM 3791 | ||
| #define IMU_ACCEL_Y_SENS_DEN 100 | ||
| #define IMU_ACCEL_Z_SENS 39.24 | ||
| #define IMU_ACCEL_Z_SENS_NUM 39.24 | ||
| #define IMU_ACCEL_Z_SENS_DEN 100 | ||
| #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 | ||
|
|
||
| struct ImuApogee { | ||
| volatile bool_t gyr_valid; | ||
| volatile bool_t acc_valid; | ||
| struct Mpu60x0_I2c mpu; | ||
| }; | ||
|
|
||
| extern struct ImuApogee imu_apogee; | ||
|
|
||
|
|
||
| /* must be defined in order to be IMU code: declared in imu.h | ||
| extern void imu_impl_init(void); | ||
| extern void imu_periodic(void); | ||
| */ | ||
|
|
||
| /* Own Extra Functions */ | ||
| extern void imu_apogee_event( void ); | ||
| extern void imu_apogee_downlink_raw( void ); | ||
|
|
||
|
|
||
| static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) { | ||
| imu_apogee_event(); | ||
| if (imu_apogee.gyr_valid) { | ||
| imu_apogee.gyr_valid = FALSE; | ||
| _gyro_handler(); | ||
| } | ||
| if (imu_apogee.acc_valid) { | ||
| imu_apogee.acc_valid = FALSE; | ||
| _accel_handler(); | ||
| } | ||
| } | ||
|
|
||
| #endif // IMU_APOGEE_H |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.