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
}
}
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();
}
}
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 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
}
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
}
}
int main(void) {
bt_init(BAUD_RATE);
Sonar_init(CONTINUOUS);
Servo_init(MANUAL, SCAN_AND_LOCK);
motorDriverInit();
while(1){
bt_getStr( tab ); // Get string from buffer
if(strlen( tab )){ // If isn't empty...
//bt_sendStr( tab ); // ...send it back.
if ( strcmp(tab, "w") == 0 ) {
driveForward(speed);
}
else if ( strcmp(tab, "s") == 0) {
driveReverse(speed);
}
else if ( strcmp(tab, " ") == 0) {
stop();
}
else if ( strcmp(tab, "a") == 0) {
driveReverseLeftTrack(40);
driveForwardRightTrack(40);
}
else if ( strcmp(tab, "d") == 0) {
driveForwardLeftTrack(40);
driveReverseRightTrack(40);
}
else if ( strcmp(tab, "ServoScanAndLock") == 0) {
ServoChangeMode(SWEEP);
ServoChangeSweepMode(SCAN_AND_LOCK);
}
else if ( strcmp(tab, "ServoScanAndGo") == 0) {
ServoChangeMode(SWEEP);
ServoChangeSweepMode(SCAN_AND_GO);
}
else if ( strcmp(tab, "ServoCenter") == 0) {
ServoChangeMode(MANUAL);
ServoMoveByDegree(0);
}
else if ( strncmp(tab, "SonarStartMeas",14) == 0) {
int new_deg = ParseIntNumber(tab,14,3);
SonarStartMeas(new_deg);
}
else if ( strncmp(tab, "SonarGetDistance",16) == 0) {
char buffor[12];
int new_deg = ParseIntNumber(tab,16,3);
sprintf(buffor, "%04d,%04hu\n",new_deg,SonarGetDistance(new_deg));
bt_sendStr(buffor);
}
else if ( strcmp(tab, "e") == 0) {
if (speed<=90){
speed+=10;
}
}
else if ( strcmp(tab, "q") == 0) {
if (speed>=10){
speed-=10;
}
}
else if (strncmp(tab, "speed",5) == 0){
int new_speed = ParseIntNumber(tab,5,3);
if (new_speed >= 0 && new_speed <= 100){
speed = new_speed;
}
}
else if ( strncmp(tab, "SonarLockRange",14) == 0) {
int new_range = ParseIntNumber(tab,14,3);
ServoChangeLockRange(new_range);
}
}
}
};
