void gpsNmeaTest(void)
{
// set the baud rate of UART 1 for NMEA
uartSetBaudRate(1,4800);
// clear screen
vt100ClearScreen();
// initialize gps library
gpsInit();
// initialize gps packet decoder
nmeaInit();
// begin gps packet processing loop
while(1)
{
// process received gps packets until receive buffer is exhausted
while( nmeaProcess(uartGetRxBuffer(1)) );
// set cursor position to top left of screen
vt100SetCursorPos(0,0);
// print/dump current formatted GPS data
gpsInfoPrint();
// print UART 1 overflow status to verify that we're processing packets
// fast enough and that our receive buffer is large enough
rprintf("Uart1RxOvfl: %d\r\n",uartRxOverflow[1]);
// pause for 100ms
timerPause(100);
}
}
void systemInit(void)
{
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
///////////////////////////////////
checkFirstTime(false);
readEEPROM();
if (eepromConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
initMixer();
ledInit();
cliInit();
BLUE_LED_ON;
delay(20000); // 20 sec total delay for sensor stabilization - probably not long enough.....
adcInit();
batteryInit();
gpsInit();
i2cInit(I2C1);
i2cInit(I2C2);
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
rxInit();
spiInit(SPI2);
spiInit(SPI3);
telemetryInit();
timingFunctionsInit();
initFirstOrderFilter();
initGPS();
initMax7456();
initPID();
GREEN_LED_ON;
initMPU6000();
initMag(HMC5883L_I2C);
initPressure(MS5611_I2C);
}
void sensorMCUInit(void) {
gpsInit();
uartInit();
loggerInit();
spiMasterInit();
protParserInit();
#ifndef NO_MAGNETO
magnetoInit();
#endif
cubeInit();
aknControlData.sensorReboot =1;
}
void valuesInit(void)
{
timeout = devReg.timeOut * 20;
gpsInit(&root);
/* App info */
veItemAddChildByUid(&root, "Mgmt/ProcessName", &processName);
veItemAddChildByUid(&root, "Mgmt/ProcessVersion", &processVersion);
veItemAddChildByUid(&root, "Mgmt/Connection", &connection);
veItemSetFmt(&processName, veVariantFmt, &none);
veItemSetFmt(&processVersion, veVariantFmt, &none);
veItemSetFmt(&connection, veVariantFmt, &none);
}
static void PeripheralInit()
{
int status = RECONN_SUCCESS;
gEqptDescriptors.powerMeterFd = -1;
#if 0 // GPS has been removed from the reconn box
if((status = gpsInit(&(gEqptDescriptors.gpsFd))) != RECONN_SUCCESS)
{
reconnDebugPrint("%s: GPS Init Failed\n", __FUNCTION__);
gpsEnabled = FALSE;
}
else
{
reconnDebugPrint("GPS Initialized\n");
}
/* end GPS Init - GPS now configured */
#endif
if((status = SpectrumAnalyzerInit(&(gEqptDescriptors.analyzerFd))) != RECONN_SUCCESS)
{
reconnDebugPrint("Spectrum Analyzer Init Failed\n");
analyzerEnabled = FALSE;
}
else
{
reconnDebugPrint("Spectrum Analyzer Initialized\n");
}
if((status = dmmInit(&(gEqptDescriptors.dmmFd))) != RECONN_SUCCESS)
{
reconnDebugPrint("%s: DMM Init Failed\n", __FUNCTION__);
dmmEnabled = FALSE;
}
else
{
reconnDebugPrint("%s: DMM Initialized\n", __FUNCTION__);
}
// Power up the Wifi chip
if((status = reconnGpioAction(GPIO_57, ENABLE, NULL)) == RECONN_FAILURE)
{
reconnDebugPrint("%s: Could not power up WiFi device \n", __FUNCTION__);
}
else
{
reconnDebugPrint("%s: Wifi powered\n", __FUNCTION__);
}
}
void SensorDetectAndINI(void) // "enabledSensors" is "0" in config.c so all sensors disabled per default
{
int16_t deg, min;
uint8_t sig = 0;
bool ack = false;
bool haveMpu6k = false;
GyroScale16 = (16.0f / 16.4f) * RADX; // GYRO part. RAD per SECOND, take into account that gyrodata are div by X
if (mpu6050Detect(&acc, &gyro)) // Autodetect gyro hardware. We have MPU3050 or MPU6050.
{
haveMpu6k = true; // this filled up acc.* struct with init values
}
else if (l3g4200dDetect(&gyro))
{
havel3g4200d = true;
GyroScale16 = (16.0f / 14.2857f) * RADX; // GYRO part. RAD per SECOND, take into account that gyrodata are div by X
}
else if (!mpu3050Detect(&gyro))
{
failureMode(3); // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
}
sensorsSet(SENSOR_ACC); // ACC part. Will be cleared if not available
retry:
switch (cfg.acc_hdw)
{
case 0: // autodetect
case 1: // ADXL345
if (adxl345Detect(&acc)) accHardware = ACC_ADXL345;
if (cfg.acc_hdw == ACC_ADXL345) break;
case 2: // MPU6050
if (haveMpu6k)
{
mpu6050Detect(&acc, &gyro); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
if (cfg.acc_hdw == ACC_MPU6050) break;
}
case 3: // MMA8452
if (mma8452Detect(&acc))
{
accHardware = ACC_MMA8452;
if (cfg.acc_hdw == ACC_MMA8452) break;
}
}
if (accHardware == ACC_DEFAULT) // Found anything? Check if user f****d up or ACC is really missing.
{
if (cfg.acc_hdw > ACC_DEFAULT)
{
cfg.acc_hdw = ACC_DEFAULT; // Nothing was found and we have a forced sensor type. User probably chose a sensor that isn't present.
goto retry;
}
else sensorsClear(SENSOR_ACC); // We're really screwed
}
if (sensors(SENSOR_ACC)) acc.init();
if (haveMpu6k && accHardware == ACC_MPU6050) MpuSpecial = true;
else MpuSpecial = false;
if (feature(FEATURE_PASS)) return; // Stop here we need just ACC for Vibrationmonitoring if present
if (feature(FEATURE_GPS) && !SerialRCRX) gpsInit(cfg.gps_baudrate);// SerialRX and GPS can not coexist.
gyro.init(); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
if (havel3g4200d) l3g4200dConfig();
else if (!haveMpu6k) mpu3050Config();
Gyro_Calibrate(); // Do Gyrocalibration here (is blocking), provides nice Warmuptime for the following rest!
#ifdef MAG
if (hmc5883lDetect())
{
sensorsSet(SENSOR_MAG);
hmc5883lInit(magCal); // Crashpilot: Calculate Gains / Scale
deg = cfg.mag_dec / 100; // calculate magnetic declination
min = cfg.mag_dec % 100;
magneticDeclination = ((float)deg + ((float)min / 60.0f));// heading is in decimaldeg units NO 0.1 deg shit here
}
#endif
#ifdef BARO // No delay necessary since Gyrocal blocked a lot already
ack = i2cRead(0x77, 0x77, 1, &sig); // Check Baroadr.(MS & BMP) BMP will say hello here, MS not
if ( ack) ack = bmp085Detect(&baro); // Are we really dealing with BMP?
if (!ack) ack = ms5611Detect(&baro); // No, Check for MS Baro
if (ack) sensorsSet(SENSOR_BARO);
if(cfg.esc_nfly) ESCnoFlyThrottle = constrain_int(cfg.esc_nfly, cfg.esc_min, cfg.esc_max); // Set the ESC PWM signal threshold for not flyable RPM
else ESCnoFlyThrottle = cfg.esc_min + (((cfg.esc_max - cfg.esc_min) * 5) / 100); // If not configured, take 5% above esc_min
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) Sonar_init(); // Initialize Sonars here depending on Rc configuration.
SonarLandWanted = cfg.snr_land; // Variable may be overwritten by failsave
#endif
MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
}
void init(void)
{
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(systemConfig()->emf_avoidance);
#endif
i2cSetOverclock(systemConfig()->i2c_highspeed);
systemInit();
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = debugConfig()->debug_mode;
#ifdef USE_EXTI
EXTIInit();
#endif
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_1) {
ledInit(false);
} else {
ledInit(true);
}
#else
ledInit(false);
#endif
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef BUTTONS
buttonsInit();
if (!isMPUSoftReset()) {
buttonsHandleColdBootButtonPresses();
}
#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
dmaInit();
serialInit(feature(FEATURE_SOFTSERIAL));
mixerInit(customMotorMixer(0));
#ifdef USE_SERVOS
mixerInitServos(customServoMixer(0));
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
sonarGPIOConfig_t sonarGPIOConfig;
if (feature(FEATURE_SONAR)) {
bool usingCurrentMeterIOPins = (feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC);
sonarHardware = sonarGetHardwareConfiguration(usingCurrentMeterIOPins);
sonarGPIOConfig.triggerGPIO = sonarHardware->trigger_gpio;
sonarGPIOConfig.triggerPin = sonarHardware->trigger_pin;
sonarGPIOConfig.echoGPIO = sonarHardware->echo_gpio;
sonarGPIOConfig.echoPin = sonarHardware->echo_pin;
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2);
#endif
#if defined(USE_UART3)
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3);
#endif
#if defined(USE_UART4)
pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4);
#endif
#if defined(USE_UART5)
pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = (
feature(FEATURE_AMPERAGE_METER)
&& batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC
);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
pwm_params.servoPwmRate = servoConfig()->servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = motorConfig()->motor_pwm_rate;
pwm_params.idlePulse = calculateMotorOff();
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit();
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params);
mixerUsePWMIOConfiguration(pwmIOConfiguration);
#ifdef DEBUG_PWM_CONFIGURATION
debug[2] = pwmIOConfiguration->pwmInputCount;
debug[3] = pwmIOConfiguration->ppmInputCount;
#endif
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#ifdef STM32F303xC
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPI3);
}
#else
spiInit(SPI3);
#endif
#endif
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_UART3);
}
#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(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.channelMask = 0;
#ifdef ADC_BATTERY
adc_params.channelMask = (feature(FEATURE_VBAT) ? ADC_CHANNEL_MASK(ADC_BATTERY) : 0);
#endif
#ifdef ADC_RSSI
adc_params.channelMask |= (feature(FEATURE_RSSI_ADC) ? ADC_CHANNEL_MASK(ADC_RSSI) : 0);
#endif
#ifdef ADC_AMPERAGE
adc_params.channelMask |= (feature(FEATURE_AMPERAGE_METER) ? ADC_CHANNEL_MASK(ADC_AMPERAGE) : 0);
#endif
#ifdef ADC_POWER_12V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_12V);
#endif
#ifdef ADC_POWER_5V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_5V);
#endif
#ifdef ADC_POWER_3V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_3V);
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.channelMask |= (hardwareRevision >= NAZE32_REV5) ? ADC_CHANNEL_MASK(ADC_EXTERNAL) : 0;
#endif
adcInit(&adc_params);
#endif
initBoardAlignment();
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit();
}
#endif
#ifdef NAZE
if (hardwareRevision < NAZE32_REV5) {
gyroConfig()->gyro_sync = 0;
}
#endif
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
flashLedsAndBeep();
mspInit();
mspSerialInit();
const uint16_t pidPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
pidSetTargetLooptime(pidPeriodUs * gyroConfig()->pid_process_denom);
pidInitFilters(pidProfile());
#ifdef USE_SERVOS
mixerInitialiseServoFiltering(targetPidLooptime);
#endif
imuInit();
#ifdef USE_CLI
cliInit();
#endif
failsafeInit();
rxInit(modeActivationProfile()->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit();
navigationInit(pidProfile());
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit();
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(transponderConfig()->data);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#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
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#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
if (feature(FEATURE_VBAT)) {
// Now that everything has powered up the voltage and cell count be determined.
voltageMeterInit();
batteryInit();
}
if (feature(FEATURE_AMPERAGE_METER)) {
amperageMeterInit();
}
#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;
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 configureScheduler(void)
{
schedulerInit();
setTaskEnabled(TASK_SYSTEM, true);
uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
rescheduleTask(TASK_GYRO, gyroPeriodUs);
setTaskEnabled(TASK_GYRO, true);
rescheduleTask(TASK_PID, gyroPeriodUs);
setTaskEnabled(TASK_PID, true);
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
}
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_AMPERAGE_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
}
int main(int argc, char* argv[])
{
char i;
/*
//CBRef A = newCircBuffer(10);
//CBRef B = newCircBuffer(20);
struct CircBuffer Anp;
struct CircBuffer Bnp;
CBRef A = &Anp;
CBRef B = &Bnp;
newCircBuffer(A);
newCircBuffer(B);
for(i=0; i<=12; i++)
{
writeBack(B, i);
writeBack(A, 15-i);
}
//printCircBuf(A);
//printCircBuf(B);
printf("\n");
printf("\n");
for(i=0; i<=12; i++)
{
printf("Read byte A: %d\n", readFront(A));
printf("Read byte B: %d\n", readFront(B));
//printCircBuf(A);
}
makeEmpty(A);
//printCircBuf(A);
freeCircBuffer(&A);
freeCircBuffer(&B);
*/
unsigned char msg1 [] = "@$GPRMC,040302.663,A,3939.7,N,10506.6,W,5.27,358.86,200804,,*1A\r\n";
unsigned char msg2 [] = "N$GPGGA,213924.000,4221.1129,N,07102.9146,W,1,04,3.9,129.7,M,-33.7,M,,0000*68\r\n";
unsigned char msg3 [] = "&$GPGGA,213922.000,4221.1129,N,07102.91";
unsigned char msg4 [] = "(46,W,1,04,3.9,129.7,M,-33.7,M,,0000*6E\r\n";
unsigned char outBuffer [128] ={0}, parsedData[128]={0};
gpsInit();
printf("Message 1\n");
gpsSeparate(msg2, outBuffer);
printf("\n");
printf(outBuffer);
printf("\nValid: %d Type:%d", outBuffer[MSIZE-1], outBuffer[0]);
printf("\n");
printf(msg2);
gpsParse(outBuffer, parsedData);
for(i=0;i<6;i++){
printf("%d ", parsedData[i]);
}
tFloatToChar flCh;
tIntToChar inCh;
flCh.chData[0] = parsedData[6];
flCh.chData[1] = parsedData[7];
flCh.chData[2] = parsedData[8];
flCh.chData[3] = parsedData[9];
printf("%f ", flCh.flData);
flCh.chData[0] = parsedData[10];
flCh.chData[1] = parsedData[11];
flCh.chData[2] = parsedData[12];
flCh.chData[3] = parsedData[13];
printf("%f ", flCh.flData);
flCh.chData[0] = parsedData[14];
flCh.chData[1] = parsedData[15];
flCh.chData[2] = parsedData[16];
flCh.chData[3] = parsedData[17];
printf("%f ", flCh.flData);
inCh.chData[0] = parsedData[18];
inCh.chData[1] = parsedData[19];
printf("%d ", inCh.inData);
inCh.chData[0] = parsedData[20];
inCh.chData[1] = parsedData[21];
printf("%d ", inCh.inData);
inCh.chData[0] = parsedData[22];
inCh.chData[1] = parsedData[23];
printf("%d ", inCh.inData);
for(i=24;i<26;i++){
printf("%d ", parsedData[i]);
}
printf("\n");
unsigned char thischr [] = "$PMTK251,19200*22\r\n";
printf("Baud Rate %d", getChecksum(thischr, 17));
unsigned char thischr2 [] = "$PMTK300,200,0,0,0,0*2F\r\n";
printf("Frequency %d", getChecksum(thischr2, 23));
unsigned char thischr3 [] = "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
printf("Sentences %d", getChecksum(thischr3, 49));
/*printf("Message 2\n");
gpsSeparate(msg2, outBuffer);
printf("\n");
printf(outBuffer);
printf("\nValid: %d Type:%d", outBuffer[MSIZE-1], outBuffer[0]);
printf("\n");
printf(msg2);
printf("First Call to GPS Separate\n");
gpsSeparate(msg3, outBuffer);
printf("============================\n");
printf("Second Call to GPS Separate\n");
gpsSeparate(msg4, outBuffer);
printf("============================\n");
printf(outBuffer);
printf("\nValid: %d Type:%d", outBuffer[MSIZE-1], outBuffer[0]);
printf("\n");
printf(msg3);
printf(msg4);
printf("============================\n");
printf("============================\n");
printf("============================\n");
printf("============================\n");
printf("Char: %d, Short: %d, Int: %d, Float: %d, Long:%d", sizeof(char), sizeof(short), sizeof(int), sizeof(float), sizeof(long));
printf("\n");
*/
return(0);
}
int main(void)
{
//***********************************DEBUG********************************************/
#if 0
nmeaPOS p1,p2;
nmeaINFO info;
info.lat = 5547.1206;
info.lon = 4906.2111;
nmea_info2pos(&info, &p1);
info.lat = 5547.1221;
info.lon = 4906.208;
nmea_info2pos(&info, &p2);
m += 23;
u32 t = nmea_distance(&p1, &p2);
if(m)
#endif
//***********************************END OF DEBUG********************************************/
NVIC_Configuration(); //for all peripheria
if (SysTick_Config(SystemCoreClock / 1000)) //1ms
{
/* Capture error */
while (1);
}
Delay(500);
USBIniPin();
signUSBMass = USBDetectPin();
signUSBMass = 0; //deb
#if not defined (VER_3)
USBCommonIni();
#endif
if(signUSBMass)
{
#if defined (VER_3)
USBCommonIni();
#endif
while (bDeviceState != CONFIGURED);
}
else //if(!signUSBMass)
{
/* Flash unlock */
FLASH_Unlock();
/* Clear All pending flags */
FLASH_ClearFlag(FLASH_FLAG_BSY | FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
//***********************************DEBUG********************************************/
//***********************************END OF DEBUG********************************************/
/* Enable CRC clock */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
ledDioGPIOInit();
led_dn(BOARD_LED_ON);
led_mid(BOARD_LED_ON);
#if defined (VER_3)
led_up(BOARD_LED_ON);
ibuttonInit();
rfmodemInit();
#endif
Delay(1000);
alarmInit();
BKPInit();
//timer6Init();
//rs485Init();
#if not defined (VER_3)
ais326dq_init();
#endif
//ais326dq_data(&ais326dq_out);
/*ADC*/
adcInit();
/*GPS*/
gpsInit();
/* reading settings */
readConfig();
/*MODEM*/
gprsModemInit();
gprsModemOn(innerState.activeSIMCard);
//***********************************DEBUG********************************************/
//GSMSpeaker(1);
//***********************************END OF DEBUG********************************************/
#ifndef BRIDGE_USB_GSM
setupGSM();
ftpGSMPrepare();
packetsIni();
#endif
led_dn(BOARD_LED_OFF);
led_mid(BOARD_LED_OFF);
#if defined (VER_3)
led_up(BOARD_LED_OFF);
#endif
rtc_init();
rtc_gettime(&rtc);
#if 1 /* WATCH DOG */
/* IWDG timeout equal to 3.27 sec (the timeout may varies due to LSI frequency dispersion) */
/* Enable write access to IWDG_PR and IWDG_RLR registers */
IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
/* IWDG counter clock: 40KHz(LSI) / 32 = 1.25 KHz */
IWDG_SetPrescaler(IWDG_Prescaler_64); //32
/* Set counter reload value to 0xFFF */
IWDG_SetReload(0xFFF);
/* Reload IWDG counter */
IWDG_ReloadCounter();
/* Enable IWDG (the LSI oscillator will be enabled by hardware) */
IWDG_Enable();
setTimerIWDG(ONE_SEC);
#endif
initSD();
#if defined (VER_3)
//DACInit();
#endif
/* Log */
saveSDInfo((u8 *)"TURN ON BLOCK ",strlen((u8 *)"TURN ON BLOCK "), SD_SENDED, SD_TYPE_MSG_LOG );
//saveSDInfo((u8 *)readRTCTime(&rtc),strlen((const char *)readRTCTime(&rtc)), SD_SENDED, SD_TYPE_MSG_LOG );
#if defined (VER_3)
//DACSpeaker(1);
//wp_play("0:/sound.wav");
//DACSpeaker(0);
#endif
} //if(!signUSBMass)
while (1)
{
if(!signUSBMass)
{
monitorWatchDog();
#ifndef BRIDGE_USB_GPS
if(!innerState.bootFTPStarted)
gpsHandling();
#endif
#ifndef BRIDGE_USB_GSM
if(!innerState.flagTmpDebug)
loopGSM();
loopFTP();
UpdatingFlash();
if(!innerState.bootFTPStarted)
naviPacketHandle();
rcvPacketHandle();
rcvPacketHandleUSB();
#endif
#if !defined (VER_3)
buttonScan();
accelScan();
#endif
//rs485Analyse();
handleFOS();
executeDelayedCmd();
#if defined (VER_3)
#if 0
if(innerState.flagDebug)
{
DACSpeaker(1);
/* Start DAC Channel1 conversion by software */
//a += 300;
//DAC_SetChannel1Data(DAC_Align_12b_R, 4000);
//DAC_SetChannel1Data(DAC_Align_12b_L, a); //for saw
//DAC_SetChannel1Data(DAC_Align_8b_R, a);
//DAC_SetChannel1Data(DAC_Align_12b_R, 4095);
//DAC_SetChannel1Data(DAC_Align_12b_R, 0);
//for (a = 0; a<4095; ++a)
//for(;;)
// DAC_SetChannel1Data(DAC_Align_12b_R, 0);
//DAC_SetChannel1Data(DAC_Align_12b_R, 0);
//for ( ; ; )
//{
// DAC_SoftwareTriggerCmd(DAC_Channel_1, ENABLE);
//}
DAC_SoftwareTriggerCmd(DAC_Channel_1, ENABLE); //debugga
}
else
{
DACSpeaker(0);
}
#endif
#endif
}
else
handleUSBPresent();
} //while(1)
}
void init(void)
{
#ifdef USE_HAL_DRIVER
HAL_Init();
#endif
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
#if !defined(USE_HAL_DRIVER)
dmaInit();
#endif
#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
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();
#ifdef USE_SERVOS
servoConfigureOutput();
#endif
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(&masterConfig.beeperConfig);
#endif
/* temp until PGs are implemented. */
#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
#ifdef USE_SPI_DEVICE_4
spiInit(SPIDEV_4);
#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
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();
mspFcInit();
mspSerialInit();
#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,
¤tProfile->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
#if defined(STM32F4) || defined(STM32F7)
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;
}
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
bool sensorsOK = false;
initPrintfSupport();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
#ifdef NAZE
detectHardwareRevision();
#endif
systemInit();
#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
ledInit();
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioMode = Mode_Out_OD,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
.gpioPeripheral = BEEP_PERIPHERAL,
.isInverted = false
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#endif
#ifdef NAZE
updateHardwareRevision();
#endif
#ifdef USE_I2C
#ifdef NAZE
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
}
#else
// Configure the rest of the stuff
i2cInit(I2C_DEVICE);
#endif
#endif
#if !defined(SPARKY)
drv_adc_config_t adc_params;
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
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination);
// if gyro was not detected due to whatever reason, we give up now.
if (!sensorsOK)
failureMode(3);
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
imuInit();
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
serialInit(&masterConfig.serialConfig);
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(SERIAL_PORT_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
failsafe = failsafeInit(&masterConfig.rxConfig);
beepcodeInit(failsafe);
rxInit(&masterConfig.rxConfig, failsafe);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
Sonar_init();
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY))
initTelemetry();
#endif
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#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.
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayEnablePageCycling();
#endif
}
#endif
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
while (1) {
loop();
processLoopback();
}
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
#if 0
// PC12, PA15
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
GPIOA->BRR = 0x8000; // set low 15
GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
GPIOC->BRR = 0x1000; // set low 12
#endif
#if 0
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOB->BRR = 0x18; // set low 4 & 3
GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif
systemInit();
#ifdef USE_LAME_PRINTF
init_printf(NULL, _putc);
#endif
checkFirstTime(false);
readEEPROM();
// configure power ADC
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel;
else {
adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0;
}
adcInit(&adc_params);
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
mixerInit(); // this will set useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;
break;
case 9:
pwm_params.adcChannel = PWM8;
break;
default:
pwm_params.adcChannel = 0;
break;
}
pwmInit(&pwm_params);
// configure PWM/CPPM read function. spektrum below will override that
rcReadRawFunc = pwmReadRawRC;
if (feature(FEATURE_SPEKTRUM)) {
spektrumInit();
rcReadRawFunc = spektrumReadRawRC;
} else {
// spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
if (feature(FEATURE_GPS))
gpsInit(mcfg.gps_baudrate);
}
#ifdef SONAR
// sonar stuff only works with PPM
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_SONAR))
Sonar_init();
}
#endif
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsAutodetect();
imuInit(); // Mag is initialized inside imuInit
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit();
serialInit(mcfg.serial_baudrate);
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1;
// loopy
while (1) {
loop();
}
}
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(_err_perf);
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
// auto-detect connected rangefinders while not armed
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
detectDistanceSensors();
}
// reset pos, vel, and terrain on arming
if (!_lastArmedState && armedState) {
// we just armed, we are at home position on the ground
_x(X_x) = 0;
_x(X_y) = 0;
// the pressure altitude of home may have drifted, so we don't
// reset z to zero
// reset flow integral
_flowX = 0;
_flowY = 0;
// we aren't moving, all velocities are zero
_x(X_vx) = 0;
_x(X_vy) = 0;
_x(X_vz) = 0;
// assume we are on the ground, so terrain alt is local alt
_x(X_tz) = _x(X_z);
// reset lowpass filter as well
_xLowPass.setState(_x);
}
_lastArmedState = armedState;
// see which updates are available
bool flowUpdated = _sub_flow.updated();
bool paramsUpdated = _sub_param_update.updated();
bool baroUpdated = _sub_sensor.updated();
bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
bool homeUpdated = _sub_home.updated();
bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
bool mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
// get new data
updateSubscriptions();
// update parameters
if (paramsUpdated) {
updateParams();
updateSSParams();
}
// update home position projection
if (homeUpdated) {
updateHome();
}
// is xy valid?
bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
if (_validXY) {
// if valid and gps has timed out, set to not valid
if (!xy_stddev_ok && !_gpsInitialized) {
_validXY = false;
}
} else {
if (xy_stddev_ok) {
_validXY = true;
}
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_validZ) {
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && !_baroInitialized) {
_validZ = false;
}
} else {
if (z_stddev_ok) {
_validZ = true;
}
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_validTZ) {
if (!tz_stddev_ok) {
_validTZ = false;
}
} else {
if (tz_stddev_ok) {
_validTZ = true;
}
}
// timeouts
if (_validXY) {
_time_last_xy = _timeStamp;
}
if (_validZ) {
_time_last_z = _timeStamp;
}
if (_validTZ) {
_time_last_tz = _timeStamp;
}
// check timeouts
checkTimeouts();
// if we have no lat, lon initialize projection at 0,0
if (_validXY && !_map_ref.init_done) {
map_projection_init(&_map_ref,
_init_home_lat.get(),
_init_home_lon.get());
}
// reinitialize x if necessary
bool reinit_x = false;
for (int i = 0; i < n_x; i++) {
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
break;
}
}
if (reinit_x) {
for (int i = 0; i < n_x; i++) {
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
}
// reinitialize P if necessary
bool reinit_P = false;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j < n_x; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
reinit_P = true;
break;
}
}
if (reinit_P) { break; }
}
if (reinit_P) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
initP();
}
// do prediction
predict();
// sensor corrections/ initializations
if (gpsUpdated) {
if (!_gpsInitialized) {
gpsInit();
} else {
gpsCorrect();
}
}
if (baroUpdated) {
if (!_baroInitialized) {
baroInit();
} else {
baroCorrect();
}
}
if (lidarUpdated) {
if (!_lidarInitialized) {
lidarInit();
} else {
lidarCorrect();
}
}
if (sonarUpdated) {
if (!_sonarInitialized) {
sonarInit();
} else {
sonarCorrect();
}
}
if (flowUpdated) {
if (!_flowInitialized) {
flowInit();
} else {
perf_begin(_loop_perf);// TODO
flowCorrect();
//perf_count(_interval_perf);
perf_end(_loop_perf);
}
}
if (visionUpdated) {
if (!_visionInitialized) {
visionInit();
} else {
visionCorrect();
}
}
if (mocapUpdated) {
if (!_mocapInitialized) {
mocapInit();
} else {
mocapCorrect();
}
}
if (_altHomeInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
if (_validXY) {
publishGlobalPos();
}
}
// propagate delayed state, no matter what
// if state is frozen, delayed state still
// needs to be propagated with frozen state
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(_err_perf);
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
// auto-detect connected rangefinders while not armed
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
detectDistanceSensors();
}
// reset pos, vel, and terrain on arming
// XXX this will be re-enabled for indoor use cases using a
// selection param, but is really not helping outdoors
// right now.
// if (!_lastArmedState && armedState) {
// // we just armed, we are at origin on the ground
// _x(X_x) = 0;
// _x(X_y) = 0;
// // reset Z or not? _x(X_z) = 0;
// // we aren't moving, all velocities are zero
// _x(X_vx) = 0;
// _x(X_vy) = 0;
// _x(X_vz) = 0;
// // assume we are on the ground, so terrain alt is local alt
// _x(X_tz) = _x(X_z);
// // reset lowpass filter as well
// _xLowPass.setState(_x);
// _aglLowPass.setState(0);
// }
_lastArmedState = armedState;
// see which updates are available
bool flowUpdated = _sub_flow.updated();
bool paramsUpdated = _sub_param_update.updated();
bool baroUpdated = _sub_sensor.updated();
bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
bool mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
bool landUpdated = (
(_sub_land.get().landed ||
((!_sub_armed.get().armed) && (!_sub_land.get().freefall)))
&& (!(_lidarInitialized || _mocapInitialized || _visionInitialized || _sonarInitialized))
&& ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE));
// get new data
updateSubscriptions();
// update parameters
if (paramsUpdated) {
updateParams();
updateSSParams();
}
// is xy valid?
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
vxy_stddev_ok = true;
}
if (_validXY) {
// if valid and gps has timed out, set to not valid
if (!vxy_stddev_ok && !_gpsInitialized) {
_validXY = false;
}
} else {
if (vxy_stddev_ok) {
if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
_validXY = true;
}
}
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_validZ) {
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && !_baroInitialized) {
_validZ = false;
}
} else {
if (z_stddev_ok) {
_validZ = true;
}
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_validTZ) {
if (!tz_stddev_ok) {
_validTZ = false;
}
} else {
if (tz_stddev_ok) {
_validTZ = true;
}
}
// timeouts
if (_validXY) {
_time_last_xy = _timeStamp;
}
if (_validZ) {
_time_last_z = _timeStamp;
}
if (_validTZ) {
_time_last_tz = _timeStamp;
}
// check timeouts
checkTimeouts();
// if we have no lat, lon initialize projection at 0,0
if (_validXY && !_map_ref.init_done) {
map_projection_init(&_map_ref,
_init_origin_lat.get(),
_init_origin_lon.get());
}
// reinitialize x if necessary
bool reinit_x = false;
for (int i = 0; i < n_x; i++) {
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
break;
}
}
if (reinit_x) {
for (int i = 0; i < n_x; i++) {
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
}
// force P symmetry and reinitialize P if necessary
bool reinit_P = false;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j <= i; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
reinit_P = true;
}
if (i == j) {
// make sure diagonal elements are positive
if (_P(i, i) <= 0) {
reinit_P = true;
}
} else {
// copy elememnt from upper triangle to force
// symmetry
_P(j, i) = _P(i, j);
}
if (reinit_P) { break; }
}
if (reinit_P) { break; }
}
if (reinit_P) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
initP();
}
// do prediction
predict();
// sensor corrections/ initializations
if (gpsUpdated) {
if (!_gpsInitialized) {
gpsInit();
} else {
gpsCorrect();
}
}
if (baroUpdated) {
if (!_baroInitialized) {
baroInit();
} else {
baroCorrect();
}
}
if (lidarUpdated) {
if (!_lidarInitialized) {
lidarInit();
} else {
lidarCorrect();
}
}
if (sonarUpdated) {
if (!_sonarInitialized) {
sonarInit();
} else {
sonarCorrect();
}
}
if (flowUpdated) {
if (!_flowInitialized) {
flowInit();
} else {
perf_begin(_loop_perf);// TODO
flowCorrect();
//perf_count(_interval_perf);
perf_end(_loop_perf);
}
}
if (visionUpdated) {
if (!_visionInitialized) {
visionInit();
} else {
visionCorrect();
}
}
if (mocapUpdated) {
if (!_mocapInitialized) {
mocapInit();
} else {
mocapCorrect();
}
}
if (landUpdated) {
if (!_landInitialized) {
landInit();
} else {
landCorrect();
}
}
if (_altOriginInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
_pub_innov.update();
if (_validXY) {
publishGlobalPos();
}
}
// propagate delayed state, no matter what
// if state is frozen, delayed state still
// needs to be propagated with frozen state
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
detectHardwareRevision();
systemInit();
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
ledInit();
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;
}
}
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
memset(&pwm_params, 0, sizeof(pwm_params));
const sonarHardware_t *sonarHardware = NULL;
if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
sonarGPIOConfig_t sonarGPIOConfig = {
.gpio = SONAR_GPIO,
.triggerPin = sonarHardware->echo_pin,
.echoPin = sonarHardware->trigger_pin,
};
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
pwm_params.useSonar = feature(FEATURE_SONAR);
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
.gpioMode = Mode_Out_PP,
.isInverted = true
};
beeperInit(&beeperConfig);
initInverter();
spiInit(SPI1);
spiInit(SPI2);
updateHardwareRevision();
serialRemovePort(SERIAL_PORT_USART3);
i2cInit(I2C_DEVICE);
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 = true;
// optional ADC5 input on rev.5 hardware
adcInit(&adc_params);
initBoardAlignment(&masterConfig.boardAlignment);
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
// 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;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
if (sensors(SENSOR_MAG))
compassInit();
imuInit();
mspInit(&masterConfig.serialConfig);
cliInit(&masterConfig.serialConfig);
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig);
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
m25p16_init();
flashfsInit();
initBlackbox();
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
if (feature(FEATURE_DISPLAY)) {
displayResetPageCycling();
displayEnablePageCycling();
}
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
int main(void) {
init();
//Mine
printf("\r\n");
printf("Init Finished!\r\n");
printf("System Init need %d ms\r\n", millis());
printf("############# Begin Test ###############\r\n");
printf("############# End Test ###############\r\n");
while (1) {
loop();
}
}
void HardFault_Handler(void)
{
// fall out of the sky
uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredState) == requiredState) {
stopMotors();
}
while (1);
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
bool sensorsOK = false;
initEEPROM();
readEEPROM();
SetSysClock(mcfg.emf_avoidance);
//hw_revision = 1;
systemInit();
delay(100);
activateConfig();
i2cInit(I2C_DEVICE);
// Do muc pin
adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0;
// configure rssi ADC
adc_params.rssiAdcChannel = 0;
mcfg.rssi_adc_channel = 0;
adcInit(&adc_params);
// Kiem tra Volt cua Pin
batteryInit();
initBoardAlignment();
sensorsSet(SENSORS_SET);
sensorsOK = sensorsAutodetect();
imuInit();
mixerInit(); // xem lai
serialInit(115200);//(mcfg.serial_baudrate);
pwm_params.motorPwmRate = 400;
pwm_params.pwmFilter = mcfg.pwm_filter;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.syncPWM = feature(FEATURE_SYNCPWM);
pwm_params.fastPWM = feature(FEATURE_FASTPWM);
pwm_params.servoCenterPulse = mcfg.midrc;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
pwm_params.adcChannel = 0;
pwmInit(&pwm_params);
for (i = 0; i < RC_CHANS; i++)
rcData[i] = 1502;
rcReadRawFunc = pwmReadRawRC;
core.numRCChannels = MAX_INPUTS;
gpsInit(mcfg.gps_baudrate);
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLE = 1;
while (1) {
loop();
}
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
#if 0
// PC12, PA15
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
GPIOA->BRR = 0x8000; // set low 15
GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
GPIOC->BRR = 0x1000; // set low 12
#endif
#if 0
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOB->BRR = 0x18; // set low 4 & 3
GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif
systemInit();
readEEPROM();
checkFirstTime(false);
serialInit(cfg.serial_baudrate);
// We have these sensors
#ifndef FY90Q
// AfroFlight32
sensorsSet(SENSOR_ACC | SENSOR_BARO | SENSOR_MAG);
#else
// FY90Q
sensorsSet(SENSOR_ACC);
#endif
mixerInit(); // this will set useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped
if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = cfg.motor_pwm_rate;
pwm_params.servoPwmRate = cfg.servo_pwm_rate;
pwmInit(&pwm_params);
// configure PWM/CPPM read function. spektrum will override that
rcReadRawFunc = pwmReadRawRC;
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsAutodetect();
imuInit(); // Mag is initialized inside imuInit
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit();
if (feature(FEATURE_SPEKTRUM)) {
spektrumInit();
rcReadRawFunc = spektrumReadRawRC;
} else {
// spektrum and GPS are mutually exclusive
// Optional GPS - available only when using PPM, otherwise required pins won't be usable
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_GPS))
gpsInit(cfg.gps_baudrate);
#ifdef SONAR
if (feature(FEATURE_SONAR))
Sonar_init();
#endif
}
}
previousTime = micros();
if (cfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000;
f.SMALL_ANGLES_25 = 1;
// loopy
while (1) {
loop();
}
}
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_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
const sonarHardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType);
if (sonarHardware) {
pwm_params.useSonar = true;
pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag;
pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag;
}
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
#if defined(USE_UART2) && defined(STM32F40_41xxx)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#if defined(USE_UART6) && defined(STM32F40_41xxx)
pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED;
// Configurator feature abused for enabling Fast PWM
pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED);
pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol;
pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
pwm_params.idlePulse = 0; // brushed motors
}
#ifdef CC3D
pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
#endif
#ifndef SKIP_RX_PWM_PPM
pwmRxInit(masterConfig.inputFilteringMode);
#endif
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm);
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.ioTag = IO_TAG(BEEPER),
#ifdef BEEPER_INVERTED
.isOD = false,
.isInverted = true
#else
.isOD = true,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.isOD = false;
beeperConfig.isInverted = true;
}
#endif
/* temp until PGs are implemented. */
#ifdef BLUEJAYF4
if (hardwareRevision <= BJF4_REV2) {
beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
}
#endif
#ifdef CC3D
if (masterConfig.use_buzzer_p6 == 1)
beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
#endif
beeperInit(&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
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();
mspInit(&masterConfig.serialConfig);
#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,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit();
}
#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(IOTAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init(IOTAG_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 * 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;
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_init(void)
{
init();
/* Setup scheduler */
schedulerInit();
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
switch (gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future
case 500:
case 375:
case 250:
case 125:
accTargetLooptime = 1000;
break;
default:
case 1000:
#ifdef STM32F10X
accTargetLooptime = 1000;
#else
accTargetLooptime = 1000;
#endif
}
rescheduleTask(TASK_ACCEL, accTargetLooptime);
}
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(USE_SPI) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
// Reschedule telemetry to 500hz for Jeti Exbus
if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) rescheduleTask(TASK_TELEMETRY, 2000);
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
#ifdef OSD
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
#endif
#ifdef USE_BST
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
#endif
}
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
// auto-detect connected rangefinders while not armed
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
// detect distance sensors
for (int i = 0; i < N_DIST_SUBS; i++) {
uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
if (s == _sub_lidar || s == _sub_sonar) { continue; }
if (s->updated()) {
s->update();
if (s->get().timestamp == 0) { continue; }
if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_lidar == nullptr) {
_sub_lidar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %i", msg_label, i);
} else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_sonar == nullptr) {
_sub_sonar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %i", msg_label, i);
}
}
}
}
// reset pos, vel, and terrain on arming
// XXX this will be re-enabled for indoor use cases using a
// selection param, but is really not helping outdoors
// right now.
// if (!_lastArmedState && armedState) {
// // we just armed, we are at origin on the ground
// _x(X_x) = 0;
// _x(X_y) = 0;
// // reset Z or not? _x(X_z) = 0;
// // we aren't moving, all velocities are zero
// _x(X_vx) = 0;
// _x(X_vy) = 0;
// _x(X_vz) = 0;
// // assume we are on the ground, so terrain alt is local alt
// _x(X_tz) = _x(X_z);
// // reset lowpass filter as well
// _xLowPass.setState(_x);
// _aglLowPass.setState(0);
// }
_lastArmedState = armedState;
// see which updates are available
bool paramsUpdated = _sub_param_update.updated();
_baroUpdated = false;
if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) {
int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) {
uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
_sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp != _timeStampLastBaro) {
_baroUpdated = true;
_timeStampLastBaro = baro_timestamp;
}
}
}
_flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
_mocapUpdated = _sub_mocap.updated();
_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
bool targetPositionUpdated = _sub_landing_target_pose.updated();
// get new data
updateSubscriptions();
// update parameters
if (paramsUpdated) {
updateParams();
updateSSParams();
}
// is xy valid?
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) {
vxy_stddev_ok = true;
}
if (_estimatorInitialized & EST_XY) {
// if valid and gps has timed out, set to not valid
if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) {
_estimatorInitialized &= ~EST_XY;
}
} else {
if (vxy_stddev_ok) {
if (!(_sensorTimeout & SENSOR_GPS)
|| !(_sensorTimeout & SENSOR_FLOW)
|| !(_sensorTimeout & SENSOR_VISION)
|| !(_sensorTimeout & SENSOR_MOCAP)
|| !(_sensorTimeout & SENSOR_LAND)
|| !(_sensorTimeout & SENSOR_LAND_TARGET)
) {
_estimatorInitialized |= EST_XY;
}
}
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_estimatorInitialized & EST_Z) {
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) {
_estimatorInitialized &= ~EST_Z;
}
} else {
if (z_stddev_ok) {
_estimatorInitialized |= EST_Z;
}
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_estimatorInitialized & EST_TZ) {
if (!tz_stddev_ok) {
_estimatorInitialized &= ~EST_TZ;
}
} else {
if (tz_stddev_ok) {
_estimatorInitialized |= EST_TZ;
}
}
// check timeouts
checkTimeouts();
// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get()) {
map_projection_init(&_map_ref,
_init_origin_lat.get(),
_init_origin_lon.get());
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
}
// reinitialize x if necessary
bool reinit_x = false;
for (int i = 0; i < n_x; i++) {
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i);
break;
}
}
if (reinit_x) {
for (int i = 0; i < n_x; i++) {
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
}
// force P symmetry and reinitialize P if necessary
bool reinit_P = false;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j <= i; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%d, %d) not finite", msg_label, i, j);
reinit_P = true;
}
if (i == j) {
// make sure diagonal elements are positive
if (_P(i, i) <= 0) {
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%d, %d) negative", msg_label, i, j);
reinit_P = true;
}
} else {
// copy elememnt from upper triangle to force
// symmetry
_P(j, i) = _P(i, j);
}
if (reinit_P) { break; }
}
if (reinit_P) { break; }
}
if (reinit_P) {
initP();
}
// do prediction
predict();
// sensor corrections/ initializations
if (_gpsUpdated) {
if (_sensorTimeout & SENSOR_GPS) {
gpsInit();
} else {
gpsCorrect();
}
}
if (_baroUpdated) {
if (_sensorTimeout & SENSOR_BARO) {
baroInit();
} else {
baroCorrect();
}
}
if (_lidarUpdated) {
if (_sensorTimeout & SENSOR_LIDAR) {
lidarInit();
} else {
lidarCorrect();
}
}
if (_sonarUpdated) {
if (_sensorTimeout & SENSOR_SONAR) {
sonarInit();
} else {
sonarCorrect();
}
}
if (_flowUpdated) {
if (_sensorTimeout & SENSOR_FLOW) {
flowInit();
} else {
flowCorrect();
}
}
if (_visionUpdated) {
if (_sensorTimeout & SENSOR_VISION) {
visionInit();
} else {
visionCorrect();
}
}
if (_mocapUpdated) {
if (_sensorTimeout & SENSOR_MOCAP) {
mocapInit();
} else {
mocapCorrect();
}
}
if (_landUpdated) {
if (_sensorTimeout & SENSOR_LAND) {
landInit();
} else {
landCorrect();
}
}
if (targetPositionUpdated) {
if (_sensorTimeout & SENSOR_LAND_TARGET) {
landingTargetInit();
} else {
landingTargetCorrect();
}
}
if (_altOriginInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
_pub_innov.get().timestamp = _timeStamp;
_pub_innov.update();
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) {
publishGlobalPos();
}
}
// propagate delayed state, no matter what
// if state is frozen, delayed state still
// needs to be propagated with frozen state
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
#ifdef STM32F40_41xxx
SetSysClock();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
ledInit();
#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
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
#if defined(USE_USART2) && defined(STM32F40_41xxx)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#if defined(USE_USART6) && defined(STM32F40_41xxx)
pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
.gpioPeripheral = BEEP_PERIPHERAL,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
spiInit(SPI3);
spiInit(SPI4);
spiInit(SPI5);
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
#if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(STM32F4DISCOVERY)
i2cInit(I2C_DEVICE_INT);
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
#ifdef I2C_DEVICE_EXT
i2cInit(I2C_DEVICE_EXT);
#endif
}
#endif
#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
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(3);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
cliInit(&masterConfig.serialConfig);
failsafeInit(&masterConfig.rxConfig);
rxInit(&masterConfig.rxConfig);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.batteryConfig);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) {
#ifdef COLIBRI
if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) {
ledStripEnable();
}
#else
ledStripEnable();
#endif
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#endif
#if defined(SPRACINGF3) || defined(CC3D) || defined(COLIBRI) || defined(REVO)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef BLACKBOX
//initBlackbox();
#endif
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
//gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#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
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
#include <stdio.h>
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
GPIO_InitTypeDef GPIO_InitStruct;
int main(void) {
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15 | GPIO_Pin_14 | GPIO_Pin_13
| GPIO_Pin_12;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOD, &GPIO_InitStruct);
printf("Hello World!\r\n");
hello();
while (1) {
static int count = 0;
static int i;
for (i = 0; i < 10000000; ++i)
;
GPIO_ToggleBits(GPIOD, GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15);
printf("%d\r\n", ++count);
}
//init();
/*
while (1) {
//loop();
int x = 1;//processLoopback();
}*/
}
void init(void)
{
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
// initialize IO (needed for all IO operations)
IOInitGlobal();
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
i2cSetOverclock(masterConfig.i2c_overclock);
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
// 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
#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(500);
timerInit(); // timer must be initialized before any channel is allocated
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
const sonarHcsr04Hardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType);
if (sonarHardware) {
pwm_params.useSonar = true;
pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag;
pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag;
}
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef USE_SERVOS
pwm_params.useServos = isServoOutputEnabled();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
#ifndef SKIP_RX_PWM_PPM
pwmRxInit(masterConfig.inputFilteringMode);
#endif
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmInit(&pwm_params);
mixerUsePWMIOConfiguration();
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.ioTag = IO_TAG(BEEPER),
#ifdef BEEPER_INVERTED
.isOD = false,
.isInverted = true
#else
.isOD = true,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.isOD = false;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#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(FURYF3) && defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#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 GPS
if (feature(FEATURE_GPS)) {
gpsPreInit(&masterConfig.gpsConfig);
}
#endif
// Set gyro sampling rate divider before initialization
gyroSetSampleRate(masterConfig.looptime, masterConfig.gyro_lpf, masterConfig.gyroSync, masterConfig.gyroSyncDenominator);
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.gyro_lpf,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
currentProfile->mag_declination)) {
// 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;
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
}
#endif
#ifdef NAV
navigationInit(
&masterConfig.navConfig,
¤tProfile->pidProfile,
¤tProfile->rcControlsConfig,
&masterConfig.rxConfig,
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig
);
#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 USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#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
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#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 CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
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
int main(void)
{
init();
/* Setup scheduler */
schedulerInit();
rescheduleTask(TASK_GYROPID, targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
while (true) {
scheduler();
processLoopback();
}
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
bool sensorsOK = false;
#ifdef SOFTSERIAL_LOOPBACK
serialPort_t* loopbackPort1 = NULL;
serialPort_t* loopbackPort2 = NULL;
#endif
initEEPROM();
checkFirstTime(false);
readEEPROM();
systemInit(mcfg.emf_avoidance);
#ifdef USE_LAME_PRINTF
init_printf(NULL, _putc);
#endif
activateConfig();
// configure power ADC
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel;
else {
adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0;
}
adcInit(&adc_params);
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit();
initBoardAlignment();
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = sensorsAutodetect();
// production debug output
#ifdef PROD_DEBUG
productionDebug();
#endif
// if gyro was not detected due to whatever reason, we give up now.
if (!sensorsOK)
failureMode(3);
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
imuInit(); // Mag is initialized inside imuInit
mixerInit(); // this will set core.useServo var depending on mixer type
serialInit(mcfg.serial_baudrate);
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
pwm_params.useServos = core.useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = mcfg.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = mcfg.midrc;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;
break;
case 9:
pwm_params.adcChannel = PWM8;
break;
default:
pwm_params.adcChannel = 0;
break;
}
pwmInit(&pwm_params);
core.numServos = pwm_params.numServos;
// configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
for (i = 0; i < RC_CHANS; i++)
rcData[i] = 1502;
rcReadRawFunc = pwmReadRawRC;
core.numRCChannels = MAX_INPUTS;
if (feature(FEATURE_SERIALRX)) {
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
spektrumInit(&rcReadRawFunc);
break;
case SERIALRX_SBUS:
sbusInit(&rcReadRawFunc);
break;
case SERIALRX_SUMD:
sumdInit(&rcReadRawFunc);
break;
case SERIALRX_MSP:
mspInit(&rcReadRawFunc);
break;
}
} else { // spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
// gpsInit will return if FEATURE_GPS is not enabled.
gpsInit(mcfg.gps_baudrate);
}
#ifdef SONAR
// sonar stuff only works with PPM
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_SONAR))
Sonar_init();
}
#endif
if (feature(FEATURE_SOFTSERIAL)) {
//mcfg.softserial_baudrate = 19200; // Uncomment to override config value
setupSoftSerialPrimary(mcfg.softserial_baudrate, mcfg.softserial_1_inverted);
setupSoftSerialSecondary(mcfg.softserial_2_inverted);
#ifdef SOFTSERIAL_LOOPBACK
loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]);
serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n");
loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]);
serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n");
#endif
//core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial.
}
if (feature(FEATURE_TELEMETRY))
initTelemetry();
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLE = 1;
// loopy
while (1) {
loop();
#ifdef SOFTSERIAL_LOOPBACK
if (loopbackPort1) {
while (serialTotalBytesWaiting(loopbackPort1)) {
uint8_t b = serialRead(loopbackPort1);
serialWrite(loopbackPort1, b);
//serialWrite(core.mainport, 0x01);
//serialWrite(core.mainport, b);
};
}
if (loopbackPort2) {
while (serialTotalBytesWaiting(loopbackPort2)) {
#ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data
serialRead(loopbackPort2);
#else
uint8_t b = serialRead(loopbackPort2);
serialWrite(loopbackPort2, b);
//serialWrite(core.mainport, 0x02);
//serialWrite(core.mainport, b);
#endif // OLIMEXINO
};
}
#endif
}
}
int main(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
serialPort_t* loopbackPort = NULL;
systemInit();
#ifdef USE_LAME_PRINTF
init_printf(NULL, _putc);
#endif
checkFirstTime(false);
readEEPROM();
// configure power ADC
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel;
else {
adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0;
}
adcInit(&adc_params);
initBoardAlignment();
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
mixerInit(); // this will set core.useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
//pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
pwm_params.useUART = feature(FEATURE_GPS);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
pwm_params.useServos = core.useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = mcfg.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = mcfg.midrc;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;
break;
case 9:
pwm_params.adcChannel = PWM8;
break;
default:
pwm_params.adcChannel = 0;
break;
}
pwmInit(&pwm_params);
// configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
for (i = 0; i < RC_CHANS; i++)
rcData[i] = 1502;
rcReadRawFunc = pwmReadRawRC;
core.numRCChannels = MAX_INPUTS;
if (feature(FEATURE_SERIALRX)) {
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
spektrumInit(&rcReadRawFunc);
break;
case SERIALRX_SBUS:
sbusInit(&rcReadRawFunc);
break;
case SERIALRX_SUMD:
sumdInit(&rcReadRawFunc);
break;
#if defined(SKYROVER)
case SERIALRX_HEXAIRBOT:
hexairbotInit(&rcReadRawFunc);
break;
#endif
}
} else { // spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
// gpsInit will return if FEATURE_GPS is not enabled.
// Sanity check below - protocols other than NMEA do not support baud rate autodetection
if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0)
mcfg.gps_baudrate = 0;
gpsInit(mcfg.gps_baudrate);
}
#ifdef SONAR
// sonar stuff only works with PPM
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_SONAR))
Sonar_init();
}
#endif
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
serialInit(mcfg.serial_baudrate);
DEBUG_PRINT("Booting..\r\n");
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsAutodetect();
imuInit(); // Mag is initialized inside imuInit
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit();
if (feature(FEATURE_SOFTSERIAL)) {
setupSoftSerial1(mcfg.softserial_baudrate, mcfg.softserial_inverted);
#ifdef SOFTSERIAL_LOOPBACK
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
serialPrint(loopbackPort, "LOOPBACK ENABLED\r\n");
#endif
}
if (feature(FEATURE_TELEMETRY))
initTelemetry();
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1;
DEBUG_PRINT("Start\r\n");
// loopy
while (1) {
loop();
#ifdef SOFTSERIAL_LOOPBACK
if (loopbackPort) {
while (serialTotalBytesWaiting(loopbackPort)) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
//serialWrite(core.mainport, b);
};
}
#endif
}
}
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
#ifdef STM32F40_41xxx
SetSysClock();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
ledInit();
#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
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
sonarGPIOConfig_t sonarGPIOConfig = {
.gpio = SONAR_GPIO,
.triggerPin = sonarHardware->echo_pin,
.echoPin = sonarHardware->trigger_pin,
};
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
#if defined(USE_USART2) && defined(STM32F40_41xxx)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#if defined(USE_USART6) && defined(STM32F40_41xxx)
pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
spiInit(SPI3);
#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
#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_INT);
#if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(SPARKY2)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
#ifdef I2C_DEVICE_EXT
i2cInit(I2C_DEVICE_EXT);
#endif
}
#endif
#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
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
// 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;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) {
#ifdef COLIBRI
if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) {
ledStripEnable();
}
#else
ledStripEnable();
#endif
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#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;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
while (1) {
loop();
processLoopback();
}
}
void HardFault_Handler(void)
{
// fall out of the sky
uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredState) == requiredState) {
stopMotors();
}
while (1);
}
void init(void)
{
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
i2cSetOverclock(masterConfig.i2c_highspeed);
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
ledInit();
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef BUTTONS
buttonsInit();
if (!isMPUSoftReset()) {
buttonsHandleColdBootButtonPresses();
}
#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();
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
sonarGPIOConfig_t sonarGPIOConfig = {
.gpio = SONAR_GPIO,
.triggerPin = sonarHardware->echo_pin,
.echoPin = sonarHardware->trigger_pin,
};
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2);
#endif
#if defined(USE_UART3)
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3);
#endif
#if defined(USE_UART4)
pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4);
#endif
#if defined(USE_UART5)
pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit(masterConfig.inputFilteringMode);
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params);
mixerUsePWMIOConfiguration(pwmIOConfiguration);
debug[2] = pwmIOConfiguration->pwmInputCount;
debug[3] = pwmIOConfiguration->ppmInputCount;
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_UART3);
}
#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(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
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
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf,
masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination,
masterConfig.looptime, masterConfig.gyroSync, masterConfig.gyroSyncDenominator)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
flashLedsAndBeep();
#ifdef USE_SERVOS
mixerInitialiseServoFiltering(targetLooptime);
#endif
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
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();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#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
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#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;
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
int main(void) {
init();
/* Setup scheduler */
if (masterConfig.gyroSync) {
rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME);
}
else {
rescheduleTask(TASK_GYROPID, targetLooptime);
}
setTaskEnabled(TASK_GYROPID, true);
setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
while (1) {
scheduler();
processLoopback();
}
}
void HardFault_Handler(void)
{
// 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
while (1);
}
int main(void)
{
// u16 len = 0;
// len = menuDisplay(bufExtBin);
// if (len)
// signUSBMass = 0;
signUSBMass = 0;
//***********************************DEBUG********************************************/
//***********************************END OF DEBUG********************************************/
/* Configure IO connected to USB PWR DET *********************/
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD,ENABLE );
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOD, &GPIO_InitStructure);
if((GPIO_ReadInputData(GPIOD) & 0x08)) // USB is present after switch - work MASS Storage
{
signUSBMass = 1;
}
signUSBMass = 0; //debugga
#if 1 //that's for temporary off USB - for debugging
NVIC_Configuration();
Set_System();
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
if(signUSBMass)
while (bDeviceState != CONFIGURED);
#endif
//
#if 1
if(!signUSBMass)
{
if (SysTick_Config(SystemCoreClock / 1000)) //1ms
{
/* Capture error */
while (1);
}
/* Flash unlock */
FLASH_Unlock();
/* Clear All pending flags */
FLASH_ClearFlag(FLASH_FLAG_BSY | FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
//***********************************DEBUG********************************************/
//***********************************END OF DEBUG********************************************/
/* Enable CRC clock */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
ledDioGPIOInit();
//LED_MIDDLE = 1;
//GPIO_ResetBits(LED_MID_PORT, LED_MID_PORT_PIN);
BKPInit();
//debugga
/*
u32 mileage = 0;
mileage = BKPReadReg(BKP_DR_MSG_NUMBER1) | ( (u32)(BKPReadReg(BKP_DR_MSG_NUMBER2) << 16) );
mileage++;
BKPWriteReg(BKP_DR_MSG_NUMBER1,mileage);BKPWriteReg(BKP_DR_MSG_NUMBER2,mileage >> 16);
mileage = BKPReadReg(BKP_DR_MSG_NUMBER1) | ( (u32)(BKPReadReg(BKP_DR_MSG_NUMBER2) << 16) );
mileage = 0;
BKPWriteReg(BKP_DR_MSG_NUMBER1,mileage);BKPWriteReg(BKP_DR_MSG_NUMBER2,mileage >> 16);
mileage = BKPReadReg(BKP_DR_MSG_NUMBER1) | ( (u32)(BKPReadReg(BKP_DR_MSG_NUMBER2) << 16) );
if(mileage)
signUSBMass = 0; //debugga
*/
Delay(1000);
//rs485Init();
//Delay(1000);
ais326dq_init();
ais326dq_data(&ais326dq_out);
adcInit();
gpsInit();
gprsModemInit();
gprsModemOn();
#ifndef BRIDGE_USB_GSM
setupGSM();
ftpGSMPrepare();
packetsIni();
#endif
rtc_init();
rtc_gettime(&rtc);
initSD();
/* reading settings */
readConfig();
/* Log */
saveSDInfo((u8 *)"TURN ON BLOCK ",strlen((u8 *)"TURN ON BLOCK "), SD_SENDED, SD_TYPE_MSG_LOG );
saveSDInfo((u8 *)readRTCTime(&rtc),strlen((const char *)readRTCTime(&rtc)), SD_SENDED, SD_TYPE_MSG_LOG );
} //if(!signUSBMass)
#endif
//u8 tempCnt;
while (1)
{
if(!signUSBMass)
{
gpsHandling();
#ifndef BRIDGE_USB_GSM
loopGSM();
loopFTP();
UpdatingFlash();
naviPacketHandle();
rcvPacketHandle();
rcvPacketHandleUSB();
#endif
buttonScan();
accelScan();
// if(getButtonUserPressed())
// {
// led_mid(BOARD_LED_XOR);
// ais326dq_data(&ais326dq_out);
// }
//rs485Analyse();
handleFOS();
adcScan();
} /* if(!signUSBMass) */
}
}
int main(int argc, char *argv[])
{
int done=0; double gpsUpdateRate, imuUpdateRate, eulerUpdateRate;
long int gpsUpdateCount, imuUpdateCount, eulerUpdateCount, kalmanUpdateCount;
double lastTime;
// Program begins with the usual logging of data and so on...
captureQuitSignal();
shared = (Q_SHARED * )attach_memory(QBOATShareKey, sizeof(Q_SHARED));
sharedDIAG = (DIAG_SHARED *)attach_memory(DIAGShareKey, sizeof(DIAG_SHARED));
createDirectory();
sleep(1);
openKalmanlogfile();
fprintf(stderr,"\nExtended Kalman Filter v 1.0 for the Q-boat (based on the Heli INS by Srik)");
initEverything();
lastTime = get_time();
// Now begin main loop...
while (!done) {
// Run a loop to check if we've received new data from the sensors...
if((get_time()-lastTime)>=1.0) {
fprintf(stderr,"\nGPS %dHz, IMU %d Hz, EUL %d Hz, KAL %dHz, LastTime %f",gpsUpdateCount, imuUpdateCount, eulerUpdateCount,kalmanUpdateCount,lastTime);
gpsUpdateCount = 0; imuUpdateCount = 0; eulerUpdateCount = 0; kalmanUpdateCount = 0; lastTime = get_time();
}
if(shared->SensorData.lastEulerKalmanTime<shared->SensorData.eulerUpdateTime) {
++eulerUpdateCount;
if(!init_compass)
compassInit();
else updateCompass();
}
#ifdef __USE3DMG_DATA__
if(shared->SensorData.lastImuKalmanTime<shared->SensorData.imuUpdateTime) {
if(!init_imu)
imuInit();
else updateImu();
++imuUpdateCount;
}
#endif
#ifdef __USEXBOW_DATA__
if(shared->SensorData.lastImuKalmanTime<sharedDIAG->xbowData.lastXbowTime) {
if(!init_imu)
imuInit();
else updateImu();
++imuUpdateCount;
}
#endif
if(shared->SensorData.lastGpsKalmanTime<shared->SensorData.gpsUpdateTime) {
if(!init_gps)
gpsInit();
else updateGPS();
++gpsUpdateCount;
}
if(!init_kf) {
if(init_gps && init_compass && init_imu)
kalmanInit();
}
// Update the State...
if(init_kf && (get_time() - shared->kalmanData.lastKalmanUpdateTime)>=0.01) {
updateState();
++kalmanUpdateCount;
}
usleep(50);
}
}
