Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity
betaflight/src/main/main.c
jflyper 6cf9086f56 CMS-OSD separation
2016-10-23 19:03:35 +09:00

744 lines
19 KiB
C
Raw Blame History

/*
* 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 <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 "drivers/nvic.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/dma.h"
#include "drivers/gpio.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_rx.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/io.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/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/display.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/serial_cli.h"
#include "io/transponder_ir.h"
#include "io/cms_types.h"
#include "io/cms.h"
#include "io/osd.h"
#include "io/vtx.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 "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"
extern uint8_t motorControlEnable;
#ifdef SOFTSERIAL_LOOPBACK
serialPort_t *loopbackPort;
#endif
#ifdef USE_DPRINTF
#include "common/printf.h"
#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
serialPort_t *debugSerialPort = NULL;
#define dprintf(x) if (debugSerialPort) printf x
#else
#define dprintf(x)
#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)
{
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
systemInit();
//i2cSetOverclock(masterConfig.i2c_overclock);
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = masterConfig.debug_mode;
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
#ifdef ALIENFLIGHTF3
ledInit(hardwareRevision == AFF3_REV_1 ? false : true);
#else
ledInit(false);
#endif
LED2_ON;
#ifdef USE_EXTI
EXTIInit();
#endif
#if defined(BUTTONS)
gpio_config_t buttonAGpioConfig = {
BUTTON_A_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
gpio_config_t buttonBGpioConfig = {
BUTTON_B_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow GPIO configuration to settle
if (!isMPUSoftReset()) {
uint8_t secondsRemaining = 5;
bool bothButtonsHeld;
do {
bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
systemReset();
}
delay(1000);
LED0_TOGGLE;
}
} while (bothButtonsHeld);
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.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(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
dmaInit();
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.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(&masterConfig.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(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
#ifdef USE_DPRINTF
// Setup debugSerialPort
debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0);
if (debugSerialPort) {
setPrintfSerialPort(debugSerialPort);
dprintf(("debugSerialPort: OK\r\n"));
}
#endif
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#ifdef USE_SERVOS
servoMixerInit(masterConfig.customServoMixer);
#endif
uint16_t idlePulse = masterConfig.motorConfig.mincommand;
if (feature(FEATURE_3D)) {
idlePulse = masterConfig.flight3DConfig.neutral3d;
}
if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
idlePulse = 0; // brushed motors
}
#ifdef USE_QUAD_MIXER_ONLY
motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT);
#else
motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount);
#endif
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
servoInit(&masterConfig.servoConfig);
}
#endif
#ifndef SKIP_RX_PWM_PPM
if (feature(FEATURE_RX_PPM)) {
ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol);
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
pwmRxInit(&masterConfig.pwmConfig);
}
pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode);
#endif
mixerConfigureOutput();
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(&masterConfig.beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_BST
bstInit(BST_DEVICE);
#endif
#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
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPIDEV_3);
}
#else
spiInit(SPIDEV_3);
#endif
#endif
#endif
#ifdef VTX
vtxInit();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_USART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI)
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.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)) {
osdInit();
}
#endif
#ifdef CMS
cmsInit();
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
// 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;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspSerialInit(mspFcInit());
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
&masterConfig.gpsProfile,
&currentProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.sonarConfig);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#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
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init(IO_TAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init(IO_TAG_NONE);
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
#ifdef STM32F4
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#endif
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles();
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#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(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#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)
{
scheduler();
processLoopback();
}
#ifndef NOMAIN
int main(void)
{
init();
while (true) {
main_step();
}
}
#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
Reference in a new issue View git blame Copy permalink