Introduced PreInit.

This commit is contained in:
blckmn 2016-12-17 17:09:08 +11:00
parent 2421c277e4
commit 4ea4f7c217
3 changed files with 70 additions and 11 deletions

View File

@ -124,6 +124,10 @@
#include "build/build_config.h"
#include "build/debug.h"
#ifdef TARGET_PREINIT
void targetPreInit(void);
#endif
#ifdef TARGET_BUS_INIT
void targetBusInit(void);
#endif
@ -180,7 +184,11 @@ void init(void)
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
ledInit(&masterConfig.statusLedConfig);
#ifdef TARGET_PREINIT
targetPreInit();
#endif
ledInit(statusLedConfig());
LED2_ON;
#ifdef USE_EXTI
@ -268,9 +276,9 @@ void init(void)
#endif
#ifdef USE_QUAD_MIXER_ONLY
motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT);
motorInit(motorConfig(), idlePulse, QUAD_MOTOR_COUNT);
#else
motorInit(&masterConfig.motorConfig, idlePulse, motorCount);
motorInit(motorConfig(), idlePulse, motorCount);
#endif
#ifdef USE_SERVOS
@ -291,7 +299,7 @@ void init(void)
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(&masterConfig.beeperConfig);
beeperInit(beeperConfig());
#endif
/* temp until PGs are implemented. */
#ifdef INVERTER
@ -350,10 +358,10 @@ void init(void)
adcConfig()->vbat.enabled = feature(FEATURE_VBAT);
adcConfig()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
adcConfig()->rssi.enabled = feature(FEATURE_RSSI_ADC);
adcInit(&masterConfig.adcConfig);
adcInit(adcConfig());
#endif
initBoardAlignment(&masterConfig.boardAlignment);
initBoardAlignment(boardAlignment());
#ifdef CMS
cmsInit();
@ -361,7 +369,7 @@ void init(void)
#ifdef USE_DASHBOARD
if (feature(FEATURE_DASHBOARD)) {
dashboardInit(&masterConfig.rxConfig);
dashboardInit(rxConfig());
}
#endif
@ -428,12 +436,12 @@ void init(void)
#endif
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
cliInit(serialConfig());
#endif
failsafeInit(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
rxInit(rxConfig(), masterConfig.modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
@ -449,7 +457,7 @@ void init(void)
#endif
#ifdef LED_STRIP
ledStripInit(&masterConfig.ledStripConfig);
ledStripInit(ledStripConfig());
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();

View File

@ -0,0 +1,48 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "hardware_revision.h"
#include "config/config_master.h"
#include "drivers/io.h"
#include "config/feature.h"
void targetPreInit(void)
{
/* enable the built in inverter if telemetry is setup for the UART1 */
if (serialConfig()->portConfigs[SERIAL_PORT_USART1].functionMask & FUNCTION_TELEMETRY_SMARTPORT &&
telemetryConfig()->telemetry_inversion &&
feature(FEATURE_TELEMETRY)) {
IO_t io = IOGetByTag(IO_TAG(UART1_INVERTER));
IOInit(io, OWNER_INVERTER, 1);
IOConfigGPIO(io, IOCFG_OUT_PP);
IOHi(io);
}
/* ensure the CS pin for the flash is pulled hi so any SD card initialisation does not impact the chip */
if (hardwareRevision == BJF4_REV3) {
IO_t io = IOGetByTag(IO_TAG(M25P16_CS_PIN));
IOConfigGPIO(io, IOCFG_OUT_PP);
IOHi(io);
}
}

View File

@ -19,6 +19,7 @@
#define TARGET_BOARD_IDENTIFIER "BJF4"
#define TARGET_CONFIG
#define TARGET_VALIDATECONFIG
#define TARGET_PREINIT
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
@ -40,6 +41,8 @@
#define INVERTER PB15
#define INVERTER_USART USART6
#define UART1_INVERTER PC9
// MPU6500 interrupt
#define USE_EXTI
#define MPU_INT_EXTI PC5