mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 23:46:04 +08:00
peripheral test programs
This commit is contained in:
Antoine Drouin
@@ -0,0 +1,111 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "init_hw.h"
|
||||
#include "sys_time.h"
|
||||
#include "led.h"
|
||||
#include "uart.h"
|
||||
#include "messages.h"
|
||||
#include "downlink.h"
|
||||
|
||||
#include "booz_imu.h"
|
||||
|
||||
#include "interrupt_hw.h"
|
||||
|
||||
|
||||
static inline void main_init( void );
|
||||
static inline void main_periodic_task( void );
|
||||
static inline void main_event_task( void );
|
||||
|
||||
static inline void on_gyro_accel_event(void);
|
||||
static inline void on_mag_event(void);
|
||||
|
||||
int main( void ) {
|
||||
main_init();
|
||||
while(1) {
|
||||
if (sys_time_periodic())
|
||||
main_periodic_task();
|
||||
main_event_task();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void main_init( void ) {
|
||||
|
||||
hw_init();
|
||||
sys_time_init();
|
||||
booz_imu_init();
|
||||
|
||||
int_enable();
|
||||
}
|
||||
|
||||
static inline void main_periodic_task( void ) {
|
||||
RunOnceEvery(100, {
|
||||
LED_TOGGLE(3);
|
||||
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
|
||||
});
|
||||
booz_imu_periodic();
|
||||
RunOnceEvery(10, { LED_PERIODIC();});
|
||||
}
|
||||
|
||||
static inline void main_event_task( void ) {
|
||||
|
||||
BoozImuEvent(on_gyro_accel_event, on_mag_event);
|
||||
|
||||
}
|
||||
|
||||
#define NB_SAMPLES 20
|
||||
|
||||
static inline void on_gyro_accel_event(void) {
|
||||
BoozImuScaleGyro();
|
||||
BoozImuScaleAccel();
|
||||
|
||||
LED_TOGGLE(2);
|
||||
|
||||
static uint8_t cnt;
|
||||
static int32_t samples[NB_SAMPLES];
|
||||
const uint8_t axis = 2;
|
||||
cnt++;
|
||||
if (cnt > NB_SAMPLES) cnt = 0;
|
||||
samples[cnt] = booz_imu.gyro_unscaled.r;
|
||||
if (cnt == 19) {
|
||||
DOWNLINK_SEND_IMU_HS_GYRO(DefaultChannel, &axis, NB_SAMPLES, samples);
|
||||
}
|
||||
|
||||
if (cnt == 10) {
|
||||
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
|
||||
&booz_imu.gyro_unscaled.p,
|
||||
&booz_imu.gyro_unscaled.q,
|
||||
&booz_imu.gyro_unscaled.r);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
static inline void on_mag_event(void) {
|
||||
|
||||
}
|
||||
@@ -32,8 +32,8 @@
|
||||
#include "sys_time.h"
|
||||
#include "downlink.h"
|
||||
|
||||
#include "peripherals/booz_itg3200.h"
|
||||
#include "peripherals/booz_hmc5843.h"
|
||||
#include "peripherals/booz_adxl345.h"
|
||||
#include "my_debug_servo.h"
|
||||
|
||||
static inline void main_init( void );
|
||||
static inline void main_periodic_task( void );
|
||||
@@ -41,7 +41,7 @@ static inline void main_event_task( void );
|
||||
|
||||
static inline void main_init_hw(void);
|
||||
|
||||
extern void exti4_irq_handler(void);
|
||||
void exti2_irq_handler(void);
|
||||
|
||||
int main(void) {
|
||||
main_init();
|
||||
@@ -64,25 +64,11 @@ static inline void main_init( void ) {
|
||||
|
||||
static void write_to_reg(uint8_t addr, uint8_t val);
|
||||
static uint8_t read_fom_reg(uint8_t addr);
|
||||
|
||||
static uint8_t foo=0;
|
||||
static volatile uint8_t inted = FALSE;
|
||||
#define CONFIGURED 6
|
||||
static uint8_t acc_status=0;
|
||||
static volatile uint8_t acc_ready_for_read = FALSE;
|
||||
static uint8_t values[6];
|
||||
|
||||
|
||||
#define ADXL345_REG_BW_RATE 0x2C
|
||||
#define ADXL345_REG_POWER_CTL 0x2D
|
||||
#define ADXL345_REG_INT_ENABLE 0x2E
|
||||
#define ADXL345_REG_DATA_FORMAT 0x31
|
||||
#define ADXL345_REG_DATA_X0 0x32
|
||||
#define ADXL345_REG_DATA_X1 0x33
|
||||
#define ADXL345_REG_DATA_Y0 0x34
|
||||
#define ADXL345_REG_DATA_Y1 0x35
|
||||
#define ADXL345_REG_DATA_Z0 0x36
|
||||
#define ADXL345_REG_DATA_Z1 0x37
|
||||
|
||||
|
||||
|
||||
#define AccUnselect() GPIOB->BSRR = GPIO_Pin_12
|
||||
#define AccSelect() GPIOB->BRR = GPIO_Pin_12
|
||||
#define AccToggleSelect() GPIOB->ODR ^= GPIO_Pin_12
|
||||
@@ -116,7 +102,7 @@ static void read_data(void) {
|
||||
SPI_I2S_SendData(SPI2, (1<<7|1<<6|ADXL345_REG_DATA_X0));
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
|
||||
uint8_t foo = SPI_I2S_ReceiveData(SPI2);
|
||||
uint8_t __attribute__ ((unused)) foo = SPI_I2S_ReceiveData(SPI2);
|
||||
|
||||
SPI_I2S_SendData(SPI2, 0x00);
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
|
||||
@@ -154,54 +140,57 @@ static inline void main_periodic_task( void ) {
|
||||
|
||||
|
||||
RunOnceEvery(10,
|
||||
{
|
||||
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
|
||||
LED_PERIODIC();
|
||||
});
|
||||
{
|
||||
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
|
||||
LED_PERIODIC();
|
||||
});
|
||||
|
||||
switch (foo) {
|
||||
switch (acc_status) {
|
||||
case 1:
|
||||
{
|
||||
/* read data rate */
|
||||
// uint8_t bar = read_fom_reg(ADXL345_REG_BW_RATE);
|
||||
write_to_reg(ADXL345_REG_BW_RATE, 0x0D);
|
||||
}
|
||||
/* set data rate to 800Hz */
|
||||
write_to_reg(ADXL345_REG_BW_RATE, 0x0D);
|
||||
break;
|
||||
case 2:
|
||||
/* switch to measurememnt mode */
|
||||
write_to_reg(ADXL345_REG_POWER_CTL, 1<<3);
|
||||
break;
|
||||
case 3:
|
||||
/* switch to measurememnt mode */
|
||||
/* enable data ready interrupt */
|
||||
write_to_reg(ADXL345_REG_INT_ENABLE, 1<<7);
|
||||
break;
|
||||
case 4:
|
||||
/* Enable full res and interrupt active low */
|
||||
write_to_reg(ADXL345_REG_DATA_FORMAT, 1<<3|1<<5);
|
||||
break;
|
||||
case 5:
|
||||
/* reads data once to bring interrupt line up */
|
||||
read_data();
|
||||
break;
|
||||
case CONFIGURED:
|
||||
// read_data();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (foo < 5) foo++;
|
||||
if (acc_status < CONFIGURED) acc_status++;
|
||||
|
||||
}
|
||||
|
||||
|
||||
static inline void main_event_task( void ) {
|
||||
|
||||
if (inted) {
|
||||
LED_TOGGLE(6);
|
||||
if (acc_status >= CONFIGURED && acc_ready_for_read) {
|
||||
read_data();
|
||||
inted = FALSE;
|
||||
int16_t* iax = &values[0];
|
||||
int16_t* iay = &values[2];
|
||||
int16_t* iaz = &values[4];
|
||||
RunOnceEvery(10, {DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, iax, iay, iaz);});
|
||||
//uint16_t* ax = &values[0];
|
||||
//uint16_t* ay = &values[2];
|
||||
//uint16_t* az = &values[4];
|
||||
//RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, ax, ay, az);});
|
||||
acc_ready_for_read = FALSE;
|
||||
int32_t iax = *((int16_t*)&values[0]);
|
||||
int32_t iay = *((int16_t*)&values[2]);
|
||||
int32_t iaz = *((int16_t*)&values[4]);
|
||||
RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, &iax, &iay, &iaz);});
|
||||
}
|
||||
|
||||
}
|
||||
@@ -264,6 +253,7 @@ static inline void main_init_hw( void ) {
|
||||
SPI_InitStructure.SPI_CRCPolynomial = 7;
|
||||
SPI_Init(SPI2, &SPI_InitStructure);
|
||||
|
||||
DEBUG_SERVO2_INIT();
|
||||
|
||||
}
|
||||
|
||||
@@ -274,10 +264,10 @@ void exti2_irq_handler(void) {
|
||||
if(EXTI_GetITStatus(EXTI_Line2) != RESET)
|
||||
EXTI_ClearITPendingBit(EXTI_Line2);
|
||||
|
||||
// AccToggleSelect();
|
||||
LED_ON(6);
|
||||
inted = TRUE;
|
||||
DEBUG_S4_TOGGLE();
|
||||
|
||||
acc_ready_for_read = TRUE;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,303 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#include <stm32/gpio.h>
|
||||
#include <stm32/flash.h>
|
||||
#include <stm32/misc.h>
|
||||
#include <stm32/exti.h>
|
||||
#include <stm32/spi.h>
|
||||
#include <stm32/dma.h>
|
||||
|
||||
#include BOARD_CONFIG
|
||||
#include "init_hw.h"
|
||||
#include "sys_time.h"
|
||||
#include "downlink.h"
|
||||
|
||||
#include "peripherals/booz_adxl345.h"
|
||||
#include "my_debug_servo.h"
|
||||
|
||||
static inline void main_init( void );
|
||||
static inline void main_periodic_task( void );
|
||||
static inline void main_event_task( void );
|
||||
|
||||
static inline void main_init_hw(void);
|
||||
|
||||
void exti2_irq_handler(void);
|
||||
void dma1_c4_irq_handler(void);
|
||||
|
||||
int main(void) {
|
||||
main_init();
|
||||
|
||||
while(1) {
|
||||
if (sys_time_periodic())
|
||||
main_periodic_task();
|
||||
main_event_task();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void main_init( void ) {
|
||||
hw_init();
|
||||
sys_time_init();
|
||||
main_init_hw();
|
||||
|
||||
}
|
||||
|
||||
static void write_to_reg(uint8_t addr, uint8_t val);
|
||||
static uint8_t read_fom_reg(uint8_t addr);
|
||||
#define CONFIGURED 6
|
||||
static uint8_t acc_status=0;
|
||||
static volatile uint8_t acc_data_available = FALSE;
|
||||
|
||||
static uint8_t dma_tx_buf[7];
|
||||
static uint8_t dma_rx_buf[7];
|
||||
|
||||
#define AccUnselect() GPIOB->BSRR = GPIO_Pin_12
|
||||
#define AccSelect() GPIOB->BRR = GPIO_Pin_12
|
||||
#define AccToggleSelect() GPIOB->ODR ^= GPIO_Pin_12
|
||||
|
||||
static void write_to_reg(uint8_t addr, uint8_t val) {
|
||||
|
||||
AccSelect();
|
||||
SPI_I2S_SendData(SPI2, addr);
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
|
||||
SPI_I2S_SendData(SPI2, val);
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
|
||||
AccUnselect();
|
||||
|
||||
}
|
||||
|
||||
static uint8_t read_fom_reg(uint8_t addr) {
|
||||
AccSelect();
|
||||
SPI_I2S_SendData(SPI2, (1<<7|addr));
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
|
||||
SPI_I2S_SendData(SPI2, 0x00);
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
|
||||
while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
|
||||
uint8_t ret = SPI_I2S_ReceiveData(SPI2);
|
||||
AccUnselect();
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void read_data(void) {
|
||||
AccSelect();
|
||||
|
||||
dma_tx_buf[0] = (1<<7|1<<6|ADXL345_REG_DATA_X0);
|
||||
|
||||
/* SPI2_Rx_DMA_Channel configuration ------------------------------------*/
|
||||
DMA_DeInit(DMA1_Channel4);
|
||||
DMA_InitTypeDef DMA_initStructure_4 = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
|
||||
.DMA_MemoryBaseAddr = (uint32_t)dma_rx_buf,
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_BufferSize = 7,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_VeryHigh,
|
||||
.DMA_M2M = DMA_M2M_Disable
|
||||
};
|
||||
DMA_Init(DMA1_Channel4, &DMA_initStructure_4);
|
||||
|
||||
/* SPI2_Tx_DMA_Channel configuration ------------------------------------*/
|
||||
DMA_DeInit(DMA1_Channel5);
|
||||
DMA_InitTypeDef DMA_initStructure_5 = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
|
||||
.DMA_MemoryBaseAddr = (uint32_t)dma_tx_buf,
|
||||
.DMA_DIR = DMA_DIR_PeripheralDST,
|
||||
.DMA_BufferSize = 7,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable
|
||||
};
|
||||
DMA_Init(DMA1_Channel5, &DMA_initStructure_5);
|
||||
|
||||
/* Enable SPI_2 Rx request */
|
||||
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, ENABLE);
|
||||
/* Enable DMA1 Channel4 */
|
||||
DMA_Cmd(DMA1_Channel4, ENABLE);
|
||||
|
||||
/* Enable SPI_2 Tx request */
|
||||
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, ENABLE);
|
||||
/* Enable DMA1 Channel5 */
|
||||
DMA_Cmd(DMA1_Channel5, ENABLE);
|
||||
|
||||
/* Enable DMA1 Channel4 Transfer Complete interrupt */
|
||||
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
|
||||
|
||||
}
|
||||
|
||||
|
||||
static inline void main_periodic_task( void ) {
|
||||
|
||||
|
||||
RunOnceEvery(10,
|
||||
{
|
||||
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
|
||||
LED_PERIODIC();
|
||||
});
|
||||
|
||||
if (acc_status != CONFIGURED) {
|
||||
{
|
||||
/* read data rate */
|
||||
// uint8_t bar = read_fom_reg(ADXL345_REG_BW_RATE);
|
||||
}
|
||||
/* set data rate to 800Hz */
|
||||
write_to_reg(ADXL345_REG_BW_RATE, 0x0D);
|
||||
/* switch to measurememnt mode */
|
||||
write_to_reg(ADXL345_REG_POWER_CTL, 1<<3);
|
||||
/* enable data ready interrupt */
|
||||
write_to_reg(ADXL345_REG_INT_ENABLE, 1<<7);
|
||||
/* Enable full res and interrupt active low */
|
||||
write_to_reg(ADXL345_REG_DATA_FORMAT, 1<<3|1<<5);
|
||||
/* reads data once to bring interrupt line up */
|
||||
uint8_t ret = SPI_I2S_ReceiveData(SPI2);
|
||||
read_data();
|
||||
acc_status = CONFIGURED;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
static inline void main_event_task( void ) {
|
||||
|
||||
if (acc_status >= CONFIGURED && acc_data_available) {
|
||||
acc_data_available = FALSE;
|
||||
int16_t ax = dma_rx_buf[1] | (dma_rx_buf[2]<<8);
|
||||
int16_t ay = dma_rx_buf[3] | (dma_rx_buf[4]<<8);
|
||||
int16_t az = dma_rx_buf[5] | (dma_rx_buf[6]<<8);
|
||||
int32_t iax = ax;
|
||||
int32_t iay = ay;
|
||||
int32_t iaz = az;
|
||||
RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, &iax, &iay, &iaz);});
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static inline void main_init_hw( void ) {
|
||||
|
||||
/* configure acc slave select */
|
||||
/* set acc slave select as output and assert it ( on PB12) */
|
||||
AccUnselect();
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
/* configure external interrupt exti2 on PD2( accel int ) */
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD | RCC_APB2Periph_AFIO, ENABLE);
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(GPIOD, &GPIO_InitStructure);
|
||||
EXTI_InitTypeDef EXTI_InitStructure;
|
||||
GPIO_EXTILineConfig(GPIO_PortSourceGPIOD, GPIO_PinSource2);
|
||||
EXTI_InitStructure.EXTI_Line = EXTI_Line2;
|
||||
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
|
||||
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
|
||||
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
|
||||
EXTI_Init(&EXTI_InitStructure);
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
|
||||
|
||||
/* Enable SPI2 Periph clock -------------------------------------------------*/
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
|
||||
|
||||
/* Configure GPIOs: SCK, MISO and MOSI --------------------------------*/
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
|
||||
SPI_Cmd(SPI2, ENABLE);
|
||||
|
||||
/* configure SPI */
|
||||
SPI_InitTypeDef SPI_InitStructure;
|
||||
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
|
||||
SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
|
||||
SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
|
||||
SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
|
||||
SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
|
||||
SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
|
||||
SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32;
|
||||
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
|
||||
SPI_InitStructure.SPI_CRCPolynomial = 7;
|
||||
SPI_Init(SPI2, &SPI_InitStructure);
|
||||
|
||||
/* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
|
||||
NVIC_InitTypeDef NVIC_init_struct = {
|
||||
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = 0,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE
|
||||
};
|
||||
NVIC_Init(&NVIC_init_struct);
|
||||
|
||||
/* Enable SPI_2 DMA clock ---------------------------------------------------*/
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
|
||||
|
||||
DEBUG_SERVO2_INIT();
|
||||
|
||||
}
|
||||
|
||||
|
||||
void exti2_irq_handler(void) {
|
||||
|
||||
/* clear EXTI */
|
||||
if(EXTI_GetITStatus(EXTI_Line2) != RESET)
|
||||
EXTI_ClearITPendingBit(EXTI_Line2);
|
||||
|
||||
DEBUG_S4_TOGGLE();
|
||||
|
||||
read_data();
|
||||
|
||||
}
|
||||
|
||||
void dma1_c4_irq_handler(void) {
|
||||
AccUnselect();
|
||||
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
|
||||
/* Disable SPI_2 Rx and TX request */
|
||||
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE);
|
||||
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE);
|
||||
/* Disable DMA1 Channel4 and 5 */
|
||||
DMA_Cmd(DMA1_Channel4, DISABLE);
|
||||
DMA_Cmd(DMA1_Channel5, DISABLE);
|
||||
|
||||
acc_data_available = TRUE;
|
||||
}
|
||||
@@ -32,6 +32,7 @@
|
||||
#include "sys_time.h"
|
||||
#include "downlink.h"
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
|
||||
#include "peripherals/booz_itg3200.h"
|
||||
#include "my_debug_servo.h"
|
||||
@@ -76,7 +77,9 @@ static inline void main_periodic_task( void ) {
|
||||
LED_PERIODIC();
|
||||
});
|
||||
RunOnceEvery(256,
|
||||
{DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, &i2c2.errc_ack_fail, &i2c2.errc_miss_start_stop,
|
||||
{DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
|
||||
&i2c2.got_unexpected_event,
|
||||
&i2c2.errc_ack_fail, &i2c2.errc_miss_start_stop,
|
||||
&i2c2.errc_arb_lost, &i2c2.errc_over_under,
|
||||
&i2c2.errc_pec_recep, &i2c2.errc_timeout_tlow,
|
||||
&i2c2.errc_smbus_alert);
|
||||
@@ -103,9 +106,9 @@ static inline void main_periodic_task( void ) {
|
||||
break;
|
||||
case INITIALISZED:
|
||||
/* reads 8 bytes from address 0x1b */
|
||||
i2c2.buf[0] = ITG3200_REG_TEMP_OUT_H;
|
||||
i2c2_transceive(ITG3200_ADDR,1, 8, &i2c_done);
|
||||
reading_gyro = TRUE;
|
||||
// i2c2.buf[0] = ITG3200_REG_TEMP_OUT_H;
|
||||
// i2c2_transceive(ITG3200_ADDR,1, 8, &i2c_done);
|
||||
// reading_gyro = TRUE;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -118,21 +121,24 @@ static inline void main_periodic_task( void ) {
|
||||
static inline void main_event_task( void ) {
|
||||
|
||||
if (gyro_state == INITIALISZED && gyro_ready_for_read) {
|
||||
/* read gyros */
|
||||
/* reads 8 bytes from address 0x1b */
|
||||
// i2c2.buf[0] = ITG3200_REG_TEMP_OUT_H;
|
||||
// i2c2_transceive(ITG3200_ADDR,1, 8, &i2c_done);
|
||||
i2c2.buf[0] = ITG3200_REG_TEMP_OUT_H;
|
||||
i2c2_transceive(ITG3200_ADDR,1, 8, &i2c_done);
|
||||
gyro_ready_for_read = FALSE;
|
||||
reading_gyro = TRUE;
|
||||
}
|
||||
|
||||
if (reading_gyro && i2c_done) {
|
||||
RunOnceEvery(10,
|
||||
{
|
||||
int16_t temp = i2c2.buf[0]<<8 | i2c2.buf[1];
|
||||
int16_t gp = i2c2.buf[2]<<8 | i2c2.buf[3];
|
||||
int16_t gq = i2c2.buf[4]<<8 | i2c2.buf[5];
|
||||
int16_t gr = i2c2.buf[6]<<8 | i2c2.buf[7];
|
||||
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, &gp, &gq, &gr);
|
||||
int16_t ttemp = i2c2.buf[0]<<8 | i2c2.buf[1];
|
||||
int16_t tgp = i2c2.buf[2]<<8 | i2c2.buf[3];
|
||||
int16_t tgq = i2c2.buf[4]<<8 | i2c2.buf[5];
|
||||
int16_t tgr = i2c2.buf[6]<<8 | i2c2.buf[7];
|
||||
int32_t temp = ttemp;
|
||||
struct Int32Rates g;
|
||||
RATES_ASSIGN(g, tgp, tgq, tgr);
|
||||
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, &g.p, &g.q, &g.r);
|
||||
// uint8_t tmp[8];
|
||||
// memcpy(tmp, i2c2.buf, 8);
|
||||
// DOWNLINK_SEND_DEBUG(DefaultChannel, 8, tmp);
|
||||
@@ -161,40 +167,40 @@ static inline void main_init_hw( void ) {
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
|
||||
GPIO_Init(GPIOC, &GPIO_InitStructure);
|
||||
|
||||
/* configure external interrupt exti2 on PC14( gyro int ) */
|
||||
/* configure external interrupt exti15_10 on PC14( gyro int ) */
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(GPIOC, &GPIO_InitStructure);
|
||||
|
||||
GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
|
||||
EXTI_InitTypeDef EXTI_InitStructure;
|
||||
EXTI_InitStructure.EXTI_Line = EXTI_Line2;
|
||||
EXTI_InitStructure.EXTI_Line = EXTI_Line14;
|
||||
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
|
||||
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
|
||||
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
|
||||
EXTI_Init(&EXTI_InitStructure);
|
||||
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
// NVIC_Init(&NVIC_InitStructure);
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
|
||||
DEBUG_SERVO2_INIT();
|
||||
|
||||
}
|
||||
|
||||
|
||||
void exti2_irq_handler(void) {
|
||||
void exti15_10_irq_handler(void) {
|
||||
|
||||
/* clear EXTI */
|
||||
if(EXTI_GetITStatus(EXTI_Line2) != RESET)
|
||||
EXTI_ClearITPendingBit(EXTI_Line2);
|
||||
if(EXTI_GetITStatus(EXTI_Line14) != RESET)
|
||||
EXTI_ClearITPendingBit(EXTI_Line14);
|
||||
|
||||
DEBUG_S4_TOGGLE();
|
||||
// DEBUG_S4_TOGGLE();
|
||||
|
||||
gyro_ready_for_read = TRUE;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user