void readEEPROM(void) { uint8_t i; // Sanity check if (!validEEPROM()) failureMode(10); // Read flash memcpy(&mcfg, (char *)FLASH_WRITE_ADDR, sizeof(master_t)); // Copy current profile if (mcfg.current_profile > 2) // sanity check mcfg.current_profile = 0; memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500; for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { int16_t tmp = 10 * i - cfg.thrMid8; uint8_t y = 1; if (tmp > 0) y = 100 - cfg.thrMid8; if (tmp < 0) y = cfg.thrMid8; lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } setPIDController(cfg.pidController); gpsSetPIDs(); }
void gpsInit(uint8_t baudrate) { portMode_t mode = MODE_RXTX; // init gpsData structure. if we're not actually enabled, don't bother doing anything else gpsSetState(GPS_UNKNOWN); if (!feature(FEATURE_GPS)) return; gpsData.baudrateIndex = baudrate; gpsData.lastMessage = millis(); gpsData.errors = 0; // only RX is needed for NMEA-style GPS if (mcfg.gps_type == GPS_NMEA) mode = MODE_RX; gpsSetPIDs(); core.gpsport = uartOpen(USART2, gpsNewData, gpsInitData[baudrate].baudrate, mode); // signal GPS "thread" to initialize when it gets to it gpsSetState(GPS_INITIALIZING); }
void activateConfig(void) { uint8_t i; for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t)cfg.rcRate8 / 2500; for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { int16_t tmp = 10 * i - cfg.thrMid8; uint8_t y = 1; if (tmp > 0) y = 100 - cfg.thrMid8; if (tmp < 0) y = cfg.thrMid8; lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t)cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; lookupThrottleRC[i] = mcfg.minthrottle + (int32_t)(mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } setPIDController(cfg.pidController); #ifdef GPS gpsSetPIDs(); #endif }