Skip to content

Commit

Permalink
Removed more target specific conditionals from the main codebase
Browse files Browse the repository at this point in the history
  • Loading branch information
blckmn committed Feb 19, 2017
1 parent 981c045 commit 8e9be42
Show file tree
Hide file tree
Showing 18 changed files with 176 additions and 121 deletions.
10 changes: 4 additions & 6 deletions src/main/drivers/accgyro_mma845x.c
Original file line number Diff line number Diff line change
Expand Up @@ -83,12 +83,10 @@ static uint8_t device_id;

static inline void mma8451ConfigureInterrupt(void)
{
#ifdef NAZE
// PA5 - ACC_INT2 output on NAZE rev3/4 hardware
// NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
// 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.
IOInit(IOGetByTag(IO_TAG(PA5)), OWNER_MPU_EXTI, 0);
IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
#ifdef MMA8451_INT_PIN
IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0);
// TODO - maybe pullup / pulldown ?
IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING);
#endif

i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
Expand Down
21 changes: 11 additions & 10 deletions src/main/drivers/compass_hmc5883l.c
Original file line number Diff line number Diff line change
Expand Up @@ -119,11 +119,9 @@

static float magGain[3] = { 1.0f, 1.0f, 1.0f };

static const hmc5883Config_t *hmc5883Config = NULL;

#ifdef USE_MAG_DATA_READY_SIGNAL

static IO_t intIO;
static IO_t hmc5883InterruptIO;
static extiCallbackRec_t hmc5883_extiCallbackRec;

static void hmc5883_extiHandler(extiCallbackRec_t* cb)
Expand All @@ -150,20 +148,19 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void)
{
#ifdef USE_MAG_DATA_READY_SIGNAL

if (!(hmc5883Config->intTag)) {
if (!(hmc5883InterruptIO)) {
return;
}
intIO = IOGetByTag(hmc5883Config->intTag);
#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
uint8_t status = IORead(intIO);
uint8_t status = IORead(hmc5883InterruptIO);
if (!status) {
return;
}
#endif

EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler);
EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(intIO, true);
EXTIConfig(hmc5883InterruptIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(hmc5883InterruptIO, true);
#endif
}

Expand Down Expand Up @@ -257,9 +254,13 @@ static bool hmc5883lInit(void)
return true;
}

bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag)
{
hmc5883Config = hmc5883ConfigToUse;
#ifdef USE_MAG_DATA_READY_SIGNAL
hmc5883InterruptIO = IOGetByTag(interruptTag);
#else
UNUSED(interruptTag);
#endif

uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
Expand Down
6 changes: 1 addition & 5 deletions src/main/drivers/compass_hmc5883l.h
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,4 @@

#include "io_types.h"

typedef struct hmc5883Config_s {
ioTag_t intTag;
} hmc5883Config_t;

bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag);
2 changes: 1 addition & 1 deletion src/main/drivers/vtx_rtc6705.c
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@
#define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
#define ENABLE_RTC6705 GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)

#if defined(SPRACINGF3NEO)
#ifdef RTC6705_POWER_PIN
static IO_t vtxPowerPin = IO_NONE;
#endif

Expand Down
55 changes: 12 additions & 43 deletions src/main/fc/config.c
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,16 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
accelerometerTrims->values.yaw = 0;
}

static void resetCompassConfig(compassConfig_t* compassConfig)
{
compassConfig->mag_align = ALIGN_DEFAULT;
#ifdef MAG_INT_EXTI
compassConfig->interruptTag = IO_TAG(MAG_INT_EXTI);
#else
compassConfig->interruptTag = IO_TAG_NONE;
#endif
}

static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
{
controlRateConfig->rcRate8 = 100;
Expand Down Expand Up @@ -642,7 +652,8 @@ void createDefaultConfig(master_t *config)

config->gyroConfig.gyro_align = ALIGN_DEFAULT;
config->accelerometerConfig.acc_align = ALIGN_DEFAULT;
config->compassConfig.mag_align = ALIGN_DEFAULT;

resetCompassConfig(&config->compassConfig);

config->boardAlignment.rollDegrees = 0;
config->boardAlignment.pitchDegrees = 0;
Expand Down Expand Up @@ -954,12 +965,6 @@ void validateAndFixConfig(void)
featureClear(FEATURE_CURRENT_METER);
}
#endif

#if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
// led strip needs the same ports
featureClear(FEATURE_LED_STRIP);
#endif

// software serial needs free PWM ports
featureClear(FEATURE_SOFTSERIAL);
}
Expand All @@ -979,42 +984,6 @@ void validateAndFixConfig(void)
}
#endif

#if defined(NAZE) && defined(SONAR)
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif

#if defined(OLIMEXINO) && defined(SONAR)
if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif

#if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3)
if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DASHBOARD)) {
featureClear(FEATURE_DASHBOARD);
}
#endif

#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
// shared pin
if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
featureClear(FEATURE_SONAR);
featureClear(FEATURE_SOFTSERIAL);
featureClear(FEATURE_RSSI_ADC);
}
#endif

#if defined(COLIBRI_RACE)
serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_PARALLEL_PWM);
featureClear(FEATURE_RX_MSP);
featureSet(FEATURE_RX_PPM);
}
#endif

useRxConfig(&masterConfig.rxConfig);

serialConfig_t *serialConfig = &masterConfig.serialConfig;
Expand Down
9 changes: 0 additions & 9 deletions src/main/sensors/acceleration.c
Original file line number Diff line number Diff line change
Expand Up @@ -98,11 +98,7 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
#ifdef USE_ACC_ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
#ifdef NAZE
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, dev)) {
#else
if (adxl345Detect(&acc_params, dev)) {
#endif
#ifdef ACC_ADXL345_ALIGN
dev->accAlign = ACC_ADXL345_ALIGN;
#endif
Expand Down Expand Up @@ -135,12 +131,7 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
; // fallthrough
case ACC_MMA8452: // MMA8452
#ifdef USE_ACC_MMA8452
#ifdef NAZE
// Not supported with this frequency
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) {
#else
if (mma8452Detect(dev)) {
#endif
#ifdef ACC_MMA8452_ALIGN
dev->accAlign = ACC_MMA8452_ALIGN;
#endif
Expand Down
6 changes: 0 additions & 6 deletions src/main/sensors/barometer.c
Original file line number Diff line number Diff line change
Expand Up @@ -71,12 +71,6 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
bmp085Config = &defaultBMP085Config;
#endif

#ifdef NAZE
if (hardwareRevision == NAZE32) {
bmp085Disable(bmp085Config);
}
#endif

#endif

switch (baroHardware) {
Expand Down
29 changes: 1 addition & 28 deletions src/main/sensors/compass.c
Original file line number Diff line number Diff line change
Expand Up @@ -56,33 +56,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
{
magSensor_e magHardware;

#ifdef USE_MAG_HMC5883
const hmc5883Config_t *hmc5883Config = 0;

#ifdef NAZE // TODO remove this target specific define
static const hmc5883Config_t nazeHmc5883Config_v1_v4 = {
.intTag = IO_TAG(PB12) /* perhaps disabled? */
};
static const hmc5883Config_t nazeHmc5883Config_v5 = {
.intTag = IO_TAG(MAG_INT_EXTI)
};
if (hardwareRevision < NAZE32_REV5) {
hmc5883Config = &nazeHmc5883Config_v1_v4;
} else {
hmc5883Config = &nazeHmc5883Config_v5;
}
#endif

#ifdef MAG_INT_EXTI
static const hmc5883Config_t extiHmc5883Config = {
.intTag = IO_TAG(MAG_INT_EXTI)
};

hmc5883Config = &extiHmc5883Config;
#endif

#endif

retry:

dev->magAlign = ALIGN_DEFAULT;
Expand All @@ -93,7 +66,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)

case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(dev, hmc5883Config)) {
if (hmc5883lDetect(dev, compassConfig()->interruptTag)) {
#ifdef MAG_HMC5883_ALIGN
dev->magAlign = MAG_HMC5883_ALIGN;
#endif
Expand Down
4 changes: 3 additions & 1 deletion src/main/sensors/compass.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,11 @@
#pragma once

#include "config/parameter_group.h"

#include "drivers/io.h"
#include "drivers/sensor.h"
#include "sensors/sensors.h"


// Type of magnetometer used/detected
typedef enum {
MAG_DEFAULT = 0,
Expand All @@ -43,6 +44,7 @@ typedef struct compassConfig_s {
// For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
sensor_align_e mag_align; // mag alignment
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
ioTag_t interruptTag;
flightDynamicsTrims_t magZero;
} compassConfig_t;

Expand Down
42 changes: 42 additions & 0 deletions src/main/target/CC3D/config.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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 3 of the License, or
* (at your option) any later version.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/

#include <platform.h>

#include "config/config_master.h"
#include "config/feature.h"
#include "sensors/battery.h"

void targetValidateConfiguration(master_t *config)
{
UNUSED(config);

#if defined(DISPLAY) && defined(USE_UART3)
if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DASHBOARD)) {
featureClear(FEATURE_DASHBOARD);
}
#endif

#if defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
// shared pin
if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
featureClear(FEATURE_SONAR);
featureClear(FEATURE_SOFTSERIAL);
featureClear(FEATURE_RSSI_ADC);
}
#endif
}
1 change: 1 addition & 0 deletions src/main/target/CC3D/target.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
*/

#define TARGET_BOARD_IDENTIFIER "CC3D" // CopterControl 3D
#define TARGET_VALIDATECONFIG

#define LED0 PB3

Expand Down
31 changes: 31 additions & 0 deletions src/main/target/CHEBUZZF3/config.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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 3 of the License, or
* (at your option) any later version.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/

#include <platform.h>

#include "config/config_master.h"
#include "config/feature.h"

void targetValidateConfiguration(master_t *config)
{
UNUSED(config);

if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
// led strip needs the same ports
featureClear(FEATURE_LED_STRIP);
}
}
13 changes: 13 additions & 0 deletions src/main/target/COLIBRI_RACE/config.c
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <platform.h>

#include "common/maths.h"
#include "common/utils.h"

#include "config/config_master.h"
#include "config/feature.h"
Expand Down Expand Up @@ -99,3 +100,15 @@ void targetConfiguration(master_t *config)
targetApplyDefaultLedStripConfig(config->ledStripConfig.ledConfigs);

}

void targetValidateConfiguration(master_t *config)
{
UNUSED(config);

serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_PARALLEL_PWM);
featureClear(FEATURE_RX_MSP);
featureSet(FEATURE_RX_PPM);
}
}
4 changes: 0 additions & 4 deletions src/main/target/COLIBRI_RACE/i2c_bst.c
Original file line number Diff line number Diff line change
Expand Up @@ -70,10 +70,6 @@
#include "config/config_master.h"
#include "config/feature.h"

#ifdef NAZE
#include "hardware_revision.h"
#endif

#include "bus_bst.h"
#include "i2c_bst.h"

Expand Down
Loading

0 comments on commit 8e9be42

Please sign in to comment.