Merge pull request #1978 from martinbudden/bf_main_tidy
Moved init() and hardfault handling out of main into fc directory
This commit is contained in:
Martin Budden
GitHub
commit
ea96e588ab
6
Makefile
6
Makefile
|
|
@ -522,9 +522,11 @@ COMMON_SRC = \
|
|||
drivers/system.c \
|
||||
drivers/timer.c \
|
||||
fc/config.c \
|
||||
fc/fc_tasks.c \
|
||||
fc/fc_msp.c \
|
||||
fc/fc_init.c \
|
||||
fc/fc_hardfaults.c \
|
||||
fc/fc_main.c \
|
||||
fc/fc_msp.c \
|
||||
fc/fc_tasks.c \
|
||||
fc/rc_controls.c \
|
||||
fc/rc_curves.c \
|
||||
fc/runtime_config.c \
|
||||
|
|
|
|||
|
|
@ -0,0 +1,118 @@
|
|||
/*
|
||||
* 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/light_led.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "fc/fc_init.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
|
||||
#ifdef DEBUG_HARDFAULTS
|
||||
//from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/
|
||||
/**
|
||||
* hard_fault_handler_c:
|
||||
* This is called from the HardFault_HandlerAsm with a pointer the Fault stack
|
||||
* as the parameter. We can then read the values from the stack and place them
|
||||
* into local variables for ease of reading.
|
||||
* We then read the various Fault Status and Address Registers to help decode
|
||||
* cause of the fault.
|
||||
* The function ends with a BKPT instruction to force control back into the debugger
|
||||
*/
|
||||
void hard_fault_handler_c(unsigned long *hardfault_args)
|
||||
{
|
||||
volatile unsigned long stacked_r0 ;
|
||||
volatile unsigned long stacked_r1 ;
|
||||
volatile unsigned long stacked_r2 ;
|
||||
volatile unsigned long stacked_r3 ;
|
||||
volatile unsigned long stacked_r12 ;
|
||||
volatile unsigned long stacked_lr ;
|
||||
volatile unsigned long stacked_pc ;
|
||||
volatile unsigned long stacked_psr ;
|
||||
volatile unsigned long _CFSR ;
|
||||
volatile unsigned long _HFSR ;
|
||||
volatile unsigned long _DFSR ;
|
||||
volatile unsigned long _AFSR ;
|
||||
volatile unsigned long _BFAR ;
|
||||
volatile unsigned long _MMAR ;
|
||||
|
||||
stacked_r0 = ((unsigned long)hardfault_args[0]) ;
|
||||
stacked_r1 = ((unsigned long)hardfault_args[1]) ;
|
||||
stacked_r2 = ((unsigned long)hardfault_args[2]) ;
|
||||
stacked_r3 = ((unsigned long)hardfault_args[3]) ;
|
||||
stacked_r12 = ((unsigned long)hardfault_args[4]) ;
|
||||
stacked_lr = ((unsigned long)hardfault_args[5]) ;
|
||||
stacked_pc = ((unsigned long)hardfault_args[6]) ;
|
||||
stacked_psr = ((unsigned long)hardfault_args[7]) ;
|
||||
|
||||
// Configurable Fault Status Register
|
||||
// Consists of MMSR, BFSR and UFSR
|
||||
_CFSR = (*((volatile unsigned long *)(0xE000ED28))) ;
|
||||
|
||||
// Hard Fault Status Register
|
||||
_HFSR = (*((volatile unsigned long *)(0xE000ED2C))) ;
|
||||
|
||||
// Debug Fault Status Register
|
||||
_DFSR = (*((volatile unsigned long *)(0xE000ED30))) ;
|
||||
|
||||
// Auxiliary Fault Status Register
|
||||
_AFSR = (*((volatile unsigned long *)(0xE000ED3C))) ;
|
||||
|
||||
// Read the Fault Address Registers. These may not contain valid values.
|
||||
// Check BFARVALID/MMARVALID to see if they are valid values
|
||||
// MemManage Fault Address Register
|
||||
_MMAR = (*((volatile unsigned long *)(0xE000ED34))) ;
|
||||
// Bus Fault Address Register
|
||||
_BFAR = (*((volatile unsigned long *)(0xE000ED38))) ;
|
||||
|
||||
__asm("BKPT #0\n") ; // Break into the debugger
|
||||
}
|
||||
|
||||
#else
|
||||
void HardFault_Handler(void)
|
||||
{
|
||||
LED2_ON;
|
||||
|
||||
// fall out of the sky
|
||||
uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
|
||||
if ((systemState & requiredStateForMotors) == requiredStateForMotors) {
|
||||
stopMotors();
|
||||
}
|
||||
#ifdef TRANSPONDER
|
||||
// prevent IR LEDs from burning out.
|
||||
uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED;
|
||||
if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) {
|
||||
transponderIrDisable();
|
||||
}
|
||||
#endif
|
||||
|
||||
LED1_OFF;
|
||||
LED0_OFF;
|
||||
|
||||
while (1) {
|
||||
#ifdef LED2
|
||||
delay(50);
|
||||
LED2_TOGGLE;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,573 @@
|
|||
/*
|
||||
* 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 <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "blackbox/blackbox.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/color.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/printf.h"
|
||||
|
||||
#include "cms/cms.h"
|
||||
#include "cms/cms_types.h"
|
||||
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/dma.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/light_led.h"
|
||||
#include "drivers/sound_beeper.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_softserial.h"
|
||||
#include "drivers/serial_uart.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/compass.h"
|
||||
#include "drivers/pwm_esc_detect.h"
|
||||
#include "drivers/rx_pwm.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/adc.h"
|
||||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/inverter.h"
|
||||
#include "drivers/flash_m25p16.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/sdcard.h"
|
||||
#include "drivers/usb_io.h"
|
||||
#include "drivers/transponder_ir.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/vtx_soft_spi_rtc6705.h"
|
||||
|
||||
#ifdef USE_BST
|
||||
#include "bus_bst.h"
|
||||
#endif
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/fc_init.h"
|
||||
#include "fc/fc_msp.h"
|
||||
#include "fc/fc_tasks.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "msp/msp_serial.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/spektrum.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/displayport_max7456.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/flashfs.h"
|
||||
#include "io/gps.h"
|
||||
#include "io/motors.h"
|
||||
#include "io/servos.h"
|
||||
#include "io/gimbal.h"
|
||||
#include "io/ledstrip.h"
|
||||
#include "io/dashboard.h"
|
||||
#include "io/asyncfatfs/asyncfatfs.h"
|
||||
#include "io/serial_cli.h"
|
||||
#include "io/transponder_ir.h"
|
||||
#include "io/osd.h"
|
||||
#include "io/displayport_msp.h"
|
||||
#include "io/vtx.h"
|
||||
#include "io/vtx_smartaudio.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/initialisation.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/navigation.h"
|
||||
|
||||
#include "config/config_eeprom.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
#endif
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#ifdef TARGET_PREINIT
|
||||
void targetPreInit(void);
|
||||
#endif
|
||||
|
||||
#ifdef TARGET_BUS_INIT
|
||||
void targetBusInit(void);
|
||||
#endif
|
||||
|
||||
extern uint8_t motorControlEnable;
|
||||
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
serialPort_t *loopbackPort;
|
||||
#endif
|
||||
|
||||
uint8_t systemState = SYSTEM_STATE_INITIALISING;
|
||||
|
||||
void processLoopback(void)
|
||||
{
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
if (loopbackPort) {
|
||||
uint8_t bytesWaiting;
|
||||
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
|
||||
uint8_t b = serialRead(loopbackPort);
|
||||
serialWrite(loopbackPort, b);
|
||||
};
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void init(void)
|
||||
{
|
||||
#ifdef USE_HAL_DRIVER
|
||||
HAL_Init();
|
||||
#endif
|
||||
|
||||
printfSupportInit();
|
||||
|
||||
systemInit();
|
||||
|
||||
// initialize IO (needed for all IO operations)
|
||||
IOInitGlobal();
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
detectHardwareRevision();
|
||||
#endif
|
||||
|
||||
#ifdef BRUSHED_ESC_AUTODETECT
|
||||
detectBrushedESC();
|
||||
#endif
|
||||
|
||||
initEEPROM();
|
||||
|
||||
ensureEEPROMContainsValidData();
|
||||
readEEPROM();
|
||||
|
||||
systemState |= SYSTEM_STATE_CONFIG_LOADED;
|
||||
|
||||
//i2cSetOverclock(masterConfig.i2c_overclock);
|
||||
|
||||
debugMode = masterConfig.debug_mode;
|
||||
|
||||
// Latch active features to be used for feature() in the remainder of init().
|
||||
latchActiveFeatures();
|
||||
|
||||
#ifdef TARGET_PREINIT
|
||||
targetPreInit();
|
||||
#endif
|
||||
|
||||
ledInit(statusLedConfig());
|
||||
LED2_ON;
|
||||
|
||||
#ifdef USE_EXTI
|
||||
EXTIInit();
|
||||
#endif
|
||||
|
||||
#if defined(BUTTONS)
|
||||
#ifdef BUTTON_A_PIN
|
||||
IO_t buttonAPin = IOGetByTag(IO_TAG(BUTTON_A_PIN));
|
||||
IOInit(buttonAPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(buttonAPin, IOCFG_IPU);
|
||||
#endif
|
||||
|
||||
#ifdef BUTTON_B_PIN
|
||||
IO_t buttonBPin = IOGetByTag(IO_TAG(BUTTON_B_PIN));
|
||||
IOInit(buttonBPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(buttonBPin, IOCFG_IPU);
|
||||
#endif
|
||||
|
||||
// Check status of bind plug and exit if not active
|
||||
delayMicroseconds(10); // allow configuration to settle
|
||||
|
||||
if (!isMPUSoftReset()) {
|
||||
#if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
|
||||
// two buttons required
|
||||
uint8_t secondsRemaining = 5;
|
||||
bool bothButtonsHeld;
|
||||
do {
|
||||
bothButtonsHeld = !IORead(buttonAPin) && !IORead(buttonBPin);
|
||||
if (bothButtonsHeld) {
|
||||
if (--secondsRemaining == 0) {
|
||||
resetEEPROM();
|
||||
systemReset();
|
||||
}
|
||||
delay(1000);
|
||||
LED0_TOGGLE;
|
||||
}
|
||||
} while (bothButtonsHeld);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SPEKTRUM_BIND
|
||||
if (feature(FEATURE_RX_SERIAL)) {
|
||||
switch (rxConfig()->serialrx_provider) {
|
||||
case SERIALRX_SPEKTRUM1024:
|
||||
case SERIALRX_SPEKTRUM2048:
|
||||
// Spektrum satellite binding if enabled on startup.
|
||||
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
|
||||
// The rest of Spektrum initialization will happen later - via spektrumInit()
|
||||
spektrumBind(rxConfig());
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
delay(100);
|
||||
|
||||
timerInit(); // timer must be initialized before any channel is allocated
|
||||
|
||||
#if defined(AVOID_UART1_FOR_PWM_PPM)
|
||||
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
|
||||
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
|
||||
#elif defined(AVOID_UART2_FOR_PWM_PPM)
|
||||
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
|
||||
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
|
||||
#elif defined(AVOID_UART3_FOR_PWM_PPM)
|
||||
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
|
||||
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
|
||||
#else
|
||||
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
|
||||
#endif
|
||||
|
||||
mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer);
|
||||
#ifdef USE_SERVOS
|
||||
servoMixerInit(masterConfig.customServoMixer);
|
||||
#endif
|
||||
|
||||
uint16_t idlePulse = motorConfig()->mincommand;
|
||||
if (feature(FEATURE_3D)) {
|
||||
idlePulse = flight3DConfig()->neutral3d;
|
||||
}
|
||||
|
||||
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
|
||||
featureClear(FEATURE_3D);
|
||||
idlePulse = 0; // brushed motors
|
||||
}
|
||||
|
||||
mixerConfigureOutput();
|
||||
motorInit(motorConfig(), idlePulse, getMotorCount());
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
servoConfigureOutput();
|
||||
if (isMixerUsingServos()) {
|
||||
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
|
||||
servoInit(servoConfig());
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_PWM) || defined(USE_PPM)
|
||||
if (feature(FEATURE_RX_PPM)) {
|
||||
ppmRxInit(ppmConfig(), motorConfig()->motorPwmProtocol);
|
||||
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||
pwmRxInit(pwmConfig());
|
||||
}
|
||||
#endif
|
||||
|
||||
systemState |= SYSTEM_STATE_MOTORS_READY;
|
||||
|
||||
#ifdef BEEPER
|
||||
beeperInit(beeperConfig());
|
||||
#endif
|
||||
/* temp until PGs are implemented. */
|
||||
#ifdef INVERTER
|
||||
initInverter();
|
||||
#endif
|
||||
|
||||
#ifdef USE_BST
|
||||
bstInit(BST_DEVICE);
|
||||
#endif
|
||||
|
||||
#ifdef TARGET_BUS_INIT
|
||||
targetBusInit();
|
||||
#else
|
||||
#ifdef USE_SPI
|
||||
#ifdef USE_SPI_DEVICE_1
|
||||
spiInit(SPIDEV_1);
|
||||
#endif
|
||||
#ifdef USE_SPI_DEVICE_2
|
||||
spiInit(SPIDEV_2);
|
||||
#endif
|
||||
#ifdef USE_SPI_DEVICE_3
|
||||
spiInit(SPIDEV_3);
|
||||
#endif
|
||||
#ifdef USE_SPI_DEVICE_4
|
||||
spiInit(SPIDEV_4);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef USE_I2C
|
||||
i2cInit(I2C_DEVICE);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
updateHardwareRevision();
|
||||
#endif
|
||||
|
||||
#ifdef VTX
|
||||
vtxInit();
|
||||
#endif
|
||||
|
||||
#if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL1)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_ADC
|
||||
/* these can be removed from features! */
|
||||
adcConfig()->vbat.enabled = feature(FEATURE_VBAT);
|
||||
adcConfig()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
|
||||
adcConfig()->rssi.enabled = feature(FEATURE_RSSI_ADC);
|
||||
adcInit(adcConfig());
|
||||
#endif
|
||||
|
||||
initBoardAlignment(boardAlignment());
|
||||
|
||||
#ifdef CMS
|
||||
cmsInit();
|
||||
#endif
|
||||
|
||||
#ifdef USE_DASHBOARD
|
||||
if (feature(FEATURE_DASHBOARD)) {
|
||||
dashboardInit(rxConfig());
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_RTC6705
|
||||
if (feature(FEATURE_VTX)) {
|
||||
rtc6705_soft_spi_init();
|
||||
current_vtx_channel = masterConfig.vtx_channel;
|
||||
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
|
||||
rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef OSD
|
||||
if (feature(FEATURE_OSD)) {
|
||||
#ifdef USE_MAX7456
|
||||
// if there is a max7456 chip for the OSD then use it, otherwise use MSP
|
||||
displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile());
|
||||
#else
|
||||
displayPort_t *osdDisplayPort = displayPortMspInit();
|
||||
#endif
|
||||
osdInit(osdDisplayPort);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
const sonarConfig_t *sonarConfig = sonarConfig();
|
||||
#else
|
||||
const void *sonarConfig = NULL;
|
||||
#endif
|
||||
if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) {
|
||||
// if gyro was not detected due to whatever reason, we give up now.
|
||||
failureMode(FAILURE_MISSING_ACC);
|
||||
}
|
||||
|
||||
systemState |= SYSTEM_STATE_SENSORS_READY;
|
||||
|
||||
LED1_ON;
|
||||
LED0_OFF;
|
||||
LED2_OFF;
|
||||
|
||||
for (int i = 0; i < 10; i++) {
|
||||
LED1_TOGGLE;
|
||||
LED0_TOGGLE;
|
||||
delay(25);
|
||||
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
|
||||
delay(25);
|
||||
BEEP_OFF;
|
||||
}
|
||||
LED0_OFF;
|
||||
LED1_OFF;
|
||||
|
||||
// gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime()
|
||||
pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * pidConfig()->pid_process_denom); // Initialize pid looptime
|
||||
pidInitFilters(¤tProfile->pidProfile);
|
||||
pidInitConfig(¤tProfile->pidProfile);
|
||||
|
||||
imuInit();
|
||||
|
||||
mspFcInit();
|
||||
mspSerialInit();
|
||||
|
||||
#if defined(USE_MSP_DISPLAYPORT) && defined(CMS)
|
||||
cmsDisplayPortRegister(displayPortMspInit());
|
||||
#endif
|
||||
|
||||
#ifdef USE_CLI
|
||||
cliInit(serialConfig());
|
||||
#endif
|
||||
|
||||
failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
|
||||
|
||||
rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
|
||||
|
||||
#ifdef GPS
|
||||
if (feature(FEATURE_GPS)) {
|
||||
gpsInit(
|
||||
serialConfig(),
|
||||
gpsConfig()
|
||||
);
|
||||
navigationInit(
|
||||
gpsProfile(),
|
||||
¤tProfile->pidProfile
|
||||
);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef LED_STRIP
|
||||
ledStripInit(ledStripConfig());
|
||||
|
||||
if (feature(FEATURE_LED_STRIP)) {
|
||||
ledStripEnable();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef TELEMETRY
|
||||
if (feature(FEATURE_TELEMETRY)) {
|
||||
telemetryInit();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_ESC_SENSOR
|
||||
if (feature(FEATURE_ESC_SENSOR)) {
|
||||
escSensorInit();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USB_CABLE_DETECTION
|
||||
usbCableDetectInit();
|
||||
#endif
|
||||
|
||||
#ifdef TRANSPONDER
|
||||
if (feature(FEATURE_TRANSPONDER)) {
|
||||
transponderInit(masterConfig.transponderData);
|
||||
transponderEnable();
|
||||
transponderStartRepeating();
|
||||
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_FLASHFS
|
||||
#if defined(USE_FLASH_M25P16)
|
||||
m25p16_init(flashConfig());
|
||||
#endif
|
||||
|
||||
flashfsInit();
|
||||
#endif
|
||||
|
||||
#ifdef USE_SDCARD
|
||||
if (feature(FEATURE_SDCARD)) {
|
||||
sdcardInsertionDetectInit();
|
||||
sdcard_init(sdcardConfig()->useDma);
|
||||
afatfs_init();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef BLACKBOX
|
||||
initBlackbox();
|
||||
#endif
|
||||
|
||||
if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
|
||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||
}
|
||||
gyroSetCalibrationCycles();
|
||||
#ifdef BARO
|
||||
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
|
||||
#endif
|
||||
|
||||
#ifdef VTX_SMARTAUDIO
|
||||
smartAudioInit();
|
||||
#endif
|
||||
|
||||
// start all timers
|
||||
// TODO - not implemented yet
|
||||
timerStart();
|
||||
|
||||
ENABLE_STATE(SMALL_ANGLE);
|
||||
DISABLE_ARMING_FLAG(PREVENT_ARMING);
|
||||
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
|
||||
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
|
||||
if (!loopbackPort->vTable) {
|
||||
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
|
||||
}
|
||||
serialPrint(loopbackPort, "LOOPBACK\r\n");
|
||||
#endif
|
||||
|
||||
// Now that everything has powered up the voltage and cell count be determined.
|
||||
|
||||
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
|
||||
batteryInit(batteryConfig());
|
||||
|
||||
#ifdef USE_DASHBOARD
|
||||
if (feature(FEATURE_DASHBOARD)) {
|
||||
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
|
||||
dashboardShowFixedPage(PAGE_GPS);
|
||||
#else
|
||||
dashboardResetPageCycling();
|
||||
dashboardEnablePageCycling();
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CJMCU
|
||||
LED2_ON;
|
||||
#endif
|
||||
|
||||
// Latch active features AGAIN since some may be modified by init().
|
||||
latchActiveFeatures();
|
||||
motorControlEnable = true;
|
||||
|
||||
fcTasksInit();
|
||||
systemState |= SYSTEM_STATE_READY;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
typedef enum {
|
||||
SYSTEM_STATE_INITIALISING = 0,
|
||||
SYSTEM_STATE_CONFIG_LOADED = (1 << 0),
|
||||
SYSTEM_STATE_SENSORS_READY = (1 << 1),
|
||||
SYSTEM_STATE_MOTORS_READY = (1 << 2),
|
||||
SYSTEM_STATE_TRANSPONDER_ENABLED = (1 << 3),
|
||||
SYSTEM_STATE_READY = (1 << 7)
|
||||
} systemState_e;
|
||||
|
||||
extern uint8_t systemState;
|
||||
|
||||
void init(void);
|
||||
void processLoopback(void);
|
||||
649
src/main/main.c
649
src/main/main.c
|
|
@ -17,570 +17,13 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "blackbox/blackbox.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/color.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/printf.h"
|
||||
|
||||
#include "cms/cms.h"
|
||||
#include "cms/cms_types.h"
|
||||
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/dma.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/light_led.h"
|
||||
#include "drivers/sound_beeper.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_softserial.h"
|
||||
#include "drivers/serial_uart.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/compass.h"
|
||||
#include "drivers/pwm_esc_detect.h"
|
||||
#include "drivers/rx_pwm.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/adc.h"
|
||||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/inverter.h"
|
||||
#include "drivers/flash_m25p16.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/sdcard.h"
|
||||
#include "drivers/usb_io.h"
|
||||
#include "drivers/transponder_ir.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/vtx_soft_spi_rtc6705.h"
|
||||
|
||||
#ifdef USE_BST
|
||||
#include "bus_bst.h"
|
||||
#endif
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/fc_tasks.h"
|
||||
#include "fc/fc_msp.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "msp/msp_serial.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/spektrum.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/displayport_max7456.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/flashfs.h"
|
||||
#include "io/gps.h"
|
||||
#include "io/motors.h"
|
||||
#include "io/servos.h"
|
||||
#include "io/gimbal.h"
|
||||
#include "io/ledstrip.h"
|
||||
#include "io/dashboard.h"
|
||||
#include "io/asyncfatfs/asyncfatfs.h"
|
||||
#include "io/serial_cli.h"
|
||||
#include "io/transponder_ir.h"
|
||||
#include "io/osd.h"
|
||||
#include "io/displayport_msp.h"
|
||||
#include "io/vtx.h"
|
||||
#include "io/vtx_smartaudio.h"
|
||||
#include "fc/fc_init.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/initialisation.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/navigation.h"
|
||||
|
||||
#include "config/config_eeprom.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
#endif
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#ifdef TARGET_PREINIT
|
||||
void targetPreInit(void);
|
||||
#endif
|
||||
|
||||
#ifdef TARGET_BUS_INIT
|
||||
void targetBusInit(void);
|
||||
#endif
|
||||
|
||||
extern uint8_t motorControlEnable;
|
||||
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
serialPort_t *loopbackPort;
|
||||
#endif
|
||||
|
||||
typedef enum {
|
||||
SYSTEM_STATE_INITIALISING = 0,
|
||||
SYSTEM_STATE_CONFIG_LOADED = (1 << 0),
|
||||
SYSTEM_STATE_SENSORS_READY = (1 << 1),
|
||||
SYSTEM_STATE_MOTORS_READY = (1 << 2),
|
||||
SYSTEM_STATE_TRANSPONDER_ENABLED = (1 << 3),
|
||||
SYSTEM_STATE_READY = (1 << 7)
|
||||
} systemState_e;
|
||||
|
||||
static uint8_t systemState = SYSTEM_STATE_INITIALISING;
|
||||
|
||||
void init(void)
|
||||
{
|
||||
#ifdef USE_HAL_DRIVER
|
||||
HAL_Init();
|
||||
#endif
|
||||
|
||||
printfSupportInit();
|
||||
|
||||
systemInit();
|
||||
|
||||
// initialize IO (needed for all IO operations)
|
||||
IOInitGlobal();
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
detectHardwareRevision();
|
||||
#endif
|
||||
|
||||
#ifdef BRUSHED_ESC_AUTODETECT
|
||||
detectBrushedESC();
|
||||
#endif
|
||||
|
||||
initEEPROM();
|
||||
|
||||
ensureEEPROMContainsValidData();
|
||||
readEEPROM();
|
||||
|
||||
systemState |= SYSTEM_STATE_CONFIG_LOADED;
|
||||
|
||||
//i2cSetOverclock(masterConfig.i2c_overclock);
|
||||
|
||||
debugMode = masterConfig.debug_mode;
|
||||
|
||||
// Latch active features to be used for feature() in the remainder of init().
|
||||
latchActiveFeatures();
|
||||
|
||||
#ifdef TARGET_PREINIT
|
||||
targetPreInit();
|
||||
#endif
|
||||
|
||||
ledInit(statusLedConfig());
|
||||
LED2_ON;
|
||||
|
||||
#ifdef USE_EXTI
|
||||
EXTIInit();
|
||||
#endif
|
||||
|
||||
#if defined(BUTTONS)
|
||||
#ifdef BUTTON_A_PIN
|
||||
IO_t buttonAPin = IOGetByTag(IO_TAG(BUTTON_A_PIN));
|
||||
IOInit(buttonAPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(buttonAPin, IOCFG_IPU);
|
||||
#endif
|
||||
|
||||
#ifdef BUTTON_B_PIN
|
||||
IO_t buttonBPin = IOGetByTag(IO_TAG(BUTTON_B_PIN));
|
||||
IOInit(buttonBPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(buttonBPin, IOCFG_IPU);
|
||||
#endif
|
||||
|
||||
// Check status of bind plug and exit if not active
|
||||
delayMicroseconds(10); // allow configuration to settle
|
||||
|
||||
if (!isMPUSoftReset()) {
|
||||
#if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
|
||||
// two buttons required
|
||||
uint8_t secondsRemaining = 5;
|
||||
bool bothButtonsHeld;
|
||||
do {
|
||||
bothButtonsHeld = !IORead(buttonAPin) && !IORead(buttonBPin);
|
||||
if (bothButtonsHeld) {
|
||||
if (--secondsRemaining == 0) {
|
||||
resetEEPROM();
|
||||
systemReset();
|
||||
}
|
||||
delay(1000);
|
||||
LED0_TOGGLE;
|
||||
}
|
||||
} while (bothButtonsHeld);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SPEKTRUM_BIND
|
||||
if (feature(FEATURE_RX_SERIAL)) {
|
||||
switch (rxConfig()->serialrx_provider) {
|
||||
case SERIALRX_SPEKTRUM1024:
|
||||
case SERIALRX_SPEKTRUM2048:
|
||||
// Spektrum satellite binding if enabled on startup.
|
||||
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
|
||||
// The rest of Spektrum initialization will happen later - via spektrumInit()
|
||||
spektrumBind(rxConfig());
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
delay(100);
|
||||
|
||||
timerInit(); // timer must be initialized before any channel is allocated
|
||||
|
||||
#if defined(AVOID_UART1_FOR_PWM_PPM)
|
||||
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
|
||||
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
|
||||
#elif defined(AVOID_UART2_FOR_PWM_PPM)
|
||||
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
|
||||
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
|
||||
#elif defined(AVOID_UART3_FOR_PWM_PPM)
|
||||
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
|
||||
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
|
||||
#else
|
||||
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
|
||||
#endif
|
||||
|
||||
mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer);
|
||||
#ifdef USE_SERVOS
|
||||
servoMixerInit(masterConfig.customServoMixer);
|
||||
#endif
|
||||
|
||||
uint16_t idlePulse = motorConfig()->mincommand;
|
||||
if (feature(FEATURE_3D)) {
|
||||
idlePulse = flight3DConfig()->neutral3d;
|
||||
}
|
||||
|
||||
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
|
||||
featureClear(FEATURE_3D);
|
||||
idlePulse = 0; // brushed motors
|
||||
}
|
||||
|
||||
mixerConfigureOutput();
|
||||
motorInit(motorConfig(), idlePulse, getMotorCount());
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
servoConfigureOutput();
|
||||
if (isMixerUsingServos()) {
|
||||
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
|
||||
servoInit(servoConfig());
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_PWM) || defined(USE_PPM)
|
||||
if (feature(FEATURE_RX_PPM)) {
|
||||
ppmRxInit(ppmConfig(), motorConfig()->motorPwmProtocol);
|
||||
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||
pwmRxInit(pwmConfig());
|
||||
}
|
||||
#endif
|
||||
|
||||
systemState |= SYSTEM_STATE_MOTORS_READY;
|
||||
|
||||
#ifdef BEEPER
|
||||
beeperInit(beeperConfig());
|
||||
#endif
|
||||
/* temp until PGs are implemented. */
|
||||
#ifdef INVERTER
|
||||
initInverter();
|
||||
#endif
|
||||
|
||||
#ifdef USE_BST
|
||||
bstInit(BST_DEVICE);
|
||||
#endif
|
||||
|
||||
#ifdef TARGET_BUS_INIT
|
||||
targetBusInit();
|
||||
#else
|
||||
#ifdef USE_SPI
|
||||
#ifdef USE_SPI_DEVICE_1
|
||||
spiInit(SPIDEV_1);
|
||||
#endif
|
||||
#ifdef USE_SPI_DEVICE_2
|
||||
spiInit(SPIDEV_2);
|
||||
#endif
|
||||
#ifdef USE_SPI_DEVICE_3
|
||||
spiInit(SPIDEV_3);
|
||||
#endif
|
||||
#ifdef USE_SPI_DEVICE_4
|
||||
spiInit(SPIDEV_4);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef USE_I2C
|
||||
i2cInit(I2C_DEVICE);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
updateHardwareRevision();
|
||||
#endif
|
||||
|
||||
#ifdef VTX
|
||||
vtxInit();
|
||||
#endif
|
||||
|
||||
#if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL1)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_ADC
|
||||
/* these can be removed from features! */
|
||||
adcConfig()->vbat.enabled = feature(FEATURE_VBAT);
|
||||
adcConfig()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
|
||||
adcConfig()->rssi.enabled = feature(FEATURE_RSSI_ADC);
|
||||
adcInit(adcConfig());
|
||||
#endif
|
||||
|
||||
initBoardAlignment(boardAlignment());
|
||||
|
||||
#ifdef CMS
|
||||
cmsInit();
|
||||
#endif
|
||||
|
||||
#ifdef USE_DASHBOARD
|
||||
if (feature(FEATURE_DASHBOARD)) {
|
||||
dashboardInit(rxConfig());
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_RTC6705
|
||||
if (feature(FEATURE_VTX)) {
|
||||
rtc6705_soft_spi_init();
|
||||
current_vtx_channel = masterConfig.vtx_channel;
|
||||
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
|
||||
rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef OSD
|
||||
if (feature(FEATURE_OSD)) {
|
||||
#ifdef USE_MAX7456
|
||||
// if there is a max7456 chip for the OSD then use it, otherwise use MSP
|
||||
displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile());
|
||||
#else
|
||||
displayPort_t *osdDisplayPort = displayPortMspInit();
|
||||
#endif
|
||||
osdInit(osdDisplayPort);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
const sonarConfig_t *sonarConfig = sonarConfig();
|
||||
#else
|
||||
const void *sonarConfig = NULL;
|
||||
#endif
|
||||
if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) {
|
||||
// if gyro was not detected due to whatever reason, we give up now.
|
||||
failureMode(FAILURE_MISSING_ACC);
|
||||
}
|
||||
|
||||
systemState |= SYSTEM_STATE_SENSORS_READY;
|
||||
|
||||
LED1_ON;
|
||||
LED0_OFF;
|
||||
LED2_OFF;
|
||||
|
||||
for (int i = 0; i < 10; i++) {
|
||||
LED1_TOGGLE;
|
||||
LED0_TOGGLE;
|
||||
delay(25);
|
||||
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
|
||||
delay(25);
|
||||
BEEP_OFF;
|
||||
}
|
||||
LED0_OFF;
|
||||
LED1_OFF;
|
||||
|
||||
// gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime()
|
||||
pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * pidConfig()->pid_process_denom); // Initialize pid looptime
|
||||
pidInitFilters(¤tProfile->pidProfile);
|
||||
pidInitConfig(¤tProfile->pidProfile);
|
||||
|
||||
imuInit();
|
||||
|
||||
mspFcInit();
|
||||
mspSerialInit();
|
||||
|
||||
#if defined(USE_MSP_DISPLAYPORT) && defined(CMS)
|
||||
cmsDisplayPortRegister(displayPortMspInit());
|
||||
#endif
|
||||
|
||||
#ifdef USE_CLI
|
||||
cliInit(serialConfig());
|
||||
#endif
|
||||
|
||||
failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
|
||||
|
||||
rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
|
||||
|
||||
#ifdef GPS
|
||||
if (feature(FEATURE_GPS)) {
|
||||
gpsInit(
|
||||
serialConfig(),
|
||||
gpsConfig()
|
||||
);
|
||||
navigationInit(
|
||||
gpsProfile(),
|
||||
¤tProfile->pidProfile
|
||||
);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef LED_STRIP
|
||||
ledStripInit(ledStripConfig());
|
||||
|
||||
if (feature(FEATURE_LED_STRIP)) {
|
||||
ledStripEnable();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef TELEMETRY
|
||||
if (feature(FEATURE_TELEMETRY)) {
|
||||
telemetryInit();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_ESC_SENSOR
|
||||
if (feature(FEATURE_ESC_SENSOR)) {
|
||||
escSensorInit();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USB_CABLE_DETECTION
|
||||
usbCableDetectInit();
|
||||
#endif
|
||||
|
||||
#ifdef TRANSPONDER
|
||||
if (feature(FEATURE_TRANSPONDER)) {
|
||||
transponderInit(masterConfig.transponderData);
|
||||
transponderEnable();
|
||||
transponderStartRepeating();
|
||||
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_FLASHFS
|
||||
#if defined(USE_FLASH_M25P16)
|
||||
m25p16_init(flashConfig());
|
||||
#endif
|
||||
|
||||
flashfsInit();
|
||||
#endif
|
||||
|
||||
#ifdef USE_SDCARD
|
||||
if (feature(FEATURE_SDCARD)) {
|
||||
sdcardInsertionDetectInit();
|
||||
sdcard_init(sdcardConfig()->useDma);
|
||||
afatfs_init();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef BLACKBOX
|
||||
initBlackbox();
|
||||
#endif
|
||||
|
||||
if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
|
||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||
}
|
||||
gyroSetCalibrationCycles();
|
||||
#ifdef BARO
|
||||
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
|
||||
#endif
|
||||
|
||||
#ifdef VTX_SMARTAUDIO
|
||||
smartAudioInit();
|
||||
#endif
|
||||
|
||||
// start all timers
|
||||
// TODO - not implemented yet
|
||||
timerStart();
|
||||
|
||||
ENABLE_STATE(SMALL_ANGLE);
|
||||
DISABLE_ARMING_FLAG(PREVENT_ARMING);
|
||||
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
|
||||
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
|
||||
if (!loopbackPort->vTable) {
|
||||
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
|
||||
}
|
||||
serialPrint(loopbackPort, "LOOPBACK\r\n");
|
||||
#endif
|
||||
|
||||
// Now that everything has powered up the voltage and cell count be determined.
|
||||
|
||||
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
|
||||
batteryInit(batteryConfig());
|
||||
|
||||
#ifdef USE_DASHBOARD
|
||||
if (feature(FEATURE_DASHBOARD)) {
|
||||
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
|
||||
dashboardShowFixedPage(PAGE_GPS);
|
||||
#else
|
||||
dashboardResetPageCycling();
|
||||
dashboardEnablePageCycling();
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CJMCU
|
||||
LED2_ON;
|
||||
#endif
|
||||
|
||||
// Latch active features AGAIN since some may be modified by init().
|
||||
latchActiveFeatures();
|
||||
motorControlEnable = true;
|
||||
|
||||
fcTasksInit();
|
||||
systemState |= SYSTEM_STATE_READY;
|
||||
}
|
||||
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
void processLoopback(void) {
|
||||
if (loopbackPort) {
|
||||
uint8_t bytesWaiting;
|
||||
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
|
||||
uint8_t b = serialRead(loopbackPort);
|
||||
serialWrite(loopbackPort, b);
|
||||
};
|
||||
}
|
||||
}
|
||||
#else
|
||||
#define processLoopback()
|
||||
#endif
|
||||
|
||||
|
||||
void main_step(void)
|
||||
{
|
||||
|
|
@ -597,93 +40,3 @@ int main(void)
|
|||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef DEBUG_HARDFAULTS
|
||||
//from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/
|
||||
/**
|
||||
* hard_fault_handler_c:
|
||||
* This is called from the HardFault_HandlerAsm with a pointer the Fault stack
|
||||
* as the parameter. We can then read the values from the stack and place them
|
||||
* into local variables for ease of reading.
|
||||
* We then read the various Fault Status and Address Registers to help decode
|
||||
* cause of the fault.
|
||||
* The function ends with a BKPT instruction to force control back into the debugger
|
||||
*/
|
||||
void hard_fault_handler_c(unsigned long *hardfault_args)
|
||||
{
|
||||
volatile unsigned long stacked_r0 ;
|
||||
volatile unsigned long stacked_r1 ;
|
||||
volatile unsigned long stacked_r2 ;
|
||||
volatile unsigned long stacked_r3 ;
|
||||
volatile unsigned long stacked_r12 ;
|
||||
volatile unsigned long stacked_lr ;
|
||||
volatile unsigned long stacked_pc ;
|
||||
volatile unsigned long stacked_psr ;
|
||||
volatile unsigned long _CFSR ;
|
||||
volatile unsigned long _HFSR ;
|
||||
volatile unsigned long _DFSR ;
|
||||
volatile unsigned long _AFSR ;
|
||||
volatile unsigned long _BFAR ;
|
||||
volatile unsigned long _MMAR ;
|
||||
|
||||
stacked_r0 = ((unsigned long)hardfault_args[0]) ;
|
||||
stacked_r1 = ((unsigned long)hardfault_args[1]) ;
|
||||
stacked_r2 = ((unsigned long)hardfault_args[2]) ;
|
||||
stacked_r3 = ((unsigned long)hardfault_args[3]) ;
|
||||
stacked_r12 = ((unsigned long)hardfault_args[4]) ;
|
||||
stacked_lr = ((unsigned long)hardfault_args[5]) ;
|
||||
stacked_pc = ((unsigned long)hardfault_args[6]) ;
|
||||
stacked_psr = ((unsigned long)hardfault_args[7]) ;
|
||||
|
||||
// Configurable Fault Status Register
|
||||
// Consists of MMSR, BFSR and UFSR
|
||||
_CFSR = (*((volatile unsigned long *)(0xE000ED28))) ;
|
||||
|
||||
// Hard Fault Status Register
|
||||
_HFSR = (*((volatile unsigned long *)(0xE000ED2C))) ;
|
||||
|
||||
// Debug Fault Status Register
|
||||
_DFSR = (*((volatile unsigned long *)(0xE000ED30))) ;
|
||||
|
||||
// Auxiliary Fault Status Register
|
||||
_AFSR = (*((volatile unsigned long *)(0xE000ED3C))) ;
|
||||
|
||||
// Read the Fault Address Registers. These may not contain valid values.
|
||||
// Check BFARVALID/MMARVALID to see if they are valid values
|
||||
// MemManage Fault Address Register
|
||||
_MMAR = (*((volatile unsigned long *)(0xE000ED34))) ;
|
||||
// Bus Fault Address Register
|
||||
_BFAR = (*((volatile unsigned long *)(0xE000ED38))) ;
|
||||
|
||||
__asm("BKPT #0\n") ; // Break into the debugger
|
||||
}
|
||||
|
||||
#else
|
||||
void HardFault_Handler(void)
|
||||
{
|
||||
LED2_ON;
|
||||
|
||||
// fall out of the sky
|
||||
uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
|
||||
if ((systemState & requiredStateForMotors) == requiredStateForMotors) {
|
||||
stopMotors();
|
||||
}
|
||||
#ifdef TRANSPONDER
|
||||
// prevent IR LEDs from burning out.
|
||||
uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED;
|
||||
if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) {
|
||||
transponderIrDisable();
|
||||
}
|
||||
#endif
|
||||
|
||||
LED1_OFF;
|
||||
LED0_OFF;
|
||||
|
||||
while (1) {
|
||||
#ifdef LED2
|
||||
delay(50);
|
||||
LED2_TOGGLE;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue