From eda1ea3271ba421612cf5c616ea7af834cbd1c0b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 14 Jun 2013 12:29:08 +0200 Subject: [PATCH 01/36] [boards] add missing defines for lisa/l board --- sw/airborne/boards/lisa_l_1.0.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h index 28e199cbe0..f18b7d34eb 100644 --- a/sw/airborne/boards/lisa_l_1.0.h +++ b/sw/airborne/boards/lisa_l_1.0.h @@ -59,7 +59,7 @@ */ #define UART1_GPIO_AF 0 #define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX -#define UART1_GPIO_RX GPIO_BANK_USART1_RX +#define UART1_GPIO_RX GPIO_USART1_RX #define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX #define UART1_GPIO_TX GPIO_USART1_TX @@ -129,6 +129,7 @@ #define PPM_CC_IF TIM_SR_CC2IF #define PPM_GPIO_PORT GPIOA #define PPM_GPIO_PIN GPIO1 +#define PPM_GPIO_AF 0 /* ADC */ From 65d0889ad08fc5f693dfadea12128916385a933f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 14 Jun 2013 13:59:31 +0200 Subject: [PATCH 02/36] [rotorcraft] fix stabilization_attitude_read_rc_setpoint_eulers Reported by Ewoud. The constants max_phi_scale etc. were zero because it is an integer divide where the denominator is larger (depending on STABILIZATION_ATTITUDE_SP_MAX_PHI). This causes the euler setpoint to always be zero. Closes #461 --- .../stabilization_attitude_rc_setpoint.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index b7b216771e..1aadf44037 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -84,16 +84,16 @@ float stabilization_attitude_get_heading_f(void) { * @param[out] sp attitude setpoint as euler angles */ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) { - const int32_t max_phi_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) / MAX_PPRZ; - const int32_t max_theta_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) / MAX_PPRZ; - const int32_t max_r_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) / MAX_PPRZ; + const float max_phi_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) / MAX_PPRZ; + const float max_theta_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) / MAX_PPRZ; + const float max_r_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) / MAX_PPRZ; - sp->phi = ((int32_t) radio_control.values[RADIO_ROLL] * max_phi_scale); - sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * max_theta_scale); + sp->phi = (int32_t) (radio_control.values[RADIO_ROLL] * max_phi_scale); + sp->theta = (int32_t) (radio_control.values[RADIO_PITCH] * max_theta_scale); if (in_flight) { if (YAW_DEADBAND_EXCEEDED()) { - sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * max_r_scale / RC_UPDATE_FREQ); + sp->psi += (int32_t) (radio_control.values[RADIO_YAW] * max_r_scale / RC_UPDATE_FREQ); INT32_ANGLE_NORMALIZE(sp->psi); } if (autopilot_mode == AP_MODE_FORWARD) { From b97d8c22366bd29479e94cc3d5c49599299755bf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 10 May 2013 01:04:06 +0200 Subject: [PATCH 03/36] [peripherals][imu] i2c slaves for the mpu60x0 new IMU drivers using the mpu60x0 peripheral with slave support - aspirin 2 (spi) driver with mag as real slave - drotek 10dof v2 with mag as passthrough slave (untested and axes might be wrong) closes #450 closes #190 --- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 2 +- .../shared/imu_aspirin_v2.1.makefile | 29 ++- ...makefile => imu_aspirin_v2.1_old.makefile} | 29 +-- .../shared/imu_aspirin_v2.2.makefile | 39 +-- .../shared/imu_drotek_10dof_v2.makefile | 81 ++++++ sw/airborne/peripherals/mpu60x0.c | 36 ++- sw/airborne/peripherals/mpu60x0.h | 38 ++- sw/airborne/peripherals/mpu60x0_i2c.c | 90 ++++++- sw/airborne/peripherals/mpu60x0_i2c.h | 17 ++ sw/airborne/peripherals/mpu60x0_regs.h | 89 ++++--- sw/airborne/peripherals/mpu60x0_spi.c | 69 +++++- sw/airborne/peripherals/mpu60x0_spi.h | 16 +- sw/airborne/subsystems/imu/imu_aspirin_2.c | 121 --------- .../subsystems/imu/imu_aspirin_2_spi.c | 232 ++++++++++++++++++ .../subsystems/imu/imu_aspirin_2_spi.h | 86 +++++++ .../subsystems/imu/imu_drotek_10dof_v2.c | 126 ++++++++++ .../subsystems/imu/imu_drotek_10dof_v2.h | 88 +++++++ ...imu_aspirin_2.h => imu_mpu60x0_defaults.h} | 60 +---- 18 files changed, 942 insertions(+), 306 deletions(-) rename conf/firmwares/subsystems/shared/{imu_aspirin_v2.1_new.makefile => imu_aspirin_v2.1_old.makefile} (69%) create mode 100644 conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile delete mode 100644 sw/airborne/subsystems/imu/imu_aspirin_2.c create mode 100644 sw/airborne/subsystems/imu/imu_aspirin_2_spi.c create mode 100644 sw/airborne/subsystems/imu/imu_aspirin_2_spi.h create mode 100644 sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c create mode 100644 sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h rename sw/airborne/subsystems/imu/{imu_aspirin_2.h => imu_mpu60x0_defaults.h} (62%) diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 1f4331d59a..e9e9aa9dba 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -32,7 +32,7 @@ - + diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile index 446b84af59..1e16703269 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile @@ -39,32 +39,37 @@ # for fixedwing firmware and ap only ifeq ($(TARGET), ap) - IMU_ASPIRIN_CFLAGS = -DUSE_IMU + IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU endif -IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\" -IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ - $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c +IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\" +IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c +IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c +IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c + +# Magnetometer +#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c include $(CFG_SHARED)/spi_master.makefile ifeq ($(ARCH), lpc21) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI1 -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0 +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0 +IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0 +IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1 +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1 else ifeq ($(ARCH), stm32) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI2 +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2 # Slave select configuration # SLAVE2 is on PB12 (NSS) (MPU600 CS) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2 +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2 endif -IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1 - # 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) +ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS) +ap.srcs += $(IMU_ASPIRIN_2_SRCS) # diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile similarity index 69% rename from conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile rename to conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile index 3ad6e9a953..446b84af59 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile @@ -39,39 +39,32 @@ # for fixedwing firmware and ap only ifeq ($(TARGET), ap) - IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU + IMU_ASPIRIN_CFLAGS = -DUSE_IMU endif -IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2.h\" -IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c -IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2.c -IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c -IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c - -# Magnetometer -#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c +IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\" +IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c include $(CFG_SHARED)/spi_master.makefile ifeq ($(ARCH), lpc21) -IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0 -IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0 -IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1 -IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1 +IMU_ASPIRIN_CFLAGS += -DUSE_SPI1 +IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0 else ifeq ($(ARCH), stm32) -IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2 +IMU_ASPIRIN_CFLAGS += -DUSE_SPI2 # Slave select configuration # SLAVE2 is on PB12 (NSS) (MPU600 CS) -IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2 +IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2 endif -#IMU_ASPIRIN_2_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1 +IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1 # 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_2_CFLAGS) -ap.srcs += $(IMU_ASPIRIN_2_SRCS) +ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS) +ap.srcs += $(IMU_ASPIRIN_SRCS) # diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile index 8dfb93613a..99968ac5a2 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile @@ -2,11 +2,13 @@ # # Aspirin IMU v2.2 # +# actually identical with v2.1 since baro is not read in IMU driver +# # # required xml: #
# -# +# # # # @@ -37,37 +39,4 @@ # -# for fixedwing firmware and ap only -ifeq ($(TARGET), ap) - IMU_ASPIRIN_CFLAGS = -DUSE_IMU -endif - -IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\" -IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ - $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c - -include $(CFG_SHARED)/spi_master.makefile - -ifeq ($(ARCH), lpc21) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI1 -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0 -else ifeq ($(ARCH), stm32) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI2 -# Slave select configuration -# SLAVE2 is on PB12 (NSS) (MPU600 CS) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2 -endif - -IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_2 - -# 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) - - -# -# NPS simulator -# -include $(CFG_SHARED)/imu_nps.makefile +include $(CFG_SHARED)/imu_aspirin_v2.1.makefile diff --git a/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile new file mode 100644 index 0000000000..b41ec9b741 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile @@ -0,0 +1,81 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Drotek 10DOF V2 IMU via I2C +# +# +# required xml: +#
+# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +#
+# +# + + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_DROTEK_2_CFLAGS = -DUSE_IMU +endif + +IMU_DROTEK_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_drotek_10dof_v2.h\" +IMU_DROTEK_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_DROTEK_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_drotek_10dof_v2.c +IMU_DROTEK_2_SRCS += peripherals/mpu60x0.c +IMU_DROTEK_2_SRCS += peripherals/mpu60x0_i2c.c + +# Magnetometer +IMU_DROTEK_2_SRCS += peripherals/hmc58xx.c + + +# set default i2c bus +ifndef DROTEK_2_I2C_DEV +ifeq ($(ARCH), lpc21) +DROTEK_2_I2C_DEV=i2c0 +else ifeq ($(ARCH), stm32) +DROTEK_2_I2C_DEV=i2c2 +endif +endif + +# convert i2cx to upper case +DROTEK_2_I2C_DEV_UPPER=$(shell echo $(DROTEK_2_I2C_DEV) | tr a-z A-Z) + +IMU_DROTEK_2_CFLAGS += -DDROTEK_2_I2C_DEV=$(DROTEK_2_I2C_DEV) +IMU_DROTEK_2_CFLAGS += -DUSE_$(DROTEK_2_I2C_DEV_UPPER) + + +# 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_DROTEK_2_CFLAGS) +ap.srcs += $(IMU_DROTEK_2_SRCS) + + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c index 34598ac5c9..ea926b0125 100644 --- a/sw/airborne/peripherals/mpu60x0.c +++ b/sw/airborne/peripherals/mpu60x0.c @@ -31,18 +31,38 @@ void mpu60x0_set_default_config(struct Mpu60x0Config *c) { + c->clk_sel = MPU60X0_DEFAULT_CLK_SEL; c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV; c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG; c->gyro_range = MPU60X0_DEFAULT_FS_SEL; c->accel_range = MPU60X0_DEFAULT_AFS_SEL; - c->i2c_bypass = TRUE; c->drdy_int_enable = FALSE; - c->clk_sel = MPU60X0_DEFAULT_CLK_SEL; + + /* Number of bytes to read starting with MPU60X0_REG_INT_STATUS + * By default read only gyro and accel data -> 15 bytes. + * Increase to include slave data. + */ + c->nb_bytes = 15; + c->nb_slaves = 0; + + c->i2c_bypass = FALSE; } void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config) { switch (config->init_status) { + case MPU60X0_CONF_RESET: + /* device reset, set register values to defaults */ + mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, (1<<6)); + config->init_status++; + break; + case MPU60X0_CONF_USER_RESET: + /* trigger FIFO, I2C_MST and SIG_COND resets */ + mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_FIFO_RESET) | + (1 << MPU60X0_I2C_MST_RESET) | + (1 << MPU60X0_SIG_COND_RESET))); + config->init_status++; + break; case MPU60X0_CONF_PWR: /* switch to gyroX clock by default */ mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6))); @@ -68,9 +88,15 @@ void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Conf mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3)); config->init_status++; break; - case MPU60X0_CONF_I2C_BYPASS: - mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1)); - config->init_status++; + case MPU60X0_CONF_I2C_SLAVES: + /* if any, set MPU for I2C slaves and configure them*/ + if (config->nb_slaves > 0) { + /* returns TRUE when all slaves are configured */ + if (mpu60x0_configure_i2c_slaves(mpu_set, mpu)) + config->init_status++; + } + else + config->init_status++; break; case MPU60X0_CONF_INT_ENABLE: /* configure data ready interrupt */ diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index 60334d8bd2..00df171767 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -48,34 +48,62 @@ enum Mpu60x0ConfStatus { MPU60X0_CONF_UNINIT, + MPU60X0_CONF_RESET, + MPU60X0_CONF_USER_RESET, MPU60X0_CONF_PWR, MPU60X0_CONF_SD, MPU60X0_CONF_DLPF, MPU60X0_CONF_GYRO, MPU60X0_CONF_ACCEL, - MPU60X0_CONF_I2C_BYPASS, + MPU60X0_CONF_I2C_SLAVES, MPU60X0_CONF_INT_ENABLE, MPU60X0_CONF_DONE }; +/// Configuration function prototype +typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val); + +/// function prototype for configuration of a single I2C slave +typedef bool_t (*Mpu60x0I2cSlaveConfigure)(Mpu60x0ConfigSet mpu_set, void* mpu); + +struct Mpu60x0I2cSlave { + Mpu60x0I2cSlaveConfigure configure; +}; + struct Mpu60x0Config { uint8_t smplrt_div; ///< Sample rate divider enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter enum Mpu60x0GyroRanges gyro_range; ///< deg/s Range enum Mpu60x0AccelRanges accel_range; ///< g Range - bool_t i2c_bypass; ///< bypass mpu i2c bool_t drdy_int_enable; ///< Enable Data Ready Interrupt uint8_t clk_sel; ///< Clock select + uint8_t nb_bytes; ///< number of bytes to read starting with MPU60X0_REG_INT_STATUS enum Mpu60x0ConfStatus init_status; ///< init status bool_t initialized; ///< config done flag + + /** Bypass MPU I2C. + * Only effective if using the I2C implementation. + */ + bool_t i2c_bypass; + + uint8_t nb_slaves; ///< number of used I2C slaves + struct Mpu60x0I2cSlave slaves[5]; ///< I2C slaves + enum Mpu60x0MstClk i2c_mst_clk; ///< MPU I2C master clock speed + uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate }; extern void mpu60x0_set_default_config(struct Mpu60x0Config *c); -/// Configuration function prototype -typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val); - /// Configuration sequence called once before normal use extern void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config); +/** + * Configure I2C slaves of the MPU. + * This is I2C/SPI implementation specific. + * @param mpu_set configuration function + * @param mpu Mpu60x0Spi or Mpu60x0I2c peripheral + * @return TRUE when all slaves are configured + */ +extern bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu); + #endif // MPU60X0_H diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c index ada9ee4b55..4d25af6585 100644 --- a/sw/airborne/peripherals/mpu60x0_i2c.c +++ b/sw/airborne/peripherals/mpu60x0_i2c.c @@ -44,6 +44,8 @@ void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t mpu->data_available = FALSE; mpu->config.initialized = FALSE; mpu->config.init_status = MPU60X0_CONF_UNINIT; + + mpu->slave_init_status = MPU60X0_I2C_CONF_UNINIT; } @@ -70,7 +72,7 @@ void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu) if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) { /* set read bit and multiple byte bit, then address */ mpu->i2c_trans.buf[0] = MPU60X0_REG_INT_STATUS; - i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, 15); + i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes); } } @@ -84,14 +86,19 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) } else if (mpu->i2c_trans.status == I2CTransSuccess) { // Successfull reading - if (bit_is_set(mpu->i2c_trans.buf[0],0)) { + if (bit_is_set(mpu->i2c_trans.buf[0], 0)) { // new data - mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf,1); - mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf,3); - mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf,5); - mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf,9); - mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf,11); - mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf,13); + mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf, 1); + mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf, 3); + mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf, 5); + mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf, 9); + mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf, 11); + mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13); + + // if we are reading slaves, copy the ext_sens_data + if (mpu->config.nb_slaves > 0) + memcpy(mpu->data_ext, (void *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); + mpu->data_available = TRUE; } mpu->i2c_trans.status = I2CTransDone; @@ -103,12 +110,75 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) mpu->config.init_status--; // Retry config (TODO max retry) case I2CTransSuccess: case I2CTransDone: - mpu->i2c_trans.status = I2CTransDone; mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, &(mpu->config)); - if (mpu->config.initialized) mpu->i2c_trans.status = I2CTransDone; + if (mpu->config.initialized) + mpu->i2c_trans.status = I2CTransDone; break; default: break; } } } + +/** @todo: only one slave so far. */ +bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu) +{ + struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu); + + if (mpu_i2c->slave_init_status == MPU60X0_I2C_CONF_UNINIT) + mpu_i2c->slave_init_status++; + + switch (mpu_i2c->slave_init_status) { + case MPU60X0_I2C_CONF_I2C_MST_DIS: + mpu_set(mpu, MPU60X0_REG_USER_CTRL, 0); + mpu_i2c->slave_init_status++; + break; + case MPU60X0_I2C_CONF_I2C_BYPASS_EN: + /* switch to I2C passthrough */ + mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (1<<1)); + mpu_i2c->slave_init_status++; + break; + case MPU60X0_I2C_CONF_SLAVES_CONFIGURE: + /* configure each slave. TODO: currently only one */ + if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) + mpu_i2c->slave_init_status++; + break; + case MPU60X0_I2C_CONF_I2C_BYPASS_DIS: + if (mpu_i2c->config.i2c_bypass) { + /* if bypassing I2C skip MPU I2C master setup */ + mpu_i2c->slave_init_status = MPU60X0_I2C_CONF_DONE; + } + else { + /* disable I2C passthrough again */ + mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (0<<1)); + mpu_i2c->slave_init_status++; + } + break; + case MPU60X0_I2C_CONF_I2C_MST_CLK: + /* configure MPU I2C master clock and stop/start between slave reads */ + mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL, + ((1<<4) | mpu_i2c->config.i2c_mst_clk)); + mpu_i2c->slave_init_status++; + break; + case MPU60X0_I2C_CONF_I2C_MST_DELAY: + /* Set I2C slaves delayed sample rate */ + mpu_set(mpu, MPU60X0_REG_I2C_MST_DELAY, mpu_i2c->config.i2c_mst_delay); + mpu_i2c->slave_init_status++; + break; + case MPU60X0_I2C_CONF_I2C_SMPLRT: + /* I2C slave0 sample rate/2 = 100/2 = 50Hz */ + mpu_set(mpu, MPU60X0_REG_I2C_SLV4_CTRL, 0); + mpu_i2c->slave_init_status++; + break; + case MPU60X0_I2C_CONF_I2C_MST_EN: + /* enable internal I2C master */ + mpu_set(mpu, MPU60X0_REG_USER_CTRL, (1 << MPU60X0_I2C_MST_EN)); + mpu_i2c->slave_init_status++; + break; + case MPU60X0_I2C_CONF_DONE: + return TRUE; + default: + break; + } + return FALSE; +} diff --git a/sw/airborne/peripherals/mpu60x0_i2c.h b/sw/airborne/peripherals/mpu60x0_i2c.h index f7d95eff61..581ad60b45 100644 --- a/sw/airborne/peripherals/mpu60x0_i2c.h +++ b/sw/airborne/peripherals/mpu60x0_i2c.h @@ -36,6 +36,21 @@ #include "peripherals/mpu60x0.h" +#define MPU60X0_BUFFER_EXT_LEN 16 + +enum Mpu60x0I2cSlaveInitStatus { + MPU60X0_I2C_CONF_UNINIT, + MPU60X0_I2C_CONF_I2C_MST_DIS, + MPU60X0_I2C_CONF_I2C_BYPASS_EN, + MPU60X0_I2C_CONF_SLAVES_CONFIGURE, + MPU60X0_I2C_CONF_I2C_BYPASS_DIS, + MPU60X0_I2C_CONF_I2C_MST_CLK, + MPU60X0_I2C_CONF_I2C_MST_DELAY, + MPU60X0_I2C_CONF_I2C_SMPLRT, + MPU60X0_I2C_CONF_I2C_MST_EN, + MPU60X0_I2C_CONF_DONE +}; + struct Mpu60x0_I2c { struct i2c_periph *i2c_p; struct i2c_transaction i2c_trans; @@ -48,7 +63,9 @@ struct Mpu60x0_I2c { struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system int16_t value[3]; ///< rates data values accessible by channel index } data_rates; + uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN]; struct Mpu60x0Config config; + enum Mpu60x0I2cSlaveInitStatus slave_init_status; }; // Functions diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h index 211d1729f0..ac48968ab6 100644 --- a/sw/airborne/peripherals/mpu60x0_regs.h +++ b/sw/airborne/peripherals/mpu60x0_regs.h @@ -41,21 +41,21 @@ #define MPU60X0_REG_PWR_MGMT_2 0x6C // FIFO -#define MPU60X0_REG_FIFO_EN 0X23 +#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 +#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 +#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 @@ -84,44 +84,51 @@ #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 +#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 +#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 +#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 +#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 0x49 #define MPU60X0_EXT_SENS_DATA_SIZE 24 -///////////////////////////////////////////////// -// MPU60X0 Definitions - -#define MPU60X0_REG_WHO_AM_I 0X75 +#define MPU60X0_REG_WHO_AM_I 0x75 #define MPU60X0_WHOAMI_REPLY 0x68 // Bit positions #define MPU60X0_I2C_BYPASS_EN 1 + +// in MPU60X0_REG_USER_CTRL +#define MPU60X0_SIG_COND_RESET 0 +#define MPU60X0_I2C_MST_RESET 1 +#define MPU60X0_FIFO_RESET 2 +#define MPU60X0_I2C_IF_DIS 4 #define MPU60X0_I2C_MST_EN 5 +#define MPU60X0_FIFO_EN 6 + +// in MPU60X0_REG_I2C_MST_STATUS +#define MPU60X0_I2C_SLV4_DONE 6 /** Digital Low Pass Filter Options * DLFP is affecting both gyro and accels, @@ -157,4 +164,26 @@ enum Mpu60x0AccelRanges { MPU60X0_ACCEL_RANGE_16G = 0x03 }; +/** + * I2C Master clock + */ +enum Mpu60x0MstClk { + MPU60X0_MST_CLK_500KHZ = 0x9, + MPU60X0_MST_CLK_471KHZ = 0xA, + MPU60X0_MST_CLK_444KHZ = 0xB, + MPU60X0_MST_CLK_421KHZ = 0xC, + MPU60X0_MST_CLK_400KHZ = 0xD, + MPU60X0_MST_CLK_381KHZ = 0xE, + MPU60X0_MST_CLK_364KHZ = 0xF, + MPU60X0_MST_CLK_348KHZ = 0x0, + MPU60X0_MST_CLK_333KHZ = 0x1, + MPU60X0_MST_CLK_320KHZ = 0x2, + MPU60X0_MST_CLK_308KHZ = 0x3, + MPU60X0_MST_CLK_296KHZ = 0x4, + MPU60X0_MST_CLK_286KHZ = 0x5, + MPU60X0_MST_CLK_276KHZ = 0x6, + MPU60X0_MST_CLK_267KHZ = 0x7, + MPU60X0_MST_CLK_254KHZ = 0x8 +}; + #endif /* MPU60X0_REGS_H */ diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c index 97993f322e..0689e972ad 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.c +++ b/sw/airborne/peripherals/mpu60x0_spi.c @@ -42,7 +42,7 @@ void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t mpu->spi_trans.select = SPISelectUnselect; mpu->spi_trans.slave_idx = slave_idx; - mpu->spi_trans.output_length = MPU60X0_BUFFER_LEN; //FIXME + mpu->spi_trans.output_length = 2; mpu->spi_trans.input_length = MPU60X0_BUFFER_LEN; mpu->spi_trans.before_cb = NULL; mpu->spi_trans.after_cb = NULL; @@ -58,6 +58,8 @@ void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t mpu->data_available = FALSE; mpu->config.initialized = FALSE; mpu->config.init_status = MPU60X0_CONF_UNINIT; + + mpu->slave_init_status = MPU60X0_SPI_CONF_UNINIT; } @@ -85,9 +87,9 @@ void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu) { if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) { mpu->spi_trans.output_length = 1; - mpu->spi_trans.input_length = 16; // FIXME external data + mpu->spi_trans.input_length = 1 + mpu->config.nb_bytes; /* set read bit and multiple byte bit, then address */ - mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ; + mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS | MPU60X0_SPI_READ; spi_submit(mpu->spi_p, &(mpu->spi_trans)); } } @@ -102,14 +104,19 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) } else if (mpu->spi_trans.status == SPITransSuccess) { // Successfull reading - if (bit_is_set(mpu->rx_buf[1],0)) { + if (bit_is_set(mpu->rx_buf[1], 0)) { // new data - mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf,2); - mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf,4); - mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf,6); - mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf,10); - mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf,12); - mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf,14); + mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf, 2); + mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf, 4); + mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf, 6); + mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf, 10); + mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf, 12); + mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14); + + // if we are reading slaves, copy the ext_sens_data + if (mpu->config.nb_slaves > 0) + memcpy(mpu->data_ext, (void *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15); + mpu->data_available = TRUE; } mpu->spi_trans.status = SPITransDone; @@ -121,12 +128,50 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) mpu->config.init_status--; // Retry config (TODO max retry) case SPITransSuccess: case SPITransDone: - mpu->spi_trans.status = SPITransDone; mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, &(mpu->config)); - if (mpu->config.initialized) mpu->spi_trans.status = SPITransDone; + if (mpu->config.initialized) + mpu->spi_trans.status = SPITransDone; break; default: break; } } } + +/** @todo: only one slave so far. */ +bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu) +{ + struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu); + + if (mpu_spi->slave_init_status == MPU60X0_SPI_CONF_UNINIT) + mpu_spi->slave_init_status++; + + switch (mpu_spi->slave_init_status) { + case MPU60X0_SPI_CONF_I2C_MST_CLK: + /* configure MPU I2C master clock and stop/start between slave reads */ + mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL, ((1<<4) | mpu_spi->config.i2c_mst_clk)); + mpu_spi->slave_init_status++; + break; + case MPU60X0_SPI_CONF_I2C_MST_DELAY: + /* Set I2C slaves delayed sample rate */ + mpu_set(mpu, MPU60X0_REG_I2C_MST_DELAY, mpu_spi->config.i2c_mst_delay); + mpu_spi->slave_init_status++; + break; + case MPU60X0_SPI_CONF_I2C_MST_EN: + /* enable internal I2C master and disable primary I2C interface */ + mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_I2C_IF_DIS) | + (1 << MPU60X0_I2C_MST_EN))); + mpu_spi->slave_init_status++; + break; + case MPU60X0_SPI_CONF_SLAVES_CONFIGURE: + /* configure first slave, only one slave supported so far */ + if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) + mpu_spi->slave_init_status++; + break; + case MPU60X0_SPI_CONF_DONE: + return TRUE; + default: + break; + } + return FALSE; +} diff --git a/sw/airborne/peripherals/mpu60x0_spi.h b/sw/airborne/peripherals/mpu60x0_spi.h index 6eac22267c..75975f851b 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.h +++ b/sw/airborne/peripherals/mpu60x0_spi.h @@ -37,12 +37,22 @@ #define MPU60X0_BUFFER_LEN 32 +#define MPU60X0_BUFFER_EXT_LEN 16 + +enum Mpu60x0SpiSlaveInitStatus { + MPU60X0_SPI_CONF_UNINIT, + MPU60X0_SPI_CONF_I2C_MST_CLK, + MPU60X0_SPI_CONF_I2C_MST_DELAY, + MPU60X0_SPI_CONF_I2C_MST_EN, + MPU60X0_SPI_CONF_SLAVES_CONFIGURE, + MPU60X0_SPI_CONF_DONE +}; struct Mpu60x0_Spi { struct spi_periph *spi_p; struct spi_transaction spi_trans; - volatile uint8_t tx_buf[MPU60X0_BUFFER_LEN]; // FIXME correct length - volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; // FIXME idem + volatile uint8_t tx_buf[2]; + volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; volatile bool_t data_available; ///< data ready flag union { struct Int16Vect3 vect; ///< accel data vector in accel coordinate system @@ -52,7 +62,9 @@ struct Mpu60x0_Spi { struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system int16_t value[3]; ///< rates data values accessible by channel index } data_rates; + uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN]; struct Mpu60x0Config config; + enum Mpu60x0SpiSlaveInitStatus slave_init_status; }; // Functions diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2.c b/sw/airborne/subsystems/imu/imu_aspirin_2.c deleted file mode 100644 index 33a444456e..0000000000 --- a/sw/airborne/subsystems/imu/imu_aspirin_2.c +++ /dev/null @@ -1,121 +0,0 @@ -/* - * Copyright (C) 2013 Felix Ruess - * - * 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 subsystems/imu/imu_aspirin_2.c - * Driver for the Aspirin v2.x IMU using SPI for the MPU6000. - */ - -#include "subsystems/imu.h" - -#include "mcu_periph/i2c.h" -#include "mcu_periph/spi.h" - - -/* defaults suitable for Lisa */ -#ifndef ASPIRIN_2_SPI_SLAVE_IDX -#define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2 -#endif -PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX) - -#ifndef ASPIRIN_2_SPI_DEV -#define ASPIRIN_2_SPI_DEV spi2 -#endif -PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV) - -#ifndef ASPIRIN_2_I2C_DEV -#define ASPIRIN_2_I2C_DEV i2c2 -#endif -PRINT_CONFIG_VAR(ASPIRIN_2_I2C_DEV) - - -/* gyro internal lowpass frequency */ -#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV -#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ -#define ASPIRIN_2_SMPLRT_DIV 1 -PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz") -#endif -PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER) -PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV) - -#ifndef ASPIRIN_2_GYRO_RANGE -#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000 -#endif -PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE) - -#ifndef ASPIRIN_2_ACCEL_RANGE -#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G -#endif -PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE) - - -struct ImuAspirin2 imu_aspirin2; - -void imu_impl_init(void) -{ - imu_aspirin2.accel_valid = FALSE; - imu_aspirin2.gyro_valid = FALSE; - imu_aspirin2.mag_valid = FALSE; - - mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX); - // change the default configuration - imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV; - imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER; - imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE; - imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE; - //imu_aspirin2.mpu.config.i2c_bypass = FALSE; - //imu_aspirin2.mpu.config.drdy_int_enable = TRUE; - - //hmc58xx_init(&imu_aspirin2.mag_hmc, &(ASPIRIN_2_I2C_DEV), HMC58XX_ADDR); -} - - -void imu_periodic(void) -{ - mpu60x0_spi_periodic(&imu_aspirin2.mpu); - - // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz) - //RunOnceEvery(10, hmc58xx_periodic(&imu_aspirin2.mag_hmc)); -} - -void imu_aspirin2_event(void) -{ - mpu60x0_spi_event(&imu_aspirin2.mpu); - if (imu_aspirin2.mpu.data_available) { - RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); - VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); - imu_aspirin2.mpu.data_available = FALSE; - imu_aspirin2.gyro_valid = TRUE; - imu_aspirin2.accel_valid = TRUE; - } - -#if 0 - /* HMC58XX event task */ - hmc58xx_event(&imu_aspirin2.mag_hmc); - if (imu_aspirin2.mag_hmc.data_available) { - imu.mag_unscaled.x = imu_aspirin2.mag_hmc.data.vect.y; - imu.mag_unscaled.y = -imu_aspirin2.mag_hmc.data.vect.x; - imu.mag_unscaled.z = imu_aspirin2.mag_hmc.data.vect.z; - imu_aspirin2.mag_hmc.data_available = FALSE; - imu_aspirin2.mag_valid = TRUE; - } -#endif -} diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c new file mode 100644 index 0000000000..84918649e4 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -0,0 +1,232 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 subsystems/imu/imu_aspirin_2_spi.c + * Driver for the Aspirin v2.x IMU using SPI for the MPU6000. + */ + +#include "subsystems/imu.h" + +#include "mcu_periph/spi.h" +#include "peripherals/hmc58xx_regs.h" + +/* defaults suitable for Lisa */ +#ifndef ASPIRIN_2_SPI_SLAVE_IDX +#define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2 +#endif +PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX) + +#ifndef ASPIRIN_2_SPI_DEV +#define ASPIRIN_2_SPI_DEV spi2 +#endif +PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV) + +/* gyro internal lowpass frequency */ +#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV +#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ +#define ASPIRIN_2_SMPLRT_DIV 1 +//PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz") +#endif +PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER) +PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV) + +#ifndef ASPIRIN_2_GYRO_RANGE +#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000 +#endif +PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE) + +#ifndef ASPIRIN_2_ACCEL_RANGE +#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G +#endif +PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE) + + +/* HMC58XX default conf */ +#ifndef HMC58XX_DO +#define HMC58XX_DO 0x6 // Data Output Rate (6 -> 75Hz with HMC5883) +#endif +#ifndef HMC58XX_MS +#define HMC58XX_MS 0x0 // Measurement configuration +#endif +#ifndef HMC58XX_GN +#define HMC58XX_GN 0x1 // Gain configuration (1 -> +- 1 Gauss) +#endif +#ifndef HMC58XX_MD +#define HMC58XX_MD 0x0 // Continious measurement mode +#endif + +#define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS)) +#define HMC58XX_CRB (HMC58XX_GN<<5) + + +struct ImuAspirin2Spi imu_aspirin2; + +void mpu_wait_slave4_ready(void); +void mpu_wait_slave4_ready_cb(struct spi_transaction * t); +bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu); + +void imu_impl_init(void) +{ + imu_aspirin2.accel_valid = FALSE; + imu_aspirin2.gyro_valid = FALSE; + imu_aspirin2.mag_valid = FALSE; + + mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX); + // change the default configuration + imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV; + imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER; + imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE; + imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE; + + /* read 15 bytes for status, accel, gyro + 6 bytes for mag slave */ + imu_aspirin2.mpu.config.nb_bytes = 21; + + /* HMC5883 magnetometer as I2C slave */ + imu_aspirin2.mpu.config.nb_slaves = 1; + + /* set function to configure mag */ + imu_aspirin2.mpu.config.slaves[0].configure = &imu_aspirin2_configure_mag_slave; + + + /* Set MPU I2C master clock */ + imu_aspirin2.mpu.config.i2c_mst_clk = MPU60X0_MST_CLK_400KHZ; + /* Enable I2C slave0 delayed sample rate */ + imu_aspirin2.mpu.config.i2c_mst_delay = 1; + + + /* configure spi transaction for wait_slave4 */ + imu_aspirin2.wait_slave4_trans.cpol = SPICpolIdleHigh; + imu_aspirin2.wait_slave4_trans.cpha = SPICphaEdge2; + imu_aspirin2.wait_slave4_trans.dss = SPIDss8bit; + imu_aspirin2.wait_slave4_trans.bitorder = SPIMSBFirst; + imu_aspirin2.wait_slave4_trans.cdiv = SPIDiv64; + + imu_aspirin2.wait_slave4_trans.select = SPISelectUnselect; + imu_aspirin2.wait_slave4_trans.slave_idx = ASPIRIN_2_SPI_SLAVE_IDX; + imu_aspirin2.wait_slave4_trans.output_length = 1; + imu_aspirin2.wait_slave4_trans.input_length = 2; + imu_aspirin2.wait_slave4_trans.before_cb = NULL; + imu_aspirin2.wait_slave4_trans.after_cb = mpu_wait_slave4_ready_cb; + imu_aspirin2.wait_slave4_trans.input_buf = &(imu_aspirin2.wait_slave4_rx_buf[0]); + imu_aspirin2.wait_slave4_trans.output_buf = &(imu_aspirin2.wait_slave4_tx_buf[0]); + + imu_aspirin2.wait_slave4_trans.status = SPITransDone; + imu_aspirin2.slave4_ready = FALSE; +} + + +void imu_periodic(void) +{ + mpu60x0_spi_periodic(&imu_aspirin2.mpu); +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1])) + +void imu_aspirin2_event(void) +{ + mpu60x0_spi_event(&imu_aspirin2.mpu); + if (imu_aspirin2.mpu.data_available) { + struct Int32Vect3 mag; + mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0); + mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); + mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); +#ifdef LISA_M_LONGITUDINAL_X + RATES_ASSIGN(imu.gyro_unscaled, + imu_aspirin2.mpu.data_rates.rates.q, + -imu_aspirin2.mpu.data_rates.rates.p, + imu_aspirin2.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, + imu_aspirin2.mpu.data_accel.vect.y, + -imu_aspirin2.mpu.data_accel.vect.x, + imu_aspirin2.mpu.data_accel.vect.z); + VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.z, mag.y); +#else + RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); + VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); + VECT3_ASSIGN(imu.mag_unscaled, mag.z, -mag.x, mag.y) +#endif + imu_aspirin2.mpu.data_available = FALSE; + imu_aspirin2.gyro_valid = TRUE; + imu_aspirin2.accel_valid = TRUE; + imu_aspirin2.mag_valid = TRUE; + } +} + +// hack with waiting to avoid creating another event loop to check the mag config status +static inline void mpu_set_and_wait(Mpu60x0ConfigSet mpu_set, void* mpu, uint8_t _reg, uint8_t _val) +{ + mpu_set(mpu, _reg, _val); + while(imu_aspirin2.mpu.spi_trans.status != SPITransSuccess); +} + +/** function to configure hmc5883 mag + * @return TRUE if mag configuration finished + */ +bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu) +{ + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable + + mpu_wait_slave4_ready(); + + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable + + mpu_wait_slave4_ready(); + + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable + + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ); + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM); + // Put the enable command as last. + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_CTRL, + (1 << 7) | // Slave 0 enable + (6 << 0) ); // Read 6 bytes + + return TRUE; +} + +void mpu_wait_slave4_ready(void) +{ + while (!imu_aspirin2.slave4_ready) { + if (imu_aspirin2.wait_slave4_trans.status == SPITransDone) { + imu_aspirin2.wait_slave4_tx_buf[0] = MPU60X0_REG_I2C_MST_STATUS | MPU60X0_SPI_READ; + spi_submit(imu_aspirin2.mpu.spi_p, &(imu_aspirin2.wait_slave4_trans)); + } + } +} + +void mpu_wait_slave4_ready_cb(struct spi_transaction * t) +{ + if (bit_is_set(t->input_buf[1], MPU60X0_I2C_SLV4_DONE)) + imu_aspirin2.slave4_ready = TRUE; + else + imu_aspirin2.slave4_ready = FALSE; + t->status = SPITransDone; +} diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h new file mode 100644 index 0000000000..9ab98631ca --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 subsystems/imu/imu_aspirin_2_spi.h + * Driver for the Aspirin v2.x IMU using SPI for the MPU6000. + */ + +#ifndef IMU_ASPIRIN_2_SPI_H +#define IMU_ASPIRIN_2_SPI_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "subsystems/imu/imu_mpu60x0_defaults.h" +#include "peripherals/mpu60x0_spi.h" + +#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 +#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 + +struct ImuAspirin2Spi { + volatile bool_t gyro_valid; + volatile bool_t accel_valid; + volatile bool_t mag_valid; + struct Mpu60x0_Spi mpu; + + struct spi_transaction wait_slave4_trans; + volatile uint8_t wait_slave4_tx_buf[1]; + volatile uint8_t wait_slave4_rx_buf[2]; + volatile bool_t slave4_ready; +}; + +extern struct ImuAspirin2Spi imu_aspirin2; + +extern void imu_aspirin2_event(void); + + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { + imu_aspirin2_event(); + if (imu_aspirin2.gyro_valid) { + imu_aspirin2.gyro_valid = FALSE; + _gyro_handler(); + } + if (imu_aspirin2.accel_valid) { + imu_aspirin2.accel_valid = FALSE; + _accel_handler(); + } + if (imu_aspirin2.mag_valid) { + imu_aspirin2.mag_valid = FALSE; + _mag_handler(); + } +} + +#endif /* IMU_ASPIRIN_2_H */ diff --git a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c new file mode 100644 index 0000000000..76c3d84f77 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 subsystems/imu/imu_drotek_10dof_v2.c + * + * Driver for the Drotek 10DOF V2 IMU. + * MPU6050 + HMC5883 + MS5611 + * + * @todo MS5611 baro not read yet + */ + +#include "subsystems/imu.h" + +#include "mcu_periph/i2c.h" + +#if !defined DROTEK_2_LOWPASS_FILTER && !defined DROTEK_2_SMPLRT_DIV +#define DROTEK_2_LOWPASS_FILTER MPU60X0_DLPF_42HZ +#define DROTEK_2_SMPLRT_DIV 9 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz") +#endif +PRINT_CONFIG_VAR(DROTEK_2_SMPLRT_DIV) +PRINT_CONFIG_VAR(DROTEK_2_LOWPASS_FILTER) + +#ifndef DROTEK_2_GYRO_RANGE +#define DROTEK_2_GYRO_RANGE MPU60X0_GYRO_RANGE_1000 +#endif +PRINT_CONFIG_VAR(DROTEK_2_GYRO_RANGE) + +#ifndef DROTEK_2_ACCEL_RANGE +#define DROTEK_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G +#endif +PRINT_CONFIG_VAR(DROTEK_2_ACCEL_RANGE) + +struct ImuDrotek2 imu_drotek2; + +void imu_impl_init(void) +{ + /* MPU-60X0 */ + mpu60x0_i2c_init(&imu_drotek2.mpu, &(DROTEK_2_I2C_DEV), MPU60X0_ADDR_ALT); + // change the default configuration + imu_drotek2.mpu.config.smplrt_div = DROTEK_2_SMPLRT_DIV; + imu_drotek2.mpu.config.dlpf_cfg = DROTEK_2_LOWPASS_FILTER; + imu_drotek2.mpu.config.gyro_range = DROTEK_2_GYRO_RANGE; + imu_drotek2.mpu.config.accel_range = DROTEK_2_ACCEL_RANGE; + + /* HMC5883 magnetometer */ + hmc58xx_init(&imu_drotek2.hmc, &(DROTEK_2_I2C_DEV), HMC58XX_ADDR); + /* set callback function to configure mag */ + imu_drotek2.mpu.config.slaves[0].configure = &imu_drotek2_configure_mag_slave; + +#if DROTEK_2_MAG_SLAVE + /* Set MPU I2C master clock to 400kHz */ + imu_drotek2.mpu.config.mst_clk = MPU60X0_MST_CLK_400KHZ; + /* Enable I2C slave0 delayed sample rate */ + imu_drotek2.mpu.config.mst_delay = 1; +#else + // use hmc mag via passthrough + imu_drotek2.mpu.config.i2c_bypass = TRUE; +#endif + + imu_drotek2.gyro_valid = FALSE; + imu_drotek2.accel_valid = FALSE; + imu_drotek2.mag_valid = FALSE; +} + +void imu_periodic(void) +{ + // Start reading the latest gyroscope data + mpu60x0_i2c_periodic(&imu_drotek2.mpu); + + // Read HMC58XX at ~50Hz (main loop for rotorcraft: 512Hz) + RunOnceEvery(10, hmc58xx_periodic(&imu_drotek2.hmc)); + +} + +void imu_drotek2_event(void) +{ + // If the MPU6050 I2C transaction has succeeded: convert the data + mpu60x0_i2c_event(&imu_drotek2.mpu); + if (imu_drotek2.mpu.data_available) { + memcpy(&imu.gyro_unscaled, &imu_drotek2.mpu.data_rates.rates, sizeof(struct Int32Rates)); + memcpy(&imu.accel_unscaled, &imu_drotek2.mpu.data_accel.vect, sizeof(struct Int32Vect3)); + imu_drotek2.mpu.data_available = FALSE; + imu_drotek2.gyro_valid = TRUE; + imu_drotek2.accel_valid = TRUE; + } + + /* HMC58XX event task */ + hmc58xx_event(&imu_drotek2.hmc); + if (imu_drotek2.hmc.data_available) { + VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect); + imu_drotek2.hmc.data_available = FALSE; + imu_drotek2.mag_valid = TRUE; + } +} + +/** callback function to configure hmc5883 mag + * @return TRUE if mag configuration finished + */ +bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu) +{ + hmc58xx_start_configure(&imu_drotek2.hmc); + if (imu_drotek2.hmc.initialized) + return TRUE; + else + return FALSE; +} diff --git a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h new file mode 100644 index 0000000000..0cb89ca681 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 subsystems/imu/imu_drotek_10dof_v2.h + * + * Driver for the Drotek 10DOF V2 IMU. + * MPU6050 + HMC5883 + MS5611 + */ + +#ifndef IMU_DROTEK_10DOF_V2_H +#define IMU_DROTEK_10DOF_V2_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "peripherals/mpu60x0_i2c.h" +#include "subsystems/imu/imu_mpu60x0_defaults.h" +#include "peripherals/hmc58xx.h" + + +#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 +#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 + + +struct ImuDrotek2 { + volatile bool_t gyro_valid; + volatile bool_t accel_valid; + volatile bool_t mag_valid; + struct Mpu60x0_I2c mpu; + struct Hmc58xx hmc; +}; + +extern struct ImuDrotek2 imu_drotek2; + +extern void imu_drotek2_event(void); +extern bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu); + + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { + imu_drotek2_event(); + if (imu_drotek2.gyro_valid) { + imu_drotek2.gyro_valid = FALSE; + _gyro_handler(); + } + if (imu_drotek2.accel_valid) { + imu_drotek2.accel_valid = FALSE; + _accel_handler(); + } + if (imu_drotek2.mag_valid) { + imu_drotek2.mag_valid = FALSE; + _mag_handler(); + } +} + +#endif /* IMU_DROTEK_10DOF_V2_H */ diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2.h b/sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h similarity index 62% rename from sw/airborne/subsystems/imu/imu_aspirin_2.h rename to sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h index 33b91686e4..b53faef232 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2.h +++ b/sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h @@ -20,35 +20,14 @@ */ /** - * @file subsystems/imu/imu_aspirin_2.h - * Driver for the Aspirin v2.x IMU using SPI for the MPU6000. + * @file subsystems/imu/imu_mpu60x0_defaults.h + * Default sensitivity definitions for an IMU using the MPU60x0. */ -#ifndef IMU_ASPIRIN_2_H -#define IMU_ASPIRIN_2_H +#ifndef IMU_MPU60X0_DEFAULTS_H +#define IMU_MPU60X0_DEFAULTS_H -#include "std.h" #include "generated/airframe.h" -#include "subsystems/imu.h" - -#include "peripherals/mpu60x0_spi.h" - -#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 - -#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 @@ -94,33 +73,4 @@ #define IMU_ACCEL_Z_NEUTRAL 0 #endif - -struct ImuAspirin2 { - volatile bool_t gyro_valid; - volatile bool_t accel_valid; - volatile bool_t mag_valid; - struct Mpu60x0_Spi mpu; -}; - -extern struct ImuAspirin2 imu_aspirin2; - -extern void imu_aspirin2_event(void); - - -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { - imu_aspirin2_event(); - if (imu_aspirin2.gyro_valid) { - imu_aspirin2.gyro_valid = FALSE; - _gyro_handler(); - } - if (imu_aspirin2.accel_valid) { - imu_aspirin2.accel_valid = FALSE; - _accel_handler(); - } - if (imu_aspirin2.mag_valid) { - imu_aspirin2.mag_valid = FALSE; - _mag_handler(); - } -} - -#endif /* IMU_ASPIRIN_2_H */ +#endif /* IMU_MPU60X0_DEFAULTS_H */ From dab1d0ef9e945f21ed0c8d6c04e1debac021a29c Mon Sep 17 00:00:00 2001 From: Ewoud Date: Fri, 14 Jun 2013 15:04:56 +0200 Subject: [PATCH 04/36] [rotorcraft] rc euler read in fixed point closes #462 --- .../stabilization_attitude_rc_setpoint.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index 1aadf44037..bdb399f3d4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -84,16 +84,16 @@ float stabilization_attitude_get_heading_f(void) { * @param[out] sp attitude setpoint as euler angles */ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) { - const float max_phi_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) / MAX_PPRZ; - const float max_theta_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) / MAX_PPRZ; - const float max_r_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) / MAX_PPRZ; - - sp->phi = (int32_t) (radio_control.values[RADIO_ROLL] * max_phi_scale); - sp->theta = (int32_t) (radio_control.values[RADIO_PITCH] * max_theta_scale); + const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI); + const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA); + const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R); + + sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ); + sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ); if (in_flight) { if (YAW_DEADBAND_EXCEEDED()) { - sp->psi += (int32_t) (radio_control.values[RADIO_YAW] * max_r_scale / RC_UPDATE_FREQ); + sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ); INT32_ANGLE_NORMALIZE(sp->psi); } if (autopilot_mode == AP_MODE_FORWARD) { From 0195a94b1e9cd0ddb81b48c093bc4364639cf288 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 14 Jun 2013 18:22:32 +0200 Subject: [PATCH 05/36] [airframe] updating some ENAC airframes --- conf/airframes/ENAC/quadrotor/blender.xml | 3 +-- conf/airframes/ENAC/quadrotor/booz2_g1.xml | 19 ++++++++++--------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index b5efec74a6..2dce3cc671 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -37,7 +37,7 @@ - + @@ -133,7 +133,6 @@
- diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 0604b4a152..472c32d189 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -47,7 +47,7 @@ - + @@ -102,13 +102,13 @@ - - - + + + - - - + + + @@ -206,9 +206,10 @@
+ - - + + From 20588e835080a539ee3981bfa2c949ece30b45d0 Mon Sep 17 00:00:00 2001 From: fvantienen Date: Fri, 14 Jun 2013 19:49:48 +0200 Subject: [PATCH 06/36] [rotorcraft ]Added ardrone2 sdk version gps, fixed some bugs. The GPS receiver from parrot is added for the SDK version of the ardrone2. This new gps receiver is the flightrecorde from parrot. Also fixed some bugs in the navdata receive of the SDK version, and added some comments to the INS. Closes #463 --- conf/airframes/ardrone2_sdk.xml | 2 +- .../rotorcraft/gps_ardrone2.makefile | 20 ++++++ sw/airborne/boards/ardrone/at_com.h | 51 +++++++++++++++ sw/airborne/boards/ardrone2_sdk.h | 2 +- sw/airborne/subsystems/ahrs/ahrs_ardrone2.c | 25 +++++++- sw/airborne/subsystems/gps/gps_ardrone2.c | 63 +++++++++++++++++++ sw/airborne/subsystems/gps/gps_ardrone2.h | 56 +++++++++++++++++ sw/airborne/subsystems/ins/ins_ardrone2.c | 5 ++ 8 files changed, 219 insertions(+), 5 deletions(-) create mode 100644 conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile create mode 100644 sw/airborne/subsystems/gps/gps_ardrone2.c create mode 100644 sw/airborne/subsystems/gps/gps_ardrone2.h diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index c7bb880258..a6850e6473 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -14,7 +14,7 @@ - + diff --git a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile new file mode 100644 index 0000000000..fad4a3919f --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile @@ -0,0 +1,20 @@ +# Hey Emacs, this is a -*- makefile -*- + +# ARDrone 2 Flightrecorder GPS unit + + +ap.CFLAGS += -DUSE_GPS -DUSE_GPS_ARDRONE2 + +ifneq ($(GPS_LED),none) + ap.CFLAGS += -DGPS_LED=$(GPS_LED) +endif + +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ardrone2.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ardrone2.c + +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c + +nps.CFLAGS += -DUSE_GPS +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c + diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h index 08605e9843..431030eb31 100644 --- a/sw/airborne/boards/ardrone/at_com.h +++ b/sw/airborne/boards/ardrone/at_com.h @@ -29,6 +29,8 @@ #ifndef BOARDS_ARDRONE_AT_COM_H #define BOARDS_ARDRONE_AT_COM_H +#define NAVDATA_HEADER (0x55667788) + //Define the AT_REF bits typedef enum { REF_TAKEOFF = 1U << 9, @@ -149,6 +151,55 @@ typedef struct _navdata_phys_measures_t { uint32_t vrefIDG; // ref volt IDG gyro [LSB] } __attribute__ ((packed)) navdata_phys_measures_t; +//Navdata gps packet +typedef double float64_t; //TODO: Fix this nicely, but this is only used here +typedef float float32_t; //TODO: Fix this nicely, but this is only used here +typedef struct _navdata_gps_t { + uint16_t tag; /*!< Navdata block ('option') identifier */ + uint16_t size; /*!< set this to the size of this structure */ + float64_t lat; /*!< Latitude */ + float64_t lon; /*!< Longitude */ + float64_t elevation; /*!< Elevation */ + float64_t hdop; /*!< hdop */ + int32_t data_available; /*!< When there is data available */ + uint8_t unk_0[8]; + float64_t lat0; /*!< Latitude ??? */ + float64_t lon0; /*!< Longitude ??? */ + float64_t lat_fuse; /*!< Latitude fused */ + float64_t lon_fuse; /*!< Longitude fused */ + uint32_t gps_state; /*!< State of the GPS, still need to figure out */ + uint8_t unk_1[40]; + float64_t vdop; /*!< vdop */ + float64_t pdop; /*!< pdop */ + float32_t speed; /*!< speed */ + uint32_t last_frame_timestamp; /*!< Timestamp from the last frame */ + float32_t degree; /*!< Degree */ + float32_t degree_mag; /*!< Degree of the magnetic */ + uint8_t unk_2[16]; + struct{ + uint8_t sat; + uint8_t unk; + }channels[12]; + int32_t gps_plugged; /*!< When the gps is plugged */ + uint8_t unk_3[108]; + float64_t gps_time; /*!< The gps time of week */ + uint16_t week; /*!< The gps week */ + uint8_t gps_fix; /*!< The gps fix */ + uint8_t num_sattelites; /*!< Number of sattelites */ + uint8_t unk_4[24]; + float64_t ned_vel_c0; /*!< NED velocity */ + float64_t ned_vel_c1; /*!< NED velocity */ + float64_t ned_vel_c2; /*!< NED velocity */ + float64_t pos_accur_c0; /*!< Position accuracy */ + float64_t pos_accur_c1; /*!< Position accuracy */ + float64_t pos_accur_c2; /*!< Position accuracy */ + float32_t speed_acur; /*!< Speed accuracy */ + float32_t time_acur; /*!< Time accuracy */ + uint8_t unk_5[72]; + float32_t temprature; + float32_t pressure; +} __attribute__ ((packed)) navdata_gps_t; + //External functions extern void init_at_com(void); extern void at_com_recieve_navdata(unsigned char* buffer); diff --git a/sw/airborne/boards/ardrone2_sdk.h b/sw/airborne/boards/ardrone2_sdk.h index c325f3ec40..01bb97452d 100644 --- a/sw/airborne/boards/ardrone2_sdk.h +++ b/sw/airborne/boards/ardrone2_sdk.h @@ -6,7 +6,7 @@ /* Internal communication */ #define ARDRONE_NAVDATA_PORT 5554 #define ARDRONE_AT_PORT 5556 -#define ARDRONE_NAVDATA_BUFFER_SIZE 2048 +#define ARDRONE_NAVDATA_BUFFER_SIZE 4096 #define ARDRONE_IP "192.168.1.1" /* Default actuators driver */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index 95c865fb90..afed7a6202 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c @@ -33,9 +33,13 @@ #include "boards/ardrone/at_com.h" #include "subsystems/electrical.h" +#ifdef USE_GPS_ARDRONE2 +#include "subsystems/gps/gps_ardrone2.h" +#endif + struct AhrsARDrone ahrs_impl; struct AhrsAligner ahrs_aligner; -unsigned char buffer[2048]; //Packet buffer +unsigned char buffer[4096]; //Packet buffer void ahrs_init(void) { init_at_com(); @@ -55,6 +59,12 @@ void ahrs_propagate(void) { //Recieve the main packet at_com_recieve_navdata(buffer); navdata_t* main_packet = (navdata_t*) &buffer; + + //When this isn't a valid packet return + if(main_packet->header != NAVDATA_HEADER) + return; + + //Set the state ahrs_impl.state = main_packet->ardrone_state; //Init the option @@ -63,10 +73,11 @@ void ahrs_propagate(void) { //The possible packets navdata_demo_t* navdata_demo; + navdata_gps_t* navdata_gps; navdata_phys_measures_t* navdata_phys_measures; //Read the navdata until packet is fully readed - while(!full_read) { + while(!full_read && navdata_option->size > 0) { //Check the tag for the right option switch(navdata_option->tag) { case 0: //NAVDATA_DEMO @@ -99,6 +110,14 @@ void ahrs_propagate(void) { //Set the AHRS accel state INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000) break; +#ifdef USE_GPS_ARDRONE2 + case 27: //NAVDATA_GPS + navdata_gps = (navdata_gps_t*) navdata_option; + + // Send the data to the gps parser + gps_ardrone2_parse(navdata_gps); + break; +#endif case 0xFFFF: //CHECKSUM //TODO: Check the checksum full_read = TRUE; @@ -107,7 +126,7 @@ void ahrs_propagate(void) { //printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size); break; } - navdata_option = (navdata_option_t*) ((int)navdata_option + navdata_option->size); + navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size); } } diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c new file mode 100644 index 0000000000..60a1e5db78 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_ardrone2.c @@ -0,0 +1,63 @@ +/* + * + * Copyright (C) 2013 Freek van Tienen + * + * 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 subsystems/gps/gps_ardrone2.c + * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2. + */ + +#include "subsystems/gps.h" +#include "math/pprz_geodetic_double.h" + +bool_t gps_ardrone2_available; + +void gps_impl_init( void ) { + gps_ardrone2_available = FALSE; +} + +void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { + // Set the lla double struct from the navdata + struct LlaCoor_d gps_lla_d; + gps_lla_d.lat = RadOfDeg(navdata_gps->lat); + gps_lla_d.lon = RadOfDeg(navdata_gps->lon); + gps_lla_d.alt = navdata_gps->elevation; + + // Convert it to ecef + struct EcefCoor_d gps_ecef_d; + ecef_of_lla_d(&gps_ecef_d, &gps_lla_d); + + // Convert the lla and ecef to int and set them in gps + ECEF_BFP_OF_REAL(gps.ecef_pos, gps_ecef_d); + LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d); + + // TODO: parse other stuff + + // Check if we have a fix TODO: check if 2D or 3D fix? + if (navdata_gps->gps_state == 1) + gps.fix = GPS_FIX_3D; + else + gps.fix = GPS_FIX_NONE; + + // Set that there is a packet + gps_ardrone2_available = TRUE; +} diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h new file mode 100644 index 0000000000..42e926ff34 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_ardrone2.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * 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 subsystems/gps/gps_ardrone2.h + * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2. + * +*/ + +#ifndef GPS_ARDRONE_H +#define GPS_ARDRONE_H + +#include "boards/ardrone/at_com.h" + +//#define GPS_NB_CHANNELS 12 // TODO: Get channels out of packet +extern bool_t gps_ardrone2_available; + +/* + * The GPS event + */ +#define GpsEvent(_sol_available_callback) { \ + if (gps_ardrone2_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_ardrone2_available = FALSE; \ + } \ + } + +void gps_ardrone2_parse(navdata_gps_t *navdata_gps); + +/* Maybe needed? +#define gps_nmea_Reset(_val) { } +*/ + +#endif /* GPS_ARDRONE_H */ diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index 3f42c8085d..a50f0ed908 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -110,6 +110,7 @@ void ins_propagate() { stateSetAccelNed_f(&ins_ltp_accel); stateSetSpeedNed_f(&ins_ltp_speed); + //Don't set the height if we use the one from the gps #if !USE_GPS_HEIGHT //Set the height and save the position ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; @@ -138,11 +139,15 @@ void ins_update_gps(void) { //Set the x and y and maybe z position in ltp and save struct NedCoor_i ins_gps_pos_cm_ned; ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); + + //When we don't want to use the height of the navdata we can use the gps height #if USE_GPS_HEIGHT INT32_VECT3_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); #else INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); #endif + + //Set the local origin stateSetPositionNed_i(&ins_ltp_pos); } #endif /* USE_GPS */ From 1931ec6628e7a961056b59466e8e20cdb6a96a70 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 14 Jun 2013 23:22:54 +0200 Subject: [PATCH 07/36] cleanup some trailing whitespace --- .../stabilization/stabilization_attitude_rc_setpoint.c | 2 +- .../rotorcraft/stabilization/stabilization_rate.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index bdb399f3d4..7054d1824b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -87,7 +87,7 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI); const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA); const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R); - + sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ); sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 486ca4deb9..b8ee4c60be 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -166,22 +166,22 @@ void stabilization_rate_read_rc( void ) { //Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired void stabilization_rate_read_rc_switched_sticks( void ) { - + if(ROLL_RATE_DEADBAND_EXCEEDED()) stabilization_rate_sp.r = (int32_t) -radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ; else stabilization_rate_sp.r = 0; - + if(PITCH_RATE_DEADBAND_EXCEEDED()) stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ; else stabilization_rate_sp.q = 0; - + if(YAW_RATE_DEADBAND_EXCEEDED()) stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ; else stabilization_rate_sp.p = 0; - + // Setpoint at ref resolution INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC); } From cf0dba85effcfd0e3f243803cb090634e7e5f3db Mon Sep 17 00:00:00 2001 From: Dino Hensen Date: Mon, 17 Jun 2013 00:11:25 +0200 Subject: [PATCH 08/36] [rotorcraft] check_in_flight without RC Closes #464 Changes to make ARDrone2 SDK fly without joystick Still needs to be properly solved as per issue #201 --- conf/airframes/ardrone2_sdk.xml | 4 +-- sw/airborne/firmwares/rotorcraft/autopilot.c | 34 ++++++++++++++++++++ 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index a6850e6473..7544a9a391 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -98,8 +98,8 @@
- - + +
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index fa4538cfda..ae91f2c2d1 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -92,6 +92,36 @@ void autopilot_init(void) { } +static inline void autopilot_check_in_flight_no_rc( bool_t motors_on ) { + if (autopilot_in_flight) { + if (autopilot_in_flight_counter > 0) { + if (stabilization_cmd[COMMAND_THRUST] == 0) { + autopilot_in_flight_counter--; + if (autopilot_in_flight_counter == 0) { + autopilot_in_flight = FALSE; + } + } + else { /* !THROTTLE_STICK_DOWN */ + autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; + } + } + } + else { /* not in flight */ + if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && + motors_on) { + if (stabilization_cmd[COMMAND_THRUST] > 0) { + autopilot_in_flight_counter++; + if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) + autopilot_in_flight = TRUE; + } + else { /* THROTTLE_STICK_DOWN */ + autopilot_in_flight_counter = 0; + } + } + } +} + + void autopilot_periodic(void) { RunOnceEvery(NAV_PRESCALER, nav_periodic_task()); @@ -118,6 +148,10 @@ INFO("Using FAILSAFE_GROUND_DETECT") SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on); } + // when we dont have RC, check in flight by looking at throttle + if (radio_control.status != RC_OK) { + autopilot_check_in_flight_no_rc(autopilot_motors_on); + } } From f1b84997d5e704d72e41b0bffcc562082ead203b Mon Sep 17 00:00:00 2001 From: Dino Hensen Date: Mon, 17 Jun 2013 00:46:12 +0200 Subject: [PATCH 09/36] [conf] update example sessions for ARDrone2 Removed Logitech Joystick because users are not going to have it and it will cause error messages when they launch it. Adding notes in Wiki on how to add joystick yourself. We now only need one session for ardrone2 --- conf/control_panel.xml.example | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example index 6e36df42b1..0c69ef79c8 100644 --- a/conf/control_panel.xml.example +++ b/conf/control_panel.xml.example @@ -83,28 +83,12 @@
- + - - - - - - - - - - - - - - - - From 877963eaf8ca47718fcf3d3517a62b2b9ad76047 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 17 Jun 2013 00:38:22 +0200 Subject: [PATCH 10/36] [release] v5.0 stable version --- CHANGELOG.md | 79 +++++++++++++++++++++++++++++++++++++++++++++-- paparazzi_version | 2 +- 2 files changed, 78 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 97e05bc455..59d90f1ad1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,11 +1,86 @@ -Paparazzi 4.9 - development branch -================================== +Paparazzi 5.0.0_stable +====================== + +Stable version release + +General +------- - STM libs completely replaced by libopencm3 +- [gcc-arm-embedded] (https://launchpad.net/gcc-arm-embedded) is the new recommended toolchain +- Use findlib (ocamlfind) for ocaml packages. Faster build. + [#274] (https://github.com/paparazzi/paparazzi/pull/274) +- Building/Running the groundsegment on an ARM (e.g. RaspberryPi). - Input2ivy uses SDL for joysticks (cross-platform, works on OSX as well now) [#220] (https://github.com/paparazzi/paparazzi/pull/220) - Option to change text papget color using a combobox [#194] (https://github.com/paparazzi/paparazzi/pull/194) +- Redundant communications + [#429] (https://github.com/paparazzi/paparazzi/pull/429) +- Log also contains includes like procedures now, so replay if these missions is possible. + [#227] (https://github.com/paparazzi/paparazzi/issues/227) +- Paparazzi Center + - New simulation launcher with dialog and detection of available ones. + [#354] (https://github.com/paparazzi/paparazzi/pull/354) + - Checkbox to print extra configuration information during build. +- GCS: + - Fix panning with mouse if there are no background tiles. + [#9] (https://github.com/paparazzi/paparazzi/issues/9) + - Higher zoom level for maps. + [#277] (https://github.com/paparazzi/paparazzi/issues/277) + +Hardware support +---------------- + +- initial support for STM32F4 + - Apogee autopilot + - KroozSD autopilot +- Parrot AR Drone 2 support: raw and sdk versions +- CH Robotics UM6 IMU/AHRS +- GPS/INS XSens Mti-G support +- GPS Sirf support +- GPS Skytraq now usable for fixedwings as well + [#167] (https://github.com/paparazzi/paparazzi/issues/167) +- Mikrokopter V2 BLDC + [#377] (https://github.com/paparazzi/paparazzi/pull/377) +- PX4Flow sensor + [#379] (https://github.com/paparazzi/paparazzi/pull/379) +- Dropped AVR support + +Airborne +-------- + +- State interface with automatic coordinate transformations + [#237] (https://github.com/paparazzi/paparazzi/pull/237) +- New AHRS filter: Multiplicative quaternion linearized Kalman Filter +- New SPI driver with transaction queues. + - Fix transactions with zero length input. + [#348] (https://github.com/paparazzi/paparazzi/issues/348) +- Peripherals: Cleanup and refactoring. + - MPU60x0 peripheral supporting SPI and I2C with slave. +- UDP datalink. +- Magnetometer current offset calibration. + [#346] (https://github.com/paparazzi/paparazzi/pull/346) +- Gain scheduling module. + [#335] (https://github.com/paparazzi/paparazzi/pull/335) + +Rotorcraft firmware specific +---------------------------- + +- Quadshot transitioning vehicle support. +- Care Free Mode + + +Paparazzi 4.2.1_stable +====================== + +Maintenance release + +- fix elf PT_LOAD type in lpc21iap LPC USB download +- fix electrical.current estimate in sim +- fix LPC+xbee_api in rotorcraft +- fix conversion of vsupply to decivolts if offset is used +- more robust dfu flash script, only upload to Lisa/M Paparazzi 4.2.0_stable diff --git a/paparazzi_version b/paparazzi_version index ad00a1aa0c..0fde2c0289 100755 --- a/paparazzi_version +++ b/paparazzi_version @@ -1,6 +1,6 @@ #!/bin/sh -DEF_VER=v4.9_devel +DEF_VER=v5.0.0_stable # First try git describe (if running on a git repo), # then use default version from above (for release tarballs). From 09c1c295ebae2ac9f94dca33210ee29b20360195 Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Sun, 16 Jun 2013 18:23:47 -0700 Subject: [PATCH 11/36] Updated to newest locm3 --- sw/ext/libopencm3 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/ext/libopencm3 b/sw/ext/libopencm3 index 0bf8324c91..fb5c86db07 160000 --- a/sw/ext/libopencm3 +++ b/sw/ext/libopencm3 @@ -1 +1 @@ -Subproject commit 0bf8324c9128b146ec6a18d62d8f05a5b61a2530 +Subproject commit fb5c86db0723aa64740df7aebb3a0fd69acf52fe From b0c1c5d30a09eb0e51e26e9d1877c8e1d888028f Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Sun, 16 Jun 2013 18:39:04 -0700 Subject: [PATCH 12/36] Updated luftboot to get support for stdint types only. --- sw/ext/luftboot | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/ext/luftboot b/sw/ext/luftboot index 850436456f..3b6a352ba5 160000 --- a/sw/ext/luftboot +++ b/sw/ext/luftboot @@ -1 +1 @@ -Subproject commit 850436456f10e9c025a101b481077f547942d322 +Subproject commit 3b6a352ba55b576f576f0eb6b116d7eab0c99fcd From bf604dd2e803af606157abc4f83efa3dca9114d9 Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Sun, 16 Jun 2013 18:45:26 -0700 Subject: [PATCH 13/36] Removed usage of the kernel style int types. --- sw/airborne/arch/stm32/mcu_periph/can_arch.c | 4 +- sw/airborne/arch/stm32/mcu_periph/i2c_arch.c | 60 ++++++------ sw/airborne/arch/stm32/mcu_periph/spi_arch.c | 94 +++++++++---------- sw/airborne/arch/stm32/mcu_periph/uart_arch.c | 70 +++++++------- .../subsystems/actuators/actuators_pwm_arch.c | 8 +- .../firmwares/beth/overo_sfb_controller.c | 2 +- 6 files changed, 119 insertions(+), 119 deletions(-) diff --git a/sw/airborne/arch/stm32/mcu_periph/can_arch.c b/sw/airborne/arch/stm32/mcu_periph/can_arch.c index ab91e8e0b1..44702dc19b 100644 --- a/sw/airborne/arch/stm32/mcu_periph/can_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/can_arch.c @@ -166,9 +166,9 @@ int can_hw_transmit(uint32_t id, const uint8_t *buf, uint8_t len) void usb_lp_can_rx0_isr(void) { - u32 id, fmi; + uint32_t id, fmi; bool ext, rtr; - u8 length, data[8]; + uint8_t length, data[8]; can_receive(CAN1, 0, /* FIFO: 0 */ diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c index e4024e94e9..902d915759 100644 --- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c @@ -71,7 +71,7 @@ static inline void __enable_irq(void) { asm volatile ("cpsie i"); } #define __I2C_REG_CRITICAL_ZONE_STOP __enable_irq(); -static inline void PPRZ_I2C_SEND_STOP(u32 i2c) +static inline void PPRZ_I2C_SEND_STOP(uint32_t i2c) { // Man: p722: Stop generation after the current byte transfer or after the current Start condition is sent. I2C_CR1(i2c) |= I2C_CR1_STOP; @@ -88,7 +88,7 @@ static inline void PPRZ_I2C_SEND_STOP(u32 i2c) static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph) { - u32 i2c = (u32) periph->reg_addr; + uint32_t i2c = (uint32_t) periph->reg_addr; // Reset the buffer pointer to the first byte periph->idx_buf = 0; @@ -134,7 +134,7 @@ enum STMI2CSubTransactionStatus { // Doc ID 13902 Rev 11 p 710/1072 // Transfer Sequence Diagram for Master Transmitter -static inline enum STMI2CSubTransactionStatus stmi2c_send(u32 i2c, struct i2c_periph *periph, struct i2c_transaction *trans) +static inline enum STMI2CSubTransactionStatus stmi2c_send(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans) { uint16_t SR1 = I2C_SR1(i2c); @@ -215,7 +215,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(u32 i2c, struct i2c_pe // Doc ID 13902 Rev 11 p 714/1072 // Transfer Sequence Diagram for Master Receiver for N=1 -static inline enum STMI2CSubTransactionStatus stmi2c_read1(u32 i2c, struct i2c_periph *periph, struct i2c_transaction *trans) +static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans) { uint16_t SR1 = I2C_SR1(i2c); @@ -278,7 +278,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(u32 i2c, struct i2c_p // Doc ID 13902 Rev 11 p 713/1072 // Transfer Sequence Diagram for Master Receiver for N=2 -static inline enum STMI2CSubTransactionStatus stmi2c_read2(u32 i2c, struct i2c_periph *periph, struct i2c_transaction *trans) +static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans) { uint16_t SR1 = I2C_SR1(i2c); @@ -348,7 +348,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(u32 i2c, struct i2c_p // Doc ID 13902 Rev 11 p 712/1072 // Transfer Sequence Diagram for Master Receiver for N>2 -static inline enum STMI2CSubTransactionStatus stmi2c_readmany(u32 i2c, struct i2c_periph *periph, struct i2c_transaction *trans) +static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans) { uint16_t SR1 = I2C_SR1(i2c); @@ -473,51 +473,51 @@ static inline void i2c_error(struct i2c_periph *periph) uint8_t err_nr = 0; #endif periph->errors->er_irq_cnt; - if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_AF) != 0) { /* Acknowledge failure */ + if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_AF) != 0) { /* Acknowledge failure */ periph->errors->ack_fail_cnt++; - I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_AF; + I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_AF; #ifdef I2C_DEBUG_LED err_nr = 1; #endif } - if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_BERR) != 0) { /* Misplaced Start or Stop condition */ + if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_BERR) != 0) { /* Misplaced Start or Stop condition */ periph->errors->miss_start_stop_cnt++; - I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_BERR; + I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_BERR; #ifdef I2C_DEBUG_LED err_nr = 2; #endif } - if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_ARLO) != 0) { /* Arbitration lost */ + if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_ARLO) != 0) { /* Arbitration lost */ periph->errors->arb_lost_cnt++; - I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_ARLO; + I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_ARLO; #ifdef I2C_DEBUG_LED err_nr = 3; #endif } - if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_OVR) != 0) { /* Overrun/Underrun */ + if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_OVR) != 0) { /* Overrun/Underrun */ periph->errors->over_under_cnt++; - I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_OVR; + I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_OVR; #ifdef I2C_DEBUG_LED err_nr = 4; #endif } - if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_PECERR) != 0) { /* PEC Error in reception */ + if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_PECERR) != 0) { /* PEC Error in reception */ periph->errors->pec_recep_cnt++; - I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_PECERR; + I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_PECERR; #ifdef I2C_DEBUG_LED err_nr = 5; #endif } - if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_TIMEOUT) != 0) { /* Timeout or Tlow error */ + if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_TIMEOUT) != 0) { /* Timeout or Tlow error */ periph->errors->timeout_tlow_cnt++; - I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_TIMEOUT; + I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_TIMEOUT; #ifdef I2C_DEBUG_LED err_nr = 6; #endif } - if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_SMBALERT) != 0) { /* SMBus alert */ + if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_SMBALERT) != 0) { /* SMBus alert */ periph->errors->smbus_alert_cnt++; - I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_SMBALERT; + I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_SMBALERT; #ifdef I2C_DEBUG_LED err_nr = 7; #endif @@ -531,7 +531,7 @@ static inline void i2c_error(struct i2c_periph *periph) } -static inline void stmi2c_clear_pending_interrupts(u32 i2c) +static inline void stmi2c_clear_pending_interrupts(uint32_t i2c) { uint16_t SR1 = I2C_SR1(i2c); @@ -648,7 +648,7 @@ static inline void i2c_irq(struct i2c_periph *periph) #endif // Save Some Direct Access to the I2C Registers ... - u32 i2c = (u32) periph->reg_addr; + uint32_t i2c = (uint32_t) periph->reg_addr; ///////////////////////////// // Check if we were ready ... @@ -944,7 +944,7 @@ void i2c1_hw_init(void) { } void i2c1_ev_isr(void) { - u32 i2c = (u32) i2c1.reg_addr; + uint32_t i2c = (uint32_t) i2c1.reg_addr; I2C_CR2(i2c) &= ~I2C_CR2_ITERREN; i2c_irq(&i2c1); i2c1_watchdog_counter = 0; @@ -952,7 +952,7 @@ void i2c1_ev_isr(void) { } void i2c1_er_isr(void) { - u32 i2c = (u32) i2c1.reg_addr; + uint32_t i2c = (uint32_t) i2c1.reg_addr; I2C_CR2(i2c) &= ~I2C_CR2_ITEVTEN; i2c_irq(&i2c1); i2c1_watchdog_counter = 0; @@ -1033,7 +1033,7 @@ void i2c2_hw_init(void) { } void i2c2_ev_isr(void) { - u32 i2c = (u32) i2c2.reg_addr; + uint32_t i2c = (uint32_t) i2c2.reg_addr; I2C_CR2(i2c) &= ~I2C_CR2_ITERREN; i2c_irq(&i2c2); i2c2_watchdog_counter = 0; @@ -1041,7 +1041,7 @@ void i2c2_ev_isr(void) { } void i2c2_er_isr(void) { - u32 i2c = (u32) i2c2.reg_addr; + uint32_t i2c = (uint32_t) i2c2.reg_addr; I2C_CR2(i2c) &= ~I2C_CR2_ITEVTEN; i2c_irq(&i2c2); i2c2_watchdog_counter = 0; @@ -1121,7 +1121,7 @@ void i2c3_hw_init(void) { } void i2c3_ev_isr(void) { - u32 i2c = (u32) i2c3.reg_addr; + uint32_t i2c = (uint32_t) i2c3.reg_addr; I2C_CR2(i2c) &= ~I2C_CR2_ITERREN; i2c_irq(&i2c3); i2c3_watchdog_counter = 0; @@ -1129,7 +1129,7 @@ void i2c3_ev_isr(void) { } void i2c3_er_isr(void) { - u32 i2c = (u32) i2c3.reg_addr; + uint32_t i2c = (uint32_t) i2c3.reg_addr; I2C_CR2(i2c) &= ~I2C_CR2_ITEVTEN; i2c_irq(&i2c3); i2c3_watchdog_counter = 0; @@ -1151,7 +1151,7 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate) volatile int devider; volatile int risetime; - u32 i2c = (u32) periph->reg_addr; + uint32_t i2c = (uint32_t) periph->reg_addr; /***************************************************** Bitrate: @@ -1361,7 +1361,7 @@ bool_t i2c_idle(struct i2c_periph* periph) // This is actually a difficult function: // -simply reading the status flags can clear bits and corrupt the transaction - u32 i2c = (u32) periph->reg_addr; + uint32_t i2c = (uint32_t) periph->reg_addr; #ifdef I2C_DEBUG_LED #ifdef USE_I2C1 diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c index 6aa01e8620..4384d15686 100644 --- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c @@ -69,11 +69,11 @@ * Libopencm3 specifc communication parameters for a SPI peripheral in master mode. */ struct locm3_spi_comm { - u32 br; ///< baudrate (clock divider) - u32 cpol; ///< clock polarity - u32 cpha; ///< clock phase - u32 dff; ///< data frame format 8/16 bits - u32 lsbfirst; ///< frame format lsb/msb first + uint32_t br; ///< baudrate (clock divider) + uint32_t cpol; ///< clock polarity + uint32_t cpha; ///< clock phase + uint32_t dff; ///< data frame format 8/16 bits + uint32_t lsbfirst; ///< frame format lsb/msb first }; /** @@ -81,19 +81,19 @@ struct locm3_spi_comm { * which allows for more code reuse. */ struct spi_periph_dma { - u32 spi; ///< SPI peripheral identifier - u32 spidr; ///< SPI DataRegister address for DMA - u32 dma; ///< DMA controller base address (DMA1 or DMA2) - u8 rx_chan; ///< receive DMA channel number - u8 tx_chan; ///< transmit DMA channel number - u8 rx_nvic_irq; ///< receive interrupt - u8 tx_nvic_irq; ///< transmit interrupt - u16 tx_dummy_buf; ///< dummy tx buffer for receive only cases + uint32_t spi; ///< SPI peripheral identifier + uint32_t spidr; ///< SPI DataRegister address for DMA + uint32_t dma; ///< DMA controller base address (DMA1 or DMA2) + uint8_t rx_chan; ///< receive DMA channel number + uint8_t tx_chan; ///< transmit DMA channel number + uint8_t rx_nvic_irq; ///< receive interrupt + uint8_t tx_nvic_irq; ///< transmit interrupt + uint16_t tx_dummy_buf; ///< dummy tx buffer for receive only cases bool_t tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len - u16 rx_dummy_buf; ///< dummy rx buffer for receive only cases + uint16_t rx_dummy_buf; ///< dummy rx buffer for receive only cases bool_t rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len struct locm3_spi_comm comm; ///< current communication paramters - u8 comm_sig; ///< comm config signature used to check for changes + uint8_t comm_sig; ///< comm config signature used to check for changes }; @@ -112,8 +112,8 @@ static struct spi_periph_dma spi3_dma; static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_transaction* _trans); static void spi_next_transaction(struct spi_periph* periph); -static void spi_configure_dma(u32 dma, u8 chan, u32 periph_addr, u32 buf_addr, - u16 len, enum SPIDataSizeSelect dss, bool_t increment); +static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr, + uint16_t len, enum SPIDataSizeSelect dss, bool_t increment); static void process_rx_dma_interrupt(struct spi_periph* periph); static void process_tx_dma_interrupt(struct spi_periph* periph); static void spi_arch_int_enable(struct spi_periph *spi); @@ -436,8 +436,8 @@ static void set_comm_from_transaction(struct locm3_spi_comm* c, struct spi_trans * Helpers for SPI transactions with DMA * *****************************************************************************/ -static void spi_configure_dma(u32 dma, u8 chan, u32 periph_addr, u32 buf_addr, - u16 len, enum SPIDataSizeSelect dss, bool_t increment) +static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr, + uint16_t len, enum SPIDataSizeSelect dss, bool_t increment) { dma_channel_reset(dma, chan); dma_set_peripheral_address(dma, chan, periph_addr); @@ -519,12 +519,12 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran dma->comm_sig = sig; /* apply the new configuration */ - spi_disable((u32)periph->reg_addr); - spi_init_master((u32)periph->reg_addr, dma->comm.br, dma->comm.cpol, + spi_disable((uint32_t)periph->reg_addr); + spi_init_master((uint32_t)periph->reg_addr, dma->comm.br, dma->comm.cpol, dma->comm.cpha, dma->comm.dff, dma->comm.lsbfirst); - spi_enable_software_slave_management((u32)periph->reg_addr); - spi_set_nss_high((u32)periph->reg_addr); - spi_enable((u32)periph->reg_addr); + spi_enable_software_slave_management((uint32_t)periph->reg_addr); + spi_set_nss_high((uint32_t)periph->reg_addr); + spi_enable((uint32_t)periph->reg_addr); } /* @@ -554,12 +554,12 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran */ if (trans->input_length == 0) { /* run the dummy rx dma for the complete transaction length */ - spi_configure_dma(dma->dma, dma->rx_chan, (u32)dma->spidr, - (u32)&(dma->rx_dummy_buf), trans->output_length, trans->dss, FALSE); + spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr, + (uint32_t)&(dma->rx_dummy_buf), trans->output_length, trans->dss, FALSE); } else { /* run the real rx dma for input_length */ - spi_configure_dma(dma->dma, dma->rx_chan, (u32)dma->spidr, - (u32)trans->input_buf, trans->input_length, trans->dss, TRUE); + spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr, + (uint32_t)trans->input_buf, trans->input_length, trans->dss, TRUE); /* use dummy rx dma for the rest */ if (trans->output_length > trans->input_length) { /* Enable use of second dma transfer with dummy buffer (cleared in ISR) */ @@ -581,11 +581,11 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran * the dummy is used right from the start. */ if (trans->output_length == 0) { - spi_configure_dma(dma->dma, dma->tx_chan, (u32)dma->spidr, - (u32)&(dma->tx_dummy_buf), trans->input_length, trans->dss, FALSE); + spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr, + (uint32_t)&(dma->tx_dummy_buf), trans->input_length, trans->dss, FALSE); } else { - spi_configure_dma(dma->dma, dma->tx_chan, (u32)dma->spidr, - (u32)trans->output_buf, trans->output_length, trans->dss, TRUE); + spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr, + (uint32_t)trans->output_buf, trans->output_length, trans->dss, TRUE); if (trans->input_length > trans->output_length) { /* Enable use of second dma transfer with dummy buffer (cleared in ISR) */ dma->tx_extra_dummy_dma = TRUE; @@ -604,8 +604,8 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran dma_enable_channel(dma->dma, dma->tx_chan); /* Enable SPI transfers via DMA */ - spi_enable_rx_dma((u32)periph->reg_addr); - spi_enable_tx_dma((u32)periph->reg_addr); + spi_enable_rx_dma((uint32_t)periph->reg_addr); + spi_enable_tx_dma((uint32_t)periph->reg_addr); } @@ -619,7 +619,7 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran void spi1_arch_init(void) { // set dma options - spi1_dma.spidr = (u32)&SPI1_DR; + spi1_dma.spidr = (uint32_t)&SPI1_DR; spi1_dma.dma = DMA1; spi1_dma.rx_chan = DMA_CHANNEL2; spi1_dma.tx_chan = DMA_CHANNEL3; @@ -690,7 +690,7 @@ void spi1_arch_init(void) { void spi2_arch_init(void) { // set dma options - spi2_dma.spidr = (u32)&SPI2_DR; + spi2_dma.spidr = (uint32_t)&SPI2_DR; spi2_dma.dma = DMA1; spi2_dma.rx_chan = DMA_CHANNEL4; spi2_dma.tx_chan = DMA_CHANNEL5; @@ -760,7 +760,7 @@ void spi2_arch_init(void) { void spi3_arch_init(void) { // set the default configuration - spi3_dma.spidr = (u32)&SPI3_DR; + spi3_dma.spidr = (uint32_t)&SPI3_DR; spi3_dma.dma = DMA2; spi3_dma.rx_chan = DMA_CHANNEL1; spi3_dma.tx_chan = DMA_CHANNEL2; @@ -914,7 +914,7 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { dma_disable_transfer_complete_interrupt(dma->dma, dma->rx_chan); /* Disable SPI Rx request */ - spi_disable_rx_dma((u32)periph->reg_addr); + spi_disable_rx_dma((uint32_t)periph->reg_addr); /* Disable DMA rx channel */ dma_disable_channel(dma->dma, dma->rx_chan); @@ -931,10 +931,10 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { dma->rx_extra_dummy_dma = FALSE; /* Use the difference in length between rx and tx */ - u16 len_remaining = trans->output_length - trans->input_length; + uint16_t len_remaining = trans->output_length - trans->input_length; - spi_configure_dma(dma->dma, dma->rx_chan, (u32)dma->spidr, - (u32)&(dma->rx_dummy_buf), len_remaining, trans->dss, FALSE); + spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr, + (uint32_t)&(dma->rx_dummy_buf), len_remaining, trans->dss, FALSE); dma_set_read_from_peripheral(dma->dma, dma->rx_chan); dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_HIGH); @@ -943,7 +943,7 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { /* Enable DMA channels */ dma_enable_channel(dma->dma, dma->rx_chan); /* Enable SPI transfers via DMA */ - spi_enable_rx_dma((u32)periph->reg_addr); + spi_enable_rx_dma((uint32_t)periph->reg_addr); } else { /* @@ -976,7 +976,7 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { dma_disable_transfer_complete_interrupt(dma->dma, dma->tx_chan); /* Disable SPI TX request */ - spi_disable_tx_dma((u32)periph->reg_addr); + spi_disable_tx_dma((uint32_t)periph->reg_addr); /* Disable DMA tx channel */ dma_disable_channel(dma->dma, dma->tx_chan); @@ -992,10 +992,10 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { dma->tx_extra_dummy_dma = FALSE; /* Use the difference in length between tx and rx */ - u16 len_remaining = trans->input_length - trans->output_length; + uint16_t len_remaining = trans->input_length - trans->output_length; - spi_configure_dma(dma->dma, dma->tx_chan, (u32)dma->spidr, - (u32)&(dma->tx_dummy_buf), len_remaining, trans->dss, FALSE); + spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr, + (uint32_t)&(dma->tx_dummy_buf), len_remaining, trans->dss, FALSE); dma_set_read_from_memory(dma->dma, dma->tx_chan); dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); @@ -1004,7 +1004,7 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { /* Enable DMA channels */ dma_enable_channel(dma->dma, dma->tx_chan); /* Enable SPI transfers via DMA */ - spi_enable_tx_dma((u32)periph->reg_addr); + spi_enable_tx_dma((uint32_t)periph->reg_addr); } } diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index 5e4058ca61..fb73f70983 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -42,40 +42,40 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) { /* Configure USART */ - usart_set_baudrate((u32)p->reg_addr, baud); - usart_set_databits((u32)p->reg_addr, 8); - usart_set_stopbits((u32)p->reg_addr, USART_STOPBITS_1); - usart_set_parity((u32)p->reg_addr, USART_PARITY_NONE); + usart_set_baudrate((uint32_t)p->reg_addr, baud); + usart_set_databits((uint32_t)p->reg_addr, 8); + usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_1); + usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_NONE); /* Disable Idle Line interrupt */ - USART_CR1((u32)p->reg_addr) &= ~USART_CR1_IDLEIE; + USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_IDLEIE; /* Disable LIN break detection interrupt */ - USART_CR2((u32)p->reg_addr) &= ~USART_CR2_LBDIE; + USART_CR2((uint32_t)p->reg_addr) &= ~USART_CR2_LBDIE; /* Enable USART1 Receive interrupts */ - USART_CR1((u32)p->reg_addr) |= USART_CR1_RXNEIE; + USART_CR1((uint32_t)p->reg_addr) |= USART_CR1_RXNEIE; /* Enable the USART */ - usart_enable((u32)p->reg_addr); + usart_enable((uint32_t)p->reg_addr); } void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control) { - u32 mode = 0; + uint32_t mode = 0; if (tx_enabled) mode |= USART_MODE_TX; if (rx_enabled) mode |= USART_MODE_RX; /* set mode to Tx, Rx or TxRx */ - usart_set_mode((u32)p->reg_addr, mode); + usart_set_mode((uint32_t)p->reg_addr, mode); if (hw_flow_control) { - usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_RTS_CTS); + usart_set_flow_control((uint32_t)p->reg_addr, USART_FLOWCONTROL_RTS_CTS); } else { - usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_NONE); + usart_set_flow_control((uint32_t)p->reg_addr, USART_FLOWCONTROL_NONE); } } @@ -86,7 +86,7 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) { if (temp == p->tx_extract_idx) return; // no room - USART_CR1((u32)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt + USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt // check if in process of sending data if (p->tx_running) { // yes, add to queue @@ -95,61 +95,61 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) { } else { // no, set running flag and write to output register p->tx_running = TRUE; - usart_send((u32)p->reg_addr, data); + usart_send((uint32_t)p->reg_addr, data); } - USART_CR1((u32)p->reg_addr) |= USART_CR1_TXEIE; // Enable TX interrupt + USART_CR1((uint32_t)p->reg_addr) |= USART_CR1_TXEIE; // Enable TX interrupt } static inline void usart_isr(struct uart_periph* p) { - if (((USART_CR1((u32)p->reg_addr) & USART_CR1_TXEIE) != 0) && - ((USART_SR((u32)p->reg_addr) & USART_SR_TXE) != 0)) { + if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_TXEIE) != 0) && + ((USART_SR((uint32_t)p->reg_addr) & USART_SR_TXE) != 0)) { // check if more data to send if (p->tx_insert_idx != p->tx_extract_idx) { - usart_send((u32)p->reg_addr,p->tx_buf[p->tx_extract_idx]); + usart_send((uint32_t)p->reg_addr,p->tx_buf[p->tx_extract_idx]); p->tx_extract_idx++; p->tx_extract_idx %= UART_TX_BUFFER_SIZE; } else { p->tx_running = FALSE; // clear running flag - USART_CR1((u32)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt + USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt } } - if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) && - ((USART_SR((u32)p->reg_addr) & USART_SR_RXNE) != 0) && - ((USART_SR((u32)p->reg_addr) & USART_SR_ORE) == 0) && - ((USART_SR((u32)p->reg_addr) & USART_SR_NE) == 0) && - ((USART_SR((u32)p->reg_addr) & USART_SR_FE) == 0)) { + if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && + ((USART_SR((uint32_t)p->reg_addr) & USART_SR_RXNE) != 0) && + ((USART_SR((uint32_t)p->reg_addr) & USART_SR_ORE) == 0) && + ((USART_SR((uint32_t)p->reg_addr) & USART_SR_NE) == 0) && + ((USART_SR((uint32_t)p->reg_addr) & USART_SR_FE) == 0)) { uint16_t temp = (p->rx_insert_idx + 1) % UART_RX_BUFFER_SIZE;; - p->rx_buf[p->rx_insert_idx] = usart_recv((u32)p->reg_addr); + p->rx_buf[p->rx_insert_idx] = usart_recv((uint32_t)p->reg_addr); // check for more room in queue if (temp != p->rx_extract_idx) p->rx_insert_idx = temp; // update insert index } else { /* ORE, NE or FE error - read USART_DR reg and log the error */ - if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) && - ((USART_SR((u32)p->reg_addr) & USART_SR_ORE) != 0)) { - usart_recv((u32)p->reg_addr); + if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && + ((USART_SR((uint32_t)p->reg_addr) & USART_SR_ORE) != 0)) { + usart_recv((uint32_t)p->reg_addr); p->ore++; } - if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) && - ((USART_SR((u32)p->reg_addr) & USART_SR_NE) != 0)) { - usart_recv((u32)p->reg_addr); + if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && + ((USART_SR((uint32_t)p->reg_addr) & USART_SR_NE) != 0)) { + usart_recv((uint32_t)p->reg_addr); p->ne_err++; } - if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) && - ((USART_SR((u32)p->reg_addr) & USART_SR_FE) != 0)) { - usart_recv((u32)p->reg_addr); + if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && + ((USART_SR((uint32_t)p->reg_addr) & USART_SR_FE) != 0)) { + usart_recv((uint32_t)p->reg_addr); p->fe_err++; } } } -static inline void usart_enable_irq(u8 IRQn) { +static inline void usart_enable_irq(uint8_t IRQn) { /* Note: * In libstm32 times the priority of this interrupt was set to * preemption priority 2 and sub priority 1 diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c index a39b5c7e39..4a56067c5a 100644 --- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c @@ -65,7 +65,7 @@ int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; /** Set PWM channel configuration */ -static inline void actuators_pwm_arch_channel_init(u32 timer_peripheral, +static inline void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, enum tim_oc_id oc_id) { timer_disable_oc_output(timer_peripheral, oc_id); @@ -80,13 +80,13 @@ static inline void actuators_pwm_arch_channel_init(u32 timer_peripheral, /** Set GPIO configuration */ #if defined(STM32F4) -static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 af_num, u32 en) { +static inline void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t af_num, uint32_t en) { rcc_peripheral_enable_clock(&RCC_AHB1ENR, en); gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, pin); gpio_set_af(gpioport, af_num, pin); } #elif defined(STM32F1) -static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 none, u32 en) { +static inline void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t none, uint32_t en) { rcc_peripheral_enable_clock(&RCC_APB2ENR, en | RCC_APB2ENR_AFIOEN); gpio_set_mode(gpioport, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin); @@ -95,7 +95,7 @@ static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 none, u32 en) { /** Set Timer configuration */ -static inline void set_servo_timer(u32 timer, u32 period, u8 channels_mask) { +static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t channels_mask) { timer_reset(timer); /* Timer global mode: diff --git a/sw/airborne/firmwares/beth/overo_sfb_controller.c b/sw/airborne/firmwares/beth/overo_sfb_controller.c index e2ef819db0..66730bbbc9 100644 --- a/sw/airborne/firmwares/beth/overo_sfb_controller.c +++ b/sw/airborne/firmwares/beth/overo_sfb_controller.c @@ -187,4 +187,4 @@ void calc_sfb_cmd(void) { //if (_CO.cmd_thrust<0.) _CO.cmd_thrust = 0; Bound(_CO.cmd_sfb_thrust,0,100); Bound(_CO.cmd_sfb_pitch,-100,100); -} \ No newline at end of file +} From bf9cb65f286160498e39b46058e7cc148ad40d2f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Jun 2013 00:43:01 +0200 Subject: [PATCH 14/36] [dox] doxygen file header fixes --- .../rotorcraft/stabilization/stabilization_attitude.h | 2 +- sw/airborne/modules/ins/ins_xsens700.c | 5 +++-- sw/airborne/modules/meteo/humid_pcap01.c | 8 -------- sw/airborne/modules/sensors/airspeed_otf.c | 5 +++-- sw/airborne/modules/sensors/baro_ms5611_i2c.c | 5 +++-- sw/airborne/subsystems/imu/imu_um6.c | 2 +- sw/airborne/subsystems/navigation/traffic_info.c | 2 +- 7 files changed, 12 insertions(+), 17 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 09777e371b..7a0572f69e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -19,7 +19,7 @@ * Boston, MA 02111-1307, USA. */ -/** @file firmwares/rotorcraft/stabilization_attitude.h +/** @file firmwares/rotorcraft/stabilization/stabilization_attitude.h * General attitude stabilization interface for rotorcrafts. * The actual implementation is automatically included. */ diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index 2615487f69..e0090a233f 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -22,8 +22,9 @@ * */ -/** \file xsens.c - * \brief Parser for the Xsens protocol +/** + * @file modules/ins/ins_xsens700.c + * Parser for the Xsens protocol. */ #include "ins_module.h" diff --git a/sw/airborne/modules/meteo/humid_pcap01.c b/sw/airborne/modules/meteo/humid_pcap01.c index 6297c0065c..f3800e147f 100644 --- a/sw/airborne/modules/meteo/humid_pcap01.c +++ b/sw/airborne/modules/meteo/humid_pcap01.c @@ -198,14 +198,6 @@ void pcap01readRegister(uint8_t reg) * * function where current measurement data from pcap01 is read into * global sensor variable -* -* \param control Control command -* possible commands: -* PCAP01_PU_RESET : Hard reset of the device -* PCAP01_IN_RESET : Software reset -* PCAP01_START : Start measurement -* PCAP01_START : Start measurement -* PCAP01_TERM : Stop measurement */ void pcap01_periodic(void) { diff --git a/sw/airborne/modules/sensors/airspeed_otf.c b/sw/airborne/modules/sensors/airspeed_otf.c index 8a2048161d..094392af91 100644 --- a/sw/airborne/modules/sensors/airspeed_otf.c +++ b/sw/airborne/modules/sensors/airspeed_otf.c @@ -20,8 +20,9 @@ * */ -/** \file met_ap_otf.c - * \brief UART interface for Aeroprobe On-The-Fly! air data computer +/** + * @file modules/sensors/airspeed_otf.c + * UART interface for Aeroprobe On-The-Fly! air data computer. * */ diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index 2ba16cc9a4..fd82d068eb 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -20,8 +20,9 @@ * */ -/** \file baro_ms5611_i2c.c - * \brief Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C +/** + * @file modules/sensors/baro_ms5611_i2c.c + * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C. * */ diff --git a/sw/airborne/subsystems/imu/imu_um6.c b/sw/airborne/subsystems/imu/imu_um6.c index 00b3ba017a..079c3f34dc 100644 --- a/sw/airborne/subsystems/imu/imu_um6.c +++ b/sw/airborne/subsystems/imu/imu_um6.c @@ -26,7 +26,7 @@ * * Takes care of configuration of the IMU, communication and parsing * the received packets. See UM6 datasheet for configuration options. - * Should be used with @ahrs_extern_euler AHRS subsystem. + * Should be used with ahrs_extern_euler AHRS subsystem. * * @author Michal Podhradsky */ diff --git a/sw/airborne/subsystems/navigation/traffic_info.c b/sw/airborne/subsystems/navigation/traffic_info.c index 2d8fe65b10..faf3811eca 100644 --- a/sw/airborne/subsystems/navigation/traffic_info.c +++ b/sw/airborne/subsystems/navigation/traffic_info.c @@ -20,7 +20,7 @@ */ /** - * @file subsystems/naviation/traffic_info.c + * @file subsystems/navigation/traffic_info.c * * Information relative to the other aircrafts. * From 097c923082226f67d4180d9acb1c3e8e2ce845a6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Jun 2013 18:40:19 +0200 Subject: [PATCH 15/36] [conf] add pcre lib to NPS For some reason this needs to be explicitly specified for Ubuntu 13.04, while it was working without it on 12.04. --- conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index d4baff8f72..f57c246e8c 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -22,7 +22,7 @@ nps.MAKEFILE = nps nps.CFLAGS += -DSITL -DUSE_NPS nps.CFLAGS += $(shell pkg-config glib-2.0 --cflags) -nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy -lgsl -lgslcblas +nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy -lpcre -lgsl -lgslcblas nps.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps nps.LDFLAGS += $(shell sdl-config --libs) From e4a3316d8102b0ae27cc4e0843e27cab7426e99a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Jun 2013 20:52:43 +0200 Subject: [PATCH 16/36] [rotorcraft] add missing include so stabilization_attitude_euler_int can be used standalone --- .../rotorcraft/stabilization/stabilization_attitude_euler_int.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 12effe492a..f435e97a1f 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -24,6 +24,7 @@ #include "subsystems/radio_control.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "paparazzi.h" #include "generated/airframe.h" From ca67df9576404b536f49e86398fb07390afcf46f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Jun 2013 21:19:07 +0200 Subject: [PATCH 17/36] [rotorcraft] settings and rough example for float ahrs/stabilization --- conf/airframes/examples/quadrotor_mlkf.xml | 228 ++++++++++++++++++ .../control/stabilization_att_float_quat.xml | 27 +++ sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c | 10 +- sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h | 4 +- 4 files changed, 265 insertions(+), 4 deletions(-) create mode 100644 conf/airframes/examples/quadrotor_mlkf.xml create mode 100644 conf/settings/control/stabilization_att_float_quat.xml diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml new file mode 100644 index 0000000000..c4f1066f5f --- /dev/null +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -0,0 +1,228 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/settings/control/stabilization_att_float_quat.xml b/conf/settings/control/stabilization_att_float_quat.xml new file mode 100644 index 0000000000..e2e3c886e8 --- /dev/null +++ b/conf/settings/control/stabilization_att_float_quat.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index ad506d136c..2ad5bc5d43 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -23,7 +23,7 @@ /** * @file subsystems/ahrs/ahrs_float_mlkf.c * - * Mulitplicative linearized Kalman Filter in quaternion formulation. + * Multiplicative linearized Kalman Filter in quaternion formulation. * * Estimate the attitude, heading and gyro bias. */ @@ -46,6 +46,10 @@ //#include +#ifndef AHRS_PROPAGATE_FREQUENCY +#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY +#endif + static inline void propagate_ref(void); static inline void propagate_state(void); static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]); @@ -144,10 +148,12 @@ static inline void propagate_ref(void) { /* converts gyro to floating point */ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev); - // printf("propagate_ref %f\n", gyro_float.p); + /* unbias measurement */ RATES_SUB(gyro_float, ahrs_impl.gyro_bias); + #ifdef AHRS_PROPAGATE_LOW_PASS_RATES + /* lowpass angular rates */ const float alpha = 0.1; FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h index 5fa1b729f8..3b5552fcad 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h @@ -21,9 +21,9 @@ */ /** - * @file subsystems/ahrs/ahrs_float_mlkf.h + * @file subsystems/ahrs/ahrs_float_mlkf_opt.h * - * Mulitplicative linearized Kalman Filter in quaternion formulation. + * Multiplicative linearized Kalman Filter in quaternion formulation. * * Estimate the attitude, heading and gyro bias. */ From 6d2e303633b454e3877dc0a00ea8c364944f84ae Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 10 Jun 2013 15:36:21 +0200 Subject: [PATCH 18/36] [ground_segment] add joystick hat to input2ivy You can use the Hat() function to trigger events, where is one of Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown Only one hat can be used, but the interface will allow multiple ones in the future. closes #460 --- conf/joystick/ms_sidewinder.xml | 22 ++++--- conf/joystick/xbox_gamepad.xml | 67 +++++++++++++++++++++ sw/ground_segment/joystick/input2ivy.ml | 72 ++++++++++++++--------- sw/ground_segment/joystick/ml_sdl_stick.c | 11 ++-- sw/ground_segment/joystick/sdl_stick.c | 49 ++++++++++----- sw/ground_segment/joystick/sdl_stick.h | 1 + 6 files changed, 166 insertions(+), 56 deletions(-) create mode 100644 conf/joystick/xbox_gamepad.xml diff --git a/conf/joystick/ms_sidewinder.xml b/conf/joystick/ms_sidewinder.xml index 29c61b7734..04ac84f305 100644 --- a/conf/joystick/ms_sidewinder.xml +++ b/conf/joystick/ms_sidewinder.xml @@ -4,8 +4,6 @@ axis 1: pitch axis 2: yaw axis 3: throttle (reversed) - axis 4: hat switch left/right (right is positive) - axis 5: hat switch up/down (down is positive) It has 9 buttons. b0 - fire @@ -18,6 +16,11 @@ It has 9 buttons. b7 - button D b8 - shift button +and a POV hat. +You can use the Hat() function to trigger events, +where is one of +Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown +so e.g. HatDown(myhat) --> @@ -26,8 +29,6 @@ It has 9 buttons. - -
+ - - + +
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index ae91f2c2d1..1652b0b4ed 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -73,8 +73,13 @@ static inline int ahrs_is_aligned(void) { #include "autopilot_arming_yaw.h" #endif +#ifndef MODE_STARTUP +#define MODE_STARTUP AP_MODE_KILL +PRINT_CONFIG_MSG("Using AP_MODE_KILL as MODE_STARTUP") +#endif + void autopilot_init(void) { - autopilot_mode = AP_MODE_KILL; + autopilot_mode = MODE_STARTUP; autopilot_motors_on = FALSE; kill_throttle = ! autopilot_motors_on; autopilot_in_flight = FALSE; From fce87d02e1ae1483d78b8a2cdd9ad509eecd7e40 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 2 Jul 2013 17:20:19 +0200 Subject: [PATCH 32/36] [lpc21] print error if trying to use SPI0 with HAL --- sw/airborne/arch/lpc21/mcu_periph/spi_arch.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c index 60134d66ca..252ba5fa09 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c @@ -382,6 +382,7 @@ __attribute__ ((always_inline)) static inline void SpiSlaveAutomaton(struct spi_ #if SPI_MASTER #if USE_SPI0 +#error "SPI0 is currently not implemented in the mcu_periph/spi HAL for the LPC!" // void spi0_ISR(void) __attribute__((naked)); // From a89aac6894eeb45fb7dcd9d73f43074d8fcf7155 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 5 Jul 2013 16:05:37 +0200 Subject: [PATCH 33/36] [boards][krooz_sd] fix PPM Capture/Compare InterruptEnable define --- sw/airborne/boards/krooz_sd.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h index 6e6899bbce..90021b38a6 100644 --- a/sw/airborne/boards/krooz_sd.h +++ b/sw/airborne/boards/krooz_sd.h @@ -387,7 +387,7 @@ #define PPM_IRQ NVIC_TIM2_IRQ //#define PPM_IRQ2 NVIC_TIM2_UP_TIM10_IRQ // Capture/Compare InteruptEnable and InterruptFlag -#define PPM_CC_EN TIM_DIER_CC2IE +#define PPM_CC_IE TIM_DIER_CC2IE #define PPM_CC_IF TIM_SR_CC2IF #define PPM_GPIO_PORT GPIOB #define PPM_GPIO_PIN GPIO3 From 72ce97b0da7ef84fb06d311ae9d17c5d1d0a39e6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 8 Jul 2013 18:05:09 +0200 Subject: [PATCH 34/36] [stm32] include stm32/flash.h instead of f1/4 variants --- sw/airborne/arch/stm32/mcu_arch.c | 6 +----- sw/airborne/arch/stm32/subsystems/settings_arch.c | 7 +------ 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index d5d8be1b28..8066250927 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -33,11 +33,7 @@ #include #include #include -#if defined(STM32F1) -#include -#elif defined(STM32F4) -#include -#endif +#include #include #include "std.h" diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c index 411e97552d..3b814a5df3 100644 --- a/sw/airborne/arch/stm32/subsystems/settings_arch.c +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -35,12 +35,7 @@ #include "subsystems/settings.h" -#if defined(STM32F1) -#include -#elif defined(STM32F4) -#include -#endif - +#include #include #include From 82707b869b2e64222d2aaec64988276dcdf4e89e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 9 Jul 2013 11:34:43 +0200 Subject: [PATCH 35/36] [stm32] F4: fix ppm input timer frequency for TIM2 - krooz: don't use TIM2 for PWM - change to 6 ticks per usec to fit all frequencies - on the F1 we assume to run at 72MHz for HCLK and both timer clocks TIM1 -> APB2 = HCLK = 72MHz TIM2 -> 2 * APB1 = 2 * 36MHz = 72MHz - on the F4 we assume 2 * AHB clock: TIM1 -> 2 * APB2 = 168MHz TIM2 -> 2 * APB1 = 84MHz closes #470 --- .../stm32/subsystems/radio_control/ppm_arch.c | 32 +++++++++++++++++-- .../stm32/subsystems/radio_control/ppm_arch.h | 13 ++++++-- sw/airborne/boards/krooz_sd.h | 6 ++-- 3 files changed, 43 insertions(+), 8 deletions(-) diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c index b979a4e2d3..db20493eda 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c +++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c @@ -52,6 +52,20 @@ uint32_t ppm_last_pulse_time; bool_t ppm_data_valid; static uint32_t timer_rollover_cnt; +/* + * Timer clock frequency (before prescaling) is twice the frequency + * of the APB domain to which the timer is connected. + */ + +#ifdef STM32F1 +/** + * HCLK = 72MHz, Timer clock also 72MHz since + * TIM1_CLK = APB2 = 72MHz + * TIM2_CLK = 2 * APB1 = 2 * 32MHz + */ +#define PPM_TIMER_CLK AHB_CLK +#endif + #if USE_PPM_TIM2 PRINT_CONFIG_MSG("Using TIM2 for PPM input.") @@ -60,6 +74,14 @@ PRINT_CONFIG_MSG("Using TIM2 for PPM input.") #define PPM_PERIPHERAL RCC_APB1ENR_TIM2EN #define PPM_TIMER TIM2 +#ifdef STM32F4 +/* Since APB prescaler != 1 : + * Timer clock frequency (before prescaling) is twice the frequency + * of the APB domain to which the timer is connected. + */ +#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2) +#endif + #elif USE_PPM_TIM1 PRINT_CONFIG_MSG("Using TIM1 for PPM input.") @@ -68,6 +90,12 @@ PRINT_CONFIG_MSG("Using TIM1 for PPM input.") #define PPM_PERIPHERAL RCC_APB2ENR_TIM1EN #define PPM_TIMER TIM1 +#ifdef STM32F4 +#define PPM_TIMER_CLK (rcc_ppre2_frequency * 2) +#endif + +#else +#error Unknown PPM input timer configuration. #endif void ppm_arch_init ( void ) { @@ -86,7 +114,7 @@ void ppm_arch_init ( void ) { timer_set_mode(PPM_TIMER, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); timer_set_period(PPM_TIMER, 0xFFFF); - timer_set_prescaler(PPM_TIMER, (AHB_CLK / (RC_PPM_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1); + timer_set_prescaler(PPM_TIMER, (PPM_TIMER_CLK / (RC_PPM_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1); /* TIM configuration: Input Capture mode --------------------- The Rising edge is used as active edge @@ -96,7 +124,7 @@ void ppm_arch_init ( void ) { #elif defined PPM_PULSE_TYPE && PPM_PULSE_TYPE == PPM_PULSE_TYPE_NEGATIVE timer_ic_set_polarity(PPM_TIMER, PPM_CHANNEL, TIM_IC_FALLING); #else -#error "Unknown PM_PULSE_TYPE" +#error "Unknown PPM_PULSE_TYPE" #endif timer_ic_set_input(PPM_TIMER, PPM_CHANNEL, PPM_TIMER_INPUT); timer_ic_set_prescaler(PPM_TIMER, PPM_CHANNEL, TIM_IC_PSC_OFF); diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h index fbe1ece8da..4d866004f1 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h +++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h @@ -33,10 +33,17 @@ #include "mcu_periph/sys_time.h" /** - * The ppm counter is running at cpu freq / 72 or 168 / 8 - * so the counter has 1/8 us resolution + * The ppm counter is set-up to have 1/6 us resolution. + * + * The timer clock frequency (before prescaling): + * STM32F1: + * TIM1 -> APB2 = HCLK = 72MHz + * TIM2 -> 2 * APB1 = 2 * 36MHz = 72MHz + * STM32F4: + * TIM1 -> 2 * APB2 = 2 * 84MHz = 168MHz + * TIM2 -> 2 * APB1 = 2 * 42MHz = 84MHz */ -#define RC_PPM_TICKS_PER_USEC 8 +#define RC_PPM_TICKS_PER_USEC 6 #define RC_PPM_TICKS_OF_USEC(_v) ((_v)*RC_PPM_TICKS_PER_USEC) #define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (int32_t)((_v)*RC_PPM_TICKS_PER_USEC) diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h index 90021b38a6..d4fb6deab1 100644 --- a/sw/airborne/boards/krooz_sd.h +++ b/sw/airborne/boards/krooz_sd.h @@ -3,7 +3,7 @@ #define BOARD_KROOZ -/* Krooz/M has a 12MHz external clock and 168MHz internal. */ +/* KroozSD has a 12MHz external clock and 168MHz internal. */ #define EXT_CLK 12000000 #define AHB_CLK 168000000 @@ -210,7 +210,7 @@ #define BOARD_HAS_BARO 1 /* PWM */ -#define PWM_USE_TIM2 1 +//#define PWM_USE_TIM2 1 #define PWM_USE_TIM3 1 #define PWM_USE_TIM4 1 #define PWM_USE_TIM5 1 @@ -383,7 +383,7 @@ #define USE_PPM_TIM2 1 #define PPM_CHANNEL TIM_IC2 -#define PPM_TIMER_INPUT TIM_IC_IN_TI1 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 #define PPM_IRQ NVIC_TIM2_IRQ //#define PPM_IRQ2 NVIC_TIM2_UP_TIM10_IRQ // Capture/Compare InteruptEnable and InterruptFlag From 76ea10470167013fed28751a78ef3b409c591704 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 10 Jul 2013 00:10:49 +0200 Subject: [PATCH 36/36] [electrical] don't try to use ADC_CHANNEL_CURRENT in SITL --- sw/airborne/subsystems/electrical.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index 7ed3b06cb5..d0075cdd0c 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -55,7 +55,7 @@ void electrical_init(void) { #endif /* measure current if available, otherwise estimate it */ -#if defined ADC_CHANNEL_CURRENT +#if defined ADC_CHANNEL_CURRENT && !defined SITL adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE); #elif defined MILLIAMP_AT_FULL_THROTTLE PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)