bool readEEPROM() {
uint8_t i;
#ifdef MULTIPLE_CONFIGURATION_PROFILES
if(global_conf.currentSet>2) global_conf.currentSet=0;
#else
global_conf.currentSet=0;
#endif
eeprom_read_block((void*)&conf, (void*)(global_conf.currentSet * sizeof(conf) + sizeof(global_conf)), sizeof(conf));
if(calculate_sum((uint8_t*)&conf, sizeof(conf)) != conf.checksum) {
blinkLED(6,100,3);
#if defined(BUZZER)
alarmArray[7] = 3;
#endif
LoadDefaults(); // force load defaults
return false; // defaults loaded, don't reload constants (EEPROM life saving)
}
// 500/128 = 3.90625 3.9062 * 3.9062 = 15.259 1526*100/128 = 1192
for(i=0;i<5;i++) {
lookupPitchRollRC[i] = (1526+conf.rcExpo8*(i*i-15))*i*(int32_t)conf.rcRate8/1192;
}
for(i=0;i<11;i++) {
int16_t tmp = 10*i-conf.thrMid8;
uint8_t y = 1;
if (tmp>0) y = 100-conf.thrMid8;
if (tmp<0) y = conf.thrMid8;
lookupThrottleRC[i] = 10*conf.thrMid8 + tmp*( 100-conf.thrExpo8+(int32_t)conf.thrExpo8*(tmp*tmp)/(y*y) )/10; // [0;1000]
lookupThrottleRC[i] = conf.minthrottle + (int32_t)(MAXTHROTTLE-conf.minthrottle)* lookupThrottleRC[i]/1000; // [0;1000] -> [conf.minthrottle;MAXTHROTTLE]
}
#if defined(POWERMETER)
pAlarm = (uint32_t) conf.powerTrigger1 * (uint32_t) PLEVELSCALE * (uint32_t) PLEVELDIV; // need to cast before multiplying
#endif
#if GPS
GPS_set_pids(); // at this time we don't have info about GPS init done
#endif
#if defined(ARMEDTIMEWARNING)
ArmedTimeWarningMicroSeconds = (conf.armedtimewarning *1000000);
#endif
return true; // setting is OK
}
void gpsInit(uint32_t baudrate)
{
int i;
int offset = 0;
gps.bytes_rx = 0;
gps.frames_rx = 0;
// Create semaphore for OSD frame drawing syncronization
vSemaphoreCreateBinary(gpsSemaphore);
// Create GPS sensor lost timer at 3 seconds
gpsLostTimer = xTimerCreate((signed char *) "TimGPSlost", 3000, pdFALSE, (void *) NULL, gpsLostTimerCallback);
// Start GPS lost timer
xTimerStart(gpsLostTimer, 10);
GPS_set_pids();
if (cfg.gps_baudrate == 0 || cfg.hil_mode != 0)
return; // GPS not configured or HIL mode
uartInit(GPS_UART, baudrate, GPS_NewData);
if (cfg.gps_type == GPS_UBLOX)
offset = 0;
else if (cfg.gps_type == GPS_MTK)
offset = 4;
if (cfg.gps_type != GPS_NMEA) {
for (i = 0; i < 5; i++) {
uartChangeBaud(GPS_UART, init_speed[i]);
switch (baudrate) {
case 19200:
gpsPrint(gpsInitStrings[offset]);
break;
case 38400:
gpsPrint(gpsInitStrings[offset + 1]);
break;
case 57600:
gpsPrint(gpsInitStrings[offset + 2]);
break;
case 115200:
gpsPrint(gpsInitStrings[offset + 3]);
break;
}
vTaskDelay(10 / portTICK_RATE_MS); // Delay on 10 ms
}
}
uartChangeBaud(GPS_UART, baudrate);
if (cfg.gps_type == GPS_UBLOX) {
for (i = 0; i < sizeof(ubloxInit); i++) {
uartWrite(GPS_UART, ubloxInit[i]); // send ubx init binary
vTaskDelay(4 / portTICK_RATE_MS); // Delay on 4 ms
}
} else if (cfg.gps_type == GPS_MTK) {
gpsPrint("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); // only GGA and RMC sentence
gpsPrint("$PMTK220,200*2C\r\n"); // 5 Hz update rate
}
}
void gpsInit(uint32_t baudrate)
{
GPS_set_pids();
uart2Init(baudrate, GPS_NewData);
sensorsSet(SENSOR_GPS);
}
void setup() {
#if !defined(GPS_PROMINI)
UARTInit(115200);
#endif
LEDPIN_PINMODE;
SHIELDLED_PINMODE;
//POWERPIN_PINMODE;
//BUZZERPIN_PINMODE;
//STABLEPIN_PINMODE;
//POWERPIN_OFF;
/* Initialize GPIO (sets up clock) */
GPIOInit();
init_microsec();
enable_microsec();
init_timer16PWM();
enable_PWMtimer();
/******** special version of MultiWii to calibrate all attached ESCs ************/
#if defined(ESC_CALIB_CANNOT_FLY)
writeAllMotors(ESC_CALIB_HIGH);
delayMs(3000);
writeAllMotors(ESC_CALIB_LOW);
delayMs(500);
while (1) {
delayMs(5000);
blinkLED(2,20, 2);
}
// statement never reached
#endif
writeAllMotors(MINCOMMAND);
delayMs(300);
readEEPROM();
checkFirstTime();
configureReceiver();
#if defined(OPENLRSv2MULTI)
initOpenLRS();
#endif
initSensors();
#if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)||defined(I2C_NAV)
GPS_set_pids();
#endif
previousTime = micros();
#if defined(GIMBAL)
calibratingA = 400;
#endif
calibratingG = 400;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#if defined(POWERMETER)
for(uint8_t i=0;i<=PMOTOR_SUM;i++)
pMeter[i]=0;
#endif
#if defined(ARMEDTIMEWARNING)
ArmedTimeWarningMicroSeconds = (ARMEDTIMEWARNING *1000000);
#endif
/************************************/
#if defined(GPS_SERIAL)
SerialOpen(GPS_SERIAL,GPS_BAUD);
delay(400);
for(uint8_t i=0;i<=5;i++){
GPS_NewData();
LEDPIN_ON
delay(20);
LEDPIN_OFF
delay(80);
}
if(!GPS_Present){
SerialEnd(GPS_SERIAL);
SerialOpen(0,SERIAL_COM_SPEED);
}
#if !defined(GPS_PROMINI)
GPS_Present = 1;
#endif
GPS_Enable = GPS_Present;
#endif
/************************************/
#if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD)|| defined(I2C_NAV)
GPS_Enable = 1;
#endif
#if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64)
initLCD();
#endif
#ifdef LCD_TELEMETRY_DEBUG
telemetry_auto = 1;
#endif
#ifdef LCD_CONF_DEBUG
configurationLoop();
#endif
#ifdef LANDING_LIGHTS_DDR
init_landing_lights();
#endif
#if defined(LED_FLASHER)
init_led_flasher();
led_flasher_set_sequence(LED_FLASHER_SEQUENCE);
#endif
f.SMALL_ANGLES_25=1; // important for gyro only conf
//initialise median filter structures
#ifdef MEDFILTER
#ifdef SONAR
initMedianFilter(&SonarFilter, 5);
#endif
#endif
initWatchDog();
}
