Introduced PreInit.
This commit is contained in:
parent
2421c277e4
commit
4ea4f7c217
|
@ -124,6 +124,10 @@
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
|
|
||||||
|
#ifdef TARGET_PREINIT
|
||||||
|
void targetPreInit(void);
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifdef TARGET_BUS_INIT
|
#ifdef TARGET_BUS_INIT
|
||||||
void targetBusInit(void);
|
void targetBusInit(void);
|
||||||
#endif
|
#endif
|
||||||
|
@ -180,7 +184,11 @@ void init(void)
|
||||||
// Latch active features to be used for feature() in the remainder of init().
|
// Latch active features to be used for feature() in the remainder of init().
|
||||||
latchActiveFeatures();
|
latchActiveFeatures();
|
||||||
|
|
||||||
ledInit(&masterConfig.statusLedConfig);
|
#ifdef TARGET_PREINIT
|
||||||
|
targetPreInit();
|
||||||
|
#endif
|
||||||
|
|
||||||
|
ledInit(statusLedConfig());
|
||||||
LED2_ON;
|
LED2_ON;
|
||||||
|
|
||||||
#ifdef USE_EXTI
|
#ifdef USE_EXTI
|
||||||
|
@ -268,9 +276,9 @@ void init(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_QUAD_MIXER_ONLY
|
#ifdef USE_QUAD_MIXER_ONLY
|
||||||
motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT);
|
motorInit(motorConfig(), idlePulse, QUAD_MOTOR_COUNT);
|
||||||
#else
|
#else
|
||||||
motorInit(&masterConfig.motorConfig, idlePulse, motorCount);
|
motorInit(motorConfig(), idlePulse, motorCount);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
|
@ -291,7 +299,7 @@ void init(void)
|
||||||
systemState |= SYSTEM_STATE_MOTORS_READY;
|
systemState |= SYSTEM_STATE_MOTORS_READY;
|
||||||
|
|
||||||
#ifdef BEEPER
|
#ifdef BEEPER
|
||||||
beeperInit(&masterConfig.beeperConfig);
|
beeperInit(beeperConfig());
|
||||||
#endif
|
#endif
|
||||||
/* temp until PGs are implemented. */
|
/* temp until PGs are implemented. */
|
||||||
#ifdef INVERTER
|
#ifdef INVERTER
|
||||||
|
@ -350,10 +358,10 @@ void init(void)
|
||||||
adcConfig()->vbat.enabled = feature(FEATURE_VBAT);
|
adcConfig()->vbat.enabled = feature(FEATURE_VBAT);
|
||||||
adcConfig()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
|
adcConfig()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
|
||||||
adcConfig()->rssi.enabled = feature(FEATURE_RSSI_ADC);
|
adcConfig()->rssi.enabled = feature(FEATURE_RSSI_ADC);
|
||||||
adcInit(&masterConfig.adcConfig);
|
adcInit(adcConfig());
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
initBoardAlignment(&masterConfig.boardAlignment);
|
initBoardAlignment(boardAlignment());
|
||||||
|
|
||||||
#ifdef CMS
|
#ifdef CMS
|
||||||
cmsInit();
|
cmsInit();
|
||||||
|
@ -361,7 +369,7 @@ void init(void)
|
||||||
|
|
||||||
#ifdef USE_DASHBOARD
|
#ifdef USE_DASHBOARD
|
||||||
if (feature(FEATURE_DASHBOARD)) {
|
if (feature(FEATURE_DASHBOARD)) {
|
||||||
dashboardInit(&masterConfig.rxConfig);
|
dashboardInit(rxConfig());
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -428,12 +436,12 @@ void init(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_CLI
|
#ifdef USE_CLI
|
||||||
cliInit(&masterConfig.serialConfig);
|
cliInit(serialConfig());
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
failsafeInit(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
|
failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
|
||||||
|
|
||||||
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
|
rxInit(rxConfig(), masterConfig.modeActivationConditions);
|
||||||
|
|
||||||
#ifdef GPS
|
#ifdef GPS
|
||||||
if (feature(FEATURE_GPS)) {
|
if (feature(FEATURE_GPS)) {
|
||||||
|
@ -449,7 +457,7 @@ void init(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef LED_STRIP
|
#ifdef LED_STRIP
|
||||||
ledStripInit(&masterConfig.ledStripConfig);
|
ledStripInit(ledStripConfig());
|
||||||
|
|
||||||
if (feature(FEATURE_LED_STRIP)) {
|
if (feature(FEATURE_LED_STRIP)) {
|
||||||
ledStripEnable();
|
ledStripEnable();
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
#define TARGET_BOARD_IDENTIFIER "BJF4"
|
#define TARGET_BOARD_IDENTIFIER "BJF4"
|
||||||
#define TARGET_CONFIG
|
#define TARGET_CONFIG
|
||||||
#define TARGET_VALIDATECONFIG
|
#define TARGET_VALIDATECONFIG
|
||||||
|
#define TARGET_PREINIT
|
||||||
|
|
||||||
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
|
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
|
||||||
|
|
||||||
|
@ -40,6 +41,8 @@
|
||||||
#define INVERTER PB15
|
#define INVERTER PB15
|
||||||
#define INVERTER_USART USART6
|
#define INVERTER_USART USART6
|
||||||
|
|
||||||
|
#define UART1_INVERTER PC9
|
||||||
|
|
||||||
// MPU6500 interrupt
|
// MPU6500 interrupt
|
||||||
#define USE_EXTI
|
#define USE_EXTI
|
||||||
#define MPU_INT_EXTI PC5
|
#define MPU_INT_EXTI PC5
|
||||||
|
|
Loading…
Reference in New Issue