From 4da8accfc060273455a084d306a046dec85a7fe7 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 6 Apr 2013 21:35:29 +0200 Subject: [PATCH] OLIMEXINO - Initial support for the OLIMEXINO board. PWM and ADC configuration is the same as the naze32 for now. Requires an accelerometer is connected via I2C2. I2C signals SDA2 and SCL2 are on the UEXT header (Pins 6 and 5 respectively) USART signals TX and RX are on the CON3/CON4 headers (TX = D7, RX = D8) Onboard LEDs (LED1/LED2) working. Buzzer support removed for now as it conflicted with PWM map and LEDs. mma845x support disabled as it conflicted with an LED. Relocated some NAZE specific comments. Disabling PWM2 as an input as it conflicts with the yellow onboard LED2. --- Makefile | 13 +++++++++++-- src/board.h | 47 ++++++++++++++++++++++++++++++++++++++++++----- src/drv_mma845x.c | 3 ++- src/drv_pwm.c | 10 ++++++++-- src/drv_system.c | 10 +++++----- src/main.c | 8 +------- src/sensors.c | 6 ++++-- 7 files changed, 73 insertions(+), 24 deletions(-) diff --git a/Makefile b/Makefile index 8477761c7..80ff2822e 100644 --- a/Makefile +++ b/Makefile @@ -14,7 +14,7 @@ # Things that the user might override on the commandline # -# The target to build, must be one of NAZE or FY90Q +# The target to build, must be one of NAZE, FY90Q OR OLIMEXINO TARGET ?= NAZE # Compile-time options @@ -24,7 +24,7 @@ OPTIONS ?= # Things that need to be maintained as the source changes # -VALID_TARGETS = NAZE FY90Q +VALID_TARGETS = NAZE FY90Q OLIMEXINO # Working directories ROOT = $(dir $(lastword $(MAKEFILE_LIST))) @@ -76,6 +76,15 @@ FY90Q_SRC = drv_adc_fy90q.c \ drv_pwm_fy90q.c \ $(COMMON_SRC) +# Source files for the OLIMEXINO target +OLIMEXINO_SRC = drv_adc.c \ + drv_adxl345.c \ + drv_mpu3050.c \ + drv_mpu6050.c \ + drv_l3g4200d.c \ + drv_pwm.c \ + $(COMMON_SRC) + # Search path for baseflight sources VPATH := $(SRC_DIR):$(SRC_DIR)/baseflight_startups diff --git a/src/board.h b/src/board.h index 798cea628..82e2ff5de 100755 --- a/src/board.h +++ b/src/board.h @@ -112,14 +112,34 @@ typedef struct baro_t #define GYRO #define ACC -#else +#define SENSORS_SET (SENSOR_ACC) + +#endif + +#ifdef OLIMEXINO +// OLIMEXINO + +// LED2 is using one of the pwm pins (PWM2), so we must not use PWM2. @See pwmInit() +#define LED0_GPIO GPIOA +#define LED0_PIN GPIO_Pin_1 // D3, PA1/USART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow +#define LED1_GPIO GPIOA +#define LED1_PIN GPIO_Pin_5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green + +#define GYRO +#define ACC + +#define SENSORS_SET (SENSOR_ACC) + +#endif + +#ifdef NAZE // Afroflight32 #define LED0_GPIO GPIOB -#define LED0_PIN GPIO_Pin_3 +#define LED0_PIN GPIO_Pin_3 // PB3 (LED) #define LED1_GPIO GPIOB -#define LED1_PIN GPIO_Pin_4 +#define LED1_PIN GPIO_Pin_4 // PB4 (LED) #define BEEP_GPIO GPIOA -#define BEEP_PIN GPIO_Pin_12 +#define BEEP_PIN GPIO_Pin_12 // PA12 (Buzzer) #define BARO_GPIO GPIOC #define BARO_PIN GPIO_Pin_13 @@ -129,6 +149,9 @@ typedef struct baro_t #define BARO #define LEDRING #define SONAR +#define BUZZER + +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #endif @@ -160,8 +183,22 @@ typedef struct baro_t #include "drv_i2c.h" #include "drv_pwm.h" #include "drv_uart.h" +#endif -#else +#ifdef OLIMEXINO +// OLIMEXINO +#include "drv_system.h" // timers, delays, etc +#include "drv_adc.h" +#include "drv_i2c.h" +#include "drv_adxl345.h" +#include "drv_mpu3050.h" +#include "drv_mpu6050.h" +#include "drv_l3g4200d.h" +#include "drv_pwm.h" +#include "drv_uart.h" +#endif + +#ifdef NAZE // AfroFlight32 #include "drv_system.h" // timers, delays, etc #include "drv_adc.h" diff --git a/src/drv_mma845x.c b/src/drv_mma845x.c index 6affd61d2..256acf63c 100644 --- a/src/drv_mma845x.c +++ b/src/drv_mma845x.c @@ -75,6 +75,7 @@ static void mma8452Init(void) GPIO_InitTypeDef GPIO_InitStructure; // PA5 - ACC_INT2 output on rev3/4 hardware + // OLIMEXINO - The PA5 pin is wired up to LED1, if you need to use an mma8452 on an Olimexino use a different pin and provide support in code. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; @@ -88,7 +89,7 @@ static void mma8452Init(void) i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver) i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G. - + acc_1G = 256; } diff --git a/src/drv_pwm.c b/src/drv_pwm.c index 90c385c94..df019fc05 100755 --- a/src/drv_pwm.c +++ b/src/drv_pwm.c @@ -48,7 +48,7 @@ PWM5..8 used for motors PWM9..10 used for servo or else motors PWM11..14 used for motors - + 2) multirotor PPM input with more servos PWM1 used for PPM PWM5..8 used for motors @@ -352,7 +352,7 @@ void TIM1_CC_IRQHandler(void) static void pwmTIMxHandler(TIM_TypeDef *tim, uint8_t portBase) { int8_t port; - + // Generic CC handler for TIM2,3,4 if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) { port = portBase + 0; @@ -449,6 +449,12 @@ bool pwmInit(drv_pwm_config_t *init) if (setup[i] == 0xFF) // terminator break; +#ifdef OLIMEXINO + // PWM2 is connected to LED2 on the board and cannot be connected. + if (port == PWM2) + continue; +#endif + // skip UART ports for GPS if (init->useUART && (port == PWM3 || port == PWM4)) continue; diff --git a/src/drv_system.c b/src/drv_system.c index 4a6b2000f..d19b0ada6 100755 --- a/src/drv_system.c +++ b/src/drv_system.c @@ -48,10 +48,10 @@ void systemInit(void) uint32_t i; gpio_config_t gpio_cfg[] = { - { LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP }, // PB3 (LED) - { LED1_GPIO, LED1_PIN, GPIO_Mode_Out_PP }, // PB4 (LED) -#ifndef FY90Q - { BEEP_GPIO, BEEP_PIN, GPIO_Mode_Out_OD }, // PA12 (Buzzer) + { LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP }, + { LED1_GPIO, LED1_PIN, GPIO_Mode_Out_PP }, +#ifdef BUZZER + { BEEP_GPIO, BEEP_PIN, GPIO_Mode_Out_OD }, #endif }; uint8_t gpio_count = sizeof(gpio_cfg) / sizeof(gpio_cfg[0]); @@ -164,7 +164,7 @@ void systemReset(bool toBootloader) // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC *((uint32_t *)0x20004FF0) = 0xDEADBEEF; // 20KB STM32F103 - } + } // Generate system reset SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; diff --git a/src/main.c b/src/main.c index cb07356db..6e1d4d47e 100755 --- a/src/main.c +++ b/src/main.c @@ -58,13 +58,7 @@ int main(void) adcInit(&adc_params); // We have these sensors -#ifndef FY90Q - // AfroFlight32 - sensorsSet(SENSOR_ACC | SENSOR_BARO | SENSOR_MAG); -#else - // FY90Q - sensorsSet(SENSOR_ACC); -#endif + sensorsSet(SENSORS_SET); mixerInit(); // this will set useServo var depending on mixer type // when using airplane/wing mixer, servo/motor outputs are remapped diff --git a/src/sensors.c b/src/sensors.c index 95fd779ec..b30a91088 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -67,12 +67,14 @@ retry: break; } ; // fallthrough +#ifndef OLIMEXINO case 3: // MMA8452 if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; if (mcfg.acc_hardware == ACC_MMA8452) break; } +#endif } // Found anything? Check if user fucked up or ACC is really missing. @@ -481,14 +483,14 @@ int Mag_getADC(void) #ifdef SONAR -void Sonar_init(void) +void Sonar_init(void) { hcsr04_init(sonar_rc78); sensorsSet(SENSOR_SONAR); sonarAlt = 0; } -void Sonar_update(void) +void Sonar_update(void) { hcsr04_get_distance(&sonarAlt); }