int main(int argc, char ** argv){
	char c ;
	FILE * fr;
	long start_time, end_time ;
	double diff_time ;
	struct timespec cpu_time ;
	unsigned int size = 0 ;	
	initGPIOs();
	#ifdef MARK1
	init_i2c(3);
	#endif
	fr = fopen (argv[1], "rb");  /* open the file for reading bytes*/
	if(fr < 0){
		printf("cannot open file %s \n", argv[1]);	
	}
	size = fread(configBits, 1, 1024*1024, fr);
	printf("bit file size : %d \n", size);
	//clock_gettime(CLOCK_REALTIME, &cpu_time);
	//start_time = cpu_time.tv_sec ;
	if(serialConfig(configBits, size) < 0){
		printf("config error \n");
		exit(0);	
	}else{
		printf("config success ! \n");	
	}
	//clock_gettime(CLOCK_REALTIME, &cpu_time);
	//end_time = cpu_time.tv_sec ;
	//diff_time = end_time - start_time ;
	//diff_time = diff_time/1000000 ;
	//printf("Configuration took %fs \n", diff_time);
	closeGPIOs();
	fclose(fr);
	return 1;
}
Exemple #2
0
serialPortConfig_t *findNextSerialPortConfig(uint16_t mask)
{
    while (findSerialPortConfigState.lastIndex < SERIAL_PORT_COUNT) {
        serialPortConfig_t *candidate = &serialConfig()->portConfigs[findSerialPortConfigState.lastIndex++];

        if (candidate->functionMask & mask) {
            return candidate;
        }
    }
    return NULL;
}
Exemple #3
0
serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier)
{
    uint8_t index;
    for (index = 0; index < SERIAL_PORT_COUNT; index++) {
        serialPortConfig_t *candidate = &serialConfig()->portConfigs[index];
        if (candidate->identifier == identifier) {
            return candidate;
        }
    }
    return NULL;
}
Exemple #4
0
void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar)
{
#ifndef USE_CLI
    UNUSED(serialPort);
#else
    if (receivedChar == '#') {
        cliEnter(serialPort);
    }
#endif
    if (receivedChar == serialConfig()->reboot_character) {
        systemResetToBootloader();
    }
}
Exemple #5
0
static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar)
{
#ifdef USE_CLI
    if (receivedChar == '#') {
        mspPort->pendingRequest = MSP_PENDING_CLI;
        return;
    }
#endif

    if (receivedChar == serialConfig()->reboot_character) {
        mspPort->pendingRequest = MSP_PENDING_BOOTLOADER;
        return;
    }
}
Exemple #6
0
void gpsInit(void)
{
    gpsState.serialConfig = serialConfig();
    gpsState.gpsConfig = gpsConfig();
    gpsState.baudrateIndex = 0;

    gpsStats.errors = 0;
    gpsStats.timeouts = 0;

    gpsResetSolution();

    // init gpsData structure. if we're not actually enabled, don't bother doing anything else
    gpsState.autoConfigStep = 0;
    gpsState.lastMessageMs = millis();
    gpsSetState(GPS_UNKNOWN);

    if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_BUS) {
        gpsSetState(GPS_INITIALIZING);
        return;
    }

    if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) {
        serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
        if (!gpsPortConfig) {
            featureClear(FEATURE_GPS);
        }
        else {
            while (gpsToSerialBaudRate[gpsState.baudrateIndex] != gpsPortConfig->gps_baudrateIndex) {
                gpsState.baudrateIndex++;
                if (gpsState.baudrateIndex >= GPS_BAUDRATE_COUNT) {
                    gpsState.baudrateIndex = 0;
                    break;
                }
            }

            portMode_t mode = gpsProviders[gpsState.gpsConfig->provider].portMode;

            // no callback - buffer will be consumed in gpsThread()
            gpsState.gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]], mode, SERIAL_NOT_INVERTED);

            if (!gpsState.gpsPort) {
                featureClear(FEATURE_GPS);
            }
            else {
                gpsSetState(GPS_INITIALIZING);
                return;
            }
        }
    }
}
Exemple #7
0
serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction)
{
    while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) {
        serialPortConfig_t *candidate = &serialConfig()->portConfigs[findSharedSerialPortState.lastIndex++];

        if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) {
            serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier);
            if (!serialPortUsage) {
                continue;
            }
            return serialPortUsage->serialPort;
        }
    }
    return NULL;
}
Exemple #8
0
void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar)
{
#ifndef USE_CLI
    UNUSED(serialPort);
#else
    if (receivedChar == '#') {
        cliEnter(serialPort);
    }
#endif
    if (receivedChar == serialConfig()->reboot_character) {
        // A 100ms guard delay to make sure reboot_character is followed by silence
        // If anything is received during the guard period - reboot_character is ignored
        for (int i = 0; i < 10; i++) {
            delay(10);
            if (serialRxBytesWaiting(serialPort)) {
                return;
            }
        }

        systemResetToBootloader();
    }
}
int mspServerCommandHandler(mspPacket_t *cmd, mspPacket_t *reply)
{
    sbuf_t *src = &cmd->buf;
    sbuf_t *dst = &reply->buf;
    int len = sbufBytesRemaining(src);

    switch (cmd->cmd) {
        case MSP_API_VERSION:
            sbufWriteU8(dst, MSP_PROTOCOL_VERSION);

            sbufWriteU8(dst, API_VERSION_MAJOR);
            sbufWriteU8(dst, API_VERSION_MINOR);
            break;

        case MSP_FC_VARIANT:
            sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
            break;

        case MSP_FC_VERSION:
            sbufWriteU8(dst, FC_VERSION_MAJOR);
            sbufWriteU8(dst, FC_VERSION_MINOR);
            sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL);
            break;

        case MSP_BOARD_INFO:
            sbufWriteData(dst, boardIdentifier, BOARD_IDENTIFIER_LENGTH);
            sbufWriteU16(dst, 0);  // hardware revision
            sbufWriteU8(dst, 1);  // 0 == FC, 1 == OSD
            break;

        case MSP_BUILD_INFO:
            sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
            sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
            sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
            break;

            // DEPRECATED - Use MSP_API_VERSION
        case MSP_IDENT:
            sbufWriteU8(dst, MW_VERSION);
            sbufWriteU8(dst, 0); // mixer mode
            sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
            sbufWriteU32(dst, CAP_DYNBALANCE); // "capability"
            break;

        case MSP_STATUS_EX:
        case MSP_STATUS:
            sbufWriteU16(dst, cycleTime);
#ifdef USE_I2C
            sbufWriteU16(dst, i2cGetErrorCounter());
#else
            sbufWriteU16(dst, 0);
#endif
            sbufWriteU16(dst, 0); // sensors
            sbufWriteU32(dst, 0); // flight mode flags
            sbufWriteU8(dst, 0);  // profile index
            if(cmd->cmd == MSP_STATUS_EX) {
                sbufWriteU16(dst, averageSystemLoadPercent);
            }
            break;

        case MSP_DEBUG:
            // output some useful QA statistics
            // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]

            for (int i = 0; i < DEBUG16_VALUE_COUNT; i++)
                sbufWriteU16(dst, debug[i]);      // 4 variables are here for general monitoring purpose
            break;

        case MSP_UID:
            sbufWriteU32(dst, U_ID_0);
            sbufWriteU32(dst, U_ID_1);
            sbufWriteU32(dst, U_ID_2);
            break;

        case MSP_VOLTAGE_METER_CONFIG:
            for (int i = 0; i < MAX_VOLTAGE_METERS; i++) {
                // FIXME update for multiple voltage sources  i.e.  use `i` and support at least OSD VBAT, OSD 12V, OSD 5V
                sbufWriteU8(dst, batteryConfig()->vbatscale);
                sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
                sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
                sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
            }
            break;

        case MSP_CURRENT_METER_CONFIG:
            sbufWriteU16(dst, batteryConfig()->currentMeterScale);
            sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
            sbufWriteU8(dst, batteryConfig()->currentMeterType);
            sbufWriteU16(dst, batteryConfig()->batteryCapacity);
            break;

        case MSP_CF_SERIAL_CONFIG:
            for (int i = 0; i < serialGetAvailablePortCount(); i++) {
                if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
                    continue;
                };
                sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
                sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
                sbufWriteU8(dst, serialConfig()->portConfigs[i].baudRates[BAUDRATE_MSP_SERVER]);
                sbufWriteU8(dst, serialConfig()->portConfigs[i].baudRates[BAUDRATE_MSP_CLIENT]);
                sbufWriteU8(dst, serialConfig()->portConfigs[i].baudRates[BAUDRATE_RESERVED1]);
                sbufWriteU8(dst, serialConfig()->portConfigs[i].baudRates[BAUDRATE_RESERVED2]);
            }
            break;

        case MSP_BF_BUILD_INFO:
            sbufWriteData(dst, buildDate, 11); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
            sbufWriteU32(dst, 0); // future exp
            sbufWriteU32(dst, 0); // future exp
            break;

        case MSP_DATAFLASH_SUMMARY: // FIXME update GUI and remove this.
            sbufWriteU8(dst, 0); // FlashFS is neither ready nor supported
            sbufWriteU32(dst, 0);
            sbufWriteU32(dst, 0);
            sbufWriteU32(dst, 0);
            break;

        case MSP_BATTERY_STATES:
            // write out battery states, once for each battery
            sbufWriteU8(dst, (uint8_t)getBatteryState() == BATTERY_NOT_PRESENT ? 0 : 1); // battery connected - 0 not connected, 1 connected
            sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
            sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
            break;

        case MSP_CURRENT_METERS:
            // write out amperage, once for each current meter.
            sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
            break;

        case MSP_VOLTAGE_METERS:
            // write out voltage, once for each meter.
            for (int i = 0; i < 3; i++) {
                // FIXME hack that needs cleanup, see issue #2221
                // This works for now, but the vbat scale also changes the 12V and 5V readings.
                switch(i) {
                    case 0:
                        sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
                        break;
                    case 1:
                        sbufWriteU8(dst, (uint8_t)constrain(batteryAdcToVoltage(adcGetChannel(ADC_12V)), 0, 255));
                        break;
                    case 2:
                        sbufWriteU8(dst, (uint8_t)constrain(batteryAdcToVoltage(adcGetChannel(ADC_5V)), 0, 255));
                        break;
                }
            }
            break;
        case MSP_OSD_VIDEO_CONFIG:
            sbufWriteU8(dst, osdVideoConfig()->videoMode); // 0 = NTSC, 1 = PAL
            break;

        case MSP_RESET_CONF:
            resetEEPROM();
            readEEPROM();
            break;

        case MSP_EEPROM_WRITE:
            writeEEPROM();
            readEEPROM();
            break;

        case MSP_SET_VOLTAGE_METER_CONFIG: {
            uint8_t i = sbufReadU8(src);
            if (i >= MAX_VOLTAGE_METERS) {
                return -1;
            }
            // FIXME use `i`, see MSP_VOLTAGE_METER_CONFIG
            batteryConfig()->vbatscale = sbufReadU8(src);               // actual vbatscale as intended
            batteryConfig()->vbatmincellvoltage = sbufReadU8(src);      // vbatlevel_warn1 in MWC2.3 GUI
            batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src);      // vbatlevel_warn2 in MWC2.3 GUI
            batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
            break;
        }

        case MSP_SET_CURRENT_METER_CONFIG:
            batteryConfig()->currentMeterScale = sbufReadU16(src);
            batteryConfig()->currentMeterOffset = sbufReadU16(src);
            batteryConfig()->currentMeterType = sbufReadU8(src);
            batteryConfig()->batteryCapacity = sbufReadU16(src);
            break;

        case MSP_SET_CF_SERIAL_CONFIG: {
            int portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);

            if (len % portConfigSize != 0)
                return -1;

            while (sbufBytesRemaining(src) >= portConfigSize) {
                uint8_t identifier = sbufReadU8(src);

                serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier);
                if (!portConfig)
                    return -1;

                portConfig->identifier = identifier;
                portConfig->functionMask = sbufReadU16(src);
                portConfig->baudRates[BAUDRATE_MSP_SERVER] = sbufReadU8(src);
                portConfig->baudRates[BAUDRATE_MSP_CLIENT] = sbufReadU8(src);
                portConfig->baudRates[BAUDRATE_RESERVED1] = sbufReadU8(src);
                portConfig->baudRates[BAUDRATE_RESERVED2] = sbufReadU8(src);
            }
            break;
        }

        case MSP_REBOOT:
            mspPostProcessFn = mspRebootFn;
            break;

        case MSP_SET_OSD_VIDEO_CONFIG:
            osdVideoConfig()->videoMode = sbufReadU8(src);
            mspPostProcessFn = mspApplyVideoConfigurationFn;
            break;

        default:
            // we do not know how to handle the message
            return 0;
    }
    return 1;     // message was handled successfully
}
Exemple #10
0
int main(int argc, char ** argv) {
    char c ;
    unsigned int i ;
    FILE * fr;
    long start_time, end_time ;
    double diff_time ;
    struct timespec cpu_time ;
    unsigned int size = 0 ;
    initGPIOs();
    init_i2c(1);

    //parse programm args
    for(i = 1 ; i < argc ; ) {
        if(argv[i][0] == '-') {
            switch(argv[i][1]) {
            case '\0':
                i ++ ;
                break ;
            case 'r' :
                resetFPGA();
                closeGPIOs();
                close(i2c_fd);
                return 1 ;
                break ;
            case 'h' :
                printHelp();
                return 1 ;
                break;
            default :
                printHelp();
                return 1 ;
                break ;
            }
        } else {
            //last argument is file to load
            break ;
        }
    }


    if(init_spi() < 0) {
        printf("cannot open spi bus \n");
        return -1 ;
    }
    fr = fopen (argv[i], "rb");  /* open the file for reading bytes*/
    if(fr < 0) {
        printf("cannot open file %s \n", argv[1]);
        return -1 ;
    }
    memset((void *) configBits, (int) 0, (size_t) 1024*1024);
    size = fread(configBits, 1, 1024*1024, fr);
    printf("bit file size : %d \n", size);

    //8*5 clock cycle more at the end of config
    if(serialConfig(configBits, size+5) < 0) {
        printf("config error \n");
        exit(0);
    } else {
        printf("config success ! \n");
    }

    closeGPIOs();
    fclose(fr);
    close(spi_fd);
    close(i2c_fd);
    return 1;
}
Exemple #11
0
// Default settings
STATIC_UNIT_TESTED void resetConf(void)
{
    pgResetAll(MAX_PROFILE_COUNT);

    setProfile(0);
    pgActivateProfile(0);

    setControlRateProfile(0);

    parseRcChannels("AETR1234", rxConfig());

    featureClearAll();

    featureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_BLACKBOX);
#ifdef DEFAULT_FEATURES
    featureSet(DEFAULT_FEATURES);
#endif

#ifdef BOARD_HAS_VOLTAGE_DIVIDER
    // only enable the feature by default if the board has supporting hardware
    featureSet(FEATURE_VBAT);
#endif

#ifdef BOARD_HAS_AMPERAGE_METER
    // only enable the feature by default if the board has supporting hardware
    featureSet(FEATURE_AMPERAGE_METER);
    batteryConfig()->amperageMeterSource = AMPERAGE_METER_ADC;
#endif

    // alternative defaults settings for ALIENFLIGHTF1 and ALIENFLIGHTF3 targets
#ifdef ALIENFLIGHT
#ifdef ALIENFLIGHTF3
    serialConfig()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
    getVoltageMeterConfig(ADC_BATTERY)->vbatscale = 20;
    sensorSelectionConfig()->mag_hardware = MAG_NONE;            // disabled by default
# else
    serialConfig()->portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
# endif
    rxConfig()->serialrx_provider = SERIALRX_SPEKTRUM2048;
    rxConfig()->spektrum_sat_bind = 5;
    motorConfig()->minthrottle = 1000;
    motorConfig()->maxthrottle = 2000;
    motorConfig()->motor_pwm_rate = 32000;
    failsafeConfig()->failsafe_delay = 2;
    failsafeConfig()->failsafe_off_delay = 0;
    mixerConfig()->yaw_jump_prevention_limit = YAW_JUMP_PREVENTION_LIMIT_HIGH;
    currentControlRateProfile->rcRate8 = 100;
    currentControlRateProfile->rates[PITCH] = 20;
    currentControlRateProfile->rates[ROLL] = 20;
    currentControlRateProfile->rates[YAW] = 20;
    parseRcChannels("TAER1234", rxConfig());

    *customMotorMixer(0) = (motorMixer_t){ 1.0f, -0.414178f,  1.0f, -1.0f };    // REAR_R
    *customMotorMixer(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f,  1.0f };    // FRONT_R
    *customMotorMixer(2) = (motorMixer_t){ 1.0f,  0.414178f,  1.0f,  1.0f };    // REAR_L
    *customMotorMixer(3) = (motorMixer_t){ 1.0f,  0.414178f, -1.0f, -1.0f };    // FRONT_L
    *customMotorMixer(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f };    // MIDFRONT_R
    *customMotorMixer(5) = (motorMixer_t){ 1.0f,  1.0f, -0.414178f,  1.0f };    // MIDFRONT_L
    *customMotorMixer(6) = (motorMixer_t){ 1.0f, -1.0f,  0.414178f,  1.0f };    // MIDREAR_R
    *customMotorMixer(7) = (motorMixer_t){ 1.0f,  1.0f,  0.414178f, -1.0f };    // MIDREAR_L
#endif

    // copy first profile into remaining profile
    PG_FOREACH_PROFILE(reg) {
        for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
            memcpy(reg->address + i * pgSize(reg), reg->address, pgSize(reg));
        }
    }
    for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
        configureRateProfileSelection(i, i % MAX_CONTROL_RATE_PROFILE_COUNT);
    }
}
Exemple #12
0
static void validateAndFixConfig(void)
{
    if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
        featureSet(DEFAULT_RX_FEATURE);
    }

    if (featureConfigured(FEATURE_RX_PPM)) {
        featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_SERIAL | FEATURE_RX_MSP);
    }

    if (featureConfigured(FEATURE_RX_MSP)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM);
    }

    if (featureConfigured(FEATURE_RX_SERIAL)) {
        featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM);
    }

    if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM);
    }

    // The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
    // The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
    if (armingConfig()->retarded_arm && mixerConfig()->pid_at_min_throttle) {
        mixerConfig()->pid_at_min_throttle = 0;
    }

    if (gyroConfig()->gyro_soft_notch_hz < gyroConfig()->gyro_soft_notch_cutoff_hz) {
        gyroConfig()->gyro_soft_notch_hz = gyroConfig()->gyro_soft_notch_cutoff_hz;
    }

#if defined(LED_STRIP)
#if (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
    if (featureConfigured(FEATURE_SOFTSERIAL) && (
            0
#ifdef USE_SOFTSERIAL1
            || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
#endif
#ifdef USE_SOFTSERIAL2
            || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
#endif
    )) {
        // led strip needs the same timer as softserial
        featureClear(FEATURE_LED_STRIP);
    }
#endif

#if defined(TRANSPONDER) && !defined(UNIT_TEST)
    if ((WS2811_DMA_TC_FLAG == TRANSPONDER_DMA_TC_FLAG) && featureConfigured(FEATURE_TRANSPONDER) && featureConfigured(FEATURE_LED_STRIP)) {
        featureClear(FEATURE_LED_STRIP);
    }
#endif
#endif // LED_STRIP

#if defined(CC3D)
#if defined(DISPLAY) && defined(USE_UART3)
    if (featureConfigured(FEATURE_DISPLAY) && doesConfigurationUsePort(SERIAL_PORT_UART3)) {
        featureClear(FEATURE_DISPLAY);
    }
#endif

#if defined(SONAR) && defined(USE_SOFTSERIAL1)
    if (featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_SOFTSERIAL)) {
        featureClear(FEATURE_SONAR);
    }
#endif

#if defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
    // shared pin
    if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
        featureClear(FEATURE_SONAR);
        featureClear(FEATURE_SOFTSERIAL);
        featureClear(FEATURE_RSSI_ADC);
    }
#endif
#endif // CC3D

#if defined(COLIBRI_RACE)
    serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP_SERVER;
    if (featureConfigured(FEATURE_RX_SERIAL)) {
        serialConfig()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
    }
#endif

#if defined(SPRACINGF3NEO_REV) && (SPRACINGF3NEO_REV < 5)
    if (featureConfigured(FEATURE_OSD) && featureConfigured(FEATURE_TRANSPONDER)) {
        featureClear(FEATURE_TRANSPONDER);
    }

    if (featureConfigured(FEATURE_OSD) && featureConfigured(FEATURE_LED_STRIP)) {
        featureClear(FEATURE_LED_STRIP);
    }
#endif

    if (!isSerialConfigValid(serialConfig())) {
        PG_RESET_CURRENT(serialConfig);
    }

#if defined(USE_VCP)
    serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP_SERVER;
#endif
}
Exemple #13
0
// Default settings
STATIC_UNIT_TESTED void resetConf(void)
{
    pgResetAll(MAX_PROFILE_COUNT);

    setProfile(0);
    pgActivateProfile(0);

    setControlRateProfile(0);

    featureClearAll();

    featureSet(DEFAULT_RX_FEATURE);

#ifdef BOARD_HAS_VOLTAGE_DIVIDER
    // only enable the VBAT feature by default if the board has a voltage divider otherwise
    // the user may see incorrect readings and unexpected issues with pin mappings may occur.
    featureSet(FEATURE_VBAT);
#endif

    featureSet(FEATURE_FAILSAFE);

    parseRcChannels("AETR1234", rxConfig());

    featureSet(FEATURE_BLACKBOX);

#if defined(COLIBRI_RACE)
    // alternative defaults settings for COLIBRI RACE targets
    imuConfig()->looptime = 1000;
    featureSet(FEATURE_ONESHOT125);
    featureSet(FEATURE_LED_STRIP);
#endif

#ifdef SPRACINGF3EVO
    featureSet(FEATURE_TRANSPONDER);
    featureSet(FEATURE_RSSI_ADC);
    featureSet(FEATURE_CURRENT_METER);
    featureSet(FEATURE_TELEMETRY);
#endif

    // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets
#ifdef ALIENWII32
    featureSet(FEATURE_RX_SERIAL);
    featureSet(FEATURE_MOTOR_STOP);
# ifdef ALIENWIIF3
    serialConfig()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
    batteryConfig()->vbatscale = 20;
# else
    serialConfig()->portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
# endif
    rxConfig()->serialrx_provider = SERIALRX_SPEKTRUM2048;
    rxConfig()->spektrum_sat_bind = 5;
    motorAndServoConfig()->minthrottle = 1000;
    motorAndServoConfig()->maxthrottle = 2000;
    motorAndServoConfig()->motor_pwm_rate = 32000;
    imuConfig()->looptime = 2000;
    pidProfile()->pidController = 3;
    pidProfile()->P8[PIDROLL] = 36;
    pidProfile()->P8[PIDPITCH] = 36;
    failsafeConfig()->failsafe_delay = 2;
    failsafeConfig()->failsafe_off_delay = 0;
    currentControlRateProfile->rcRate8 = 130;
    currentControlRateProfile->rates[ROLL] = 20;
    currentControlRateProfile->rates[PITCH] = 20;
    currentControlRateProfile->rates[YAW] = 100;
    parseRcChannels("TAER1234", rxConfig());
    *customMotorMixer(0) = (motorMixer_t){ 1.0f, -0.414178f,  1.0f, -1.0f };    // REAR_R
    *customMotorMixer(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f,  1.0f };    // FRONT_R
    *customMotorMixer(2) = (motorMixer_t){ 1.0f,  0.414178f,  1.0f,  1.0f };    // REAR_L
    *customMotorMixer(3) = (motorMixer_t){ 1.0f,  0.414178f, -1.0f, -1.0f };    // FRONT_L
    *customMotorMixer(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f };    // MIDFRONT_R
    *customMotorMixer(5) = (motorMixer_t){ 1.0f,  1.0f, -0.414178f,  1.0f };    // MIDFRONT_L
    *customMotorMixer(6) = (motorMixer_t){ 1.0f, -1.0f,  0.414178f,  1.0f };    // MIDREAR_R
    *customMotorMixer(7) = (motorMixer_t){ 1.0f,  1.0f,  0.414178f, -1.0f };    // MIDREAR_L
#endif

    // copy first profile into remaining profile
    PG_FOREACH_PROFILE(reg) {
        for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
            memcpy(reg->address + i * pgSize(reg), reg->address, pgSize(reg));
        }
    }
    for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
        configureRateProfileSelection(i, i % MAX_CONTROL_RATE_PROFILE_COUNT);
    }
}
Exemple #14
0
void validateAndFixConfig(void)
{
    if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
        featureSet(DEFAULT_RX_FEATURE);
    }

    if (featureConfigured(FEATURE_RX_PPM)) {
        featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_SERIAL | FEATURE_RX_MSP);
    }

    if (featureConfigured(FEATURE_RX_MSP)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM);
    }

    if (featureConfigured(FEATURE_RX_SERIAL)) {
        featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM);
    }

    if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM);
    }

#ifdef STM32F10X
    // avoid overloading the CPU on F1 targets when using gyro sync and GPS.
    if (imuConfig()->gyroSync && imuConfig()->gyroSyncDenominator < 2 && featureConfigured(FEATURE_GPS)) {
        imuConfig()->gyroSyncDenominator = 2;
    }
#endif

#if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
    if (featureConfigured(FEATURE_SOFTSERIAL) && (
            0
#ifdef USE_SOFTSERIAL1
            || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
#endif
#ifdef USE_SOFTSERIAL2
            || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
#endif
    )) {
        // led strip needs the same timer as softserial
        featureClear(FEATURE_LED_STRIP);
    }
#endif

#if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3)
    if (doesConfigurationUsePort(SERIAL_PORT_UART3) && featureConfigured(FEATURE_DISPLAY)) {
        featureClear(FEATURE_DISPLAY);
    }
#endif

#ifdef STM32F303xC
    // hardware supports serial port inversion, make users life easier for those that want to connect SBus RX's
#ifdef TELEMETRY
    telemetryConfig()->telemetry_inversion = 1;
#endif
#endif

    /*
     * The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
     * The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
     */
    if (armingConfig()->retarded_arm && mixerConfig()->pid_at_min_throttle) {
        mixerConfig()->pid_at_min_throttle = 0;
    }

#if defined(LED_STRIP) && defined(TRANSPONDER) && !defined(UNIT_TEST)
    if ((WS2811_DMA_TC_FLAG == TRANSPONDER_DMA_TC_FLAG) && featureConfigured(FEATURE_TRANSPONDER) && featureConfigured(FEATURE_LED_STRIP)) {
        featureClear(FEATURE_LED_STRIP);
    }
#endif

#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
    // shared pin
    if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
    	featureClear(FEATURE_SONAR);
    	featureClear(FEATURE_SOFTSERIAL);
    	featureClear(FEATURE_RSSI_ADC);
    }
#endif

#if defined(COLIBRI_RACE)
    serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
    if (featureConfigured(FEATURE_RX_SERIAL)) {
        serialConfig()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
    }
#endif

    if (!isSerialConfigValid(serialConfig())) {
        PG_RESET_CURRENT(serialConfig);
    }

#if defined(USE_VCP)
    serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
#endif
}
Exemple #15
0
static void validateAndFixConfig(void)
{
#if !defined(USE_QUAD_MIXER_ONLY) && !defined(USE_OSD_SLAVE)
    // Reset unsupported mixer mode to default.
    // This check will be gone when motor/servo mixers are loaded dynamically
    // by configurator as a part of configuration procedure.

    mixerMode_e mixerMode = mixerConfigMutable()->mixerMode;

    if (!(mixerMode == MIXER_CUSTOM || mixerMode == MIXER_CUSTOM_AIRPLANE || mixerMode == MIXER_CUSTOM_TRI)) {
        if (mixers[mixerMode].motorCount && mixers[mixerMode].motor == NULL)
            mixerConfigMutable()->mixerMode = MIXER_CUSTOM;
#ifdef USE_SERVOS
        if (mixers[mixerMode].useServo && servoMixers[mixerMode].servoRuleCount == 0)
            mixerConfigMutable()->mixerMode = MIXER_CUSTOM_AIRPLANE;
#endif
    }
#endif

#ifndef USE_OSD_SLAVE
    if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {
        systemConfigMutable()->activeRateProfile = 0;
    }
    setControlRateProfile(systemConfig()->activeRateProfile);

    if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) {
        systemConfigMutable()->pidProfileIndex = 0;
    }
    setPidProfile(systemConfig()->pidProfileIndex);

    // Prevent invalid notch cutoff
    if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) {
        currentPidProfile->dterm_notch_hz = 0;
    }

    if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
        motorConfigMutable()->mincommand = 1000;
    }

    if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
        motorConfigMutable()->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
    }

    validateAndFixGyroConfig();

    if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
        featureSet(DEFAULT_RX_FEATURE);
    }

    if (featureConfigured(FEATURE_RX_PPM)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_SPI);
    }

    if (featureConfigured(FEATURE_RX_MSP)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_SPI);
    }

    if (featureConfigured(FEATURE_RX_SERIAL)) {
        featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI);
    }

#ifdef USE_RX_SPI
    if (featureConfigured(FEATURE_RX_SPI)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP);
    }
#endif // USE_RX_SPI

    if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI);
#if defined(STM32F10X)
        // rssi adc needs the same ports
        featureClear(FEATURE_RSSI_ADC);
        // current meter needs the same ports
        if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
            batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
        }
#endif // STM32F10X
        // software serial needs free PWM ports
        featureClear(FEATURE_SOFTSERIAL);
    }

#ifdef USE_SOFTSPI
    if (featureConfigured(FEATURE_SOFTSPI)) {
        featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL);
        batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_NONE;
#if defined(STM32F10X)
        featureClear(FEATURE_LED_STRIP);
        // rssi adc needs the same ports
        featureClear(FEATURE_RSSI_ADC);
        // current meter needs the same ports
        if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
            batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
        }
#endif // STM32F10X
    }
#endif // USE_SOFTSPI

#endif // USE_OSD_SLAVE

    if (!isSerialConfigValid(serialConfig())) {
        pgResetFn_serialConfig(serialConfigMutable());
    }

// clear features that are not supported.
// I have kept them all here in one place, some could be moved to sections of code above.

#ifndef USE_PPM
    featureClear(FEATURE_RX_PPM);
#endif

#ifndef USE_SERIAL_RX
    featureClear(FEATURE_RX_SERIAL);
#endif

#if !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2)
    featureClear(FEATURE_SOFTSERIAL);
#endif

#ifndef USE_GPS
    featureClear(FEATURE_GPS);
#endif

#ifndef USE_RANGEFINDER
    featureClear(FEATURE_RANGEFINDER);
#endif

#ifndef USE_TELEMETRY
    featureClear(FEATURE_TELEMETRY);
#endif

#ifndef USE_PWM
    featureClear(FEATURE_RX_PARALLEL_PWM);
#endif

#ifndef USE_RX_MSP
    featureClear(FEATURE_RX_MSP);
#endif

#ifndef USE_LED_STRIP
    featureClear(FEATURE_LED_STRIP);
#endif

#ifndef USE_DASHBOARD
    featureClear(FEATURE_DASHBOARD);
#endif

#ifndef USE_OSD
    featureClear(FEATURE_OSD);
#endif

#ifndef USE_SERVOS
    featureClear(FEATURE_SERVO_TILT | FEATURE_CHANNEL_FORWARDING);
#endif

#ifndef USE_TRANSPONDER
    featureClear(FEATURE_TRANSPONDER);
#endif

#ifndef USE_RX_SPI
    featureClear(FEATURE_RX_SPI);
#endif

#ifndef USE_SOFTSPI
    featureClear(FEATURE_SOFTSPI);
#endif

#ifndef USE_ESC_SENSOR
    featureClear(FEATURE_ESC_SENSOR);
#endif

#ifndef USE_GYRO_DATA_ANALYSE
    featureClear(FEATURE_DYNAMIC_FILTER);
#endif

#if defined(TARGET_VALIDATECONFIG)
    targetValidateConfiguration();
#endif
}
Exemple #16
0
void fcTasksInit(void)
{
    schedulerInit();

    if (sensors(SENSOR_GYRO)) {
        rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
        setTaskEnabled(TASK_GYROPID, true);
    }

    if (sensors(SENSOR_ACC)) {
        setTaskEnabled(TASK_ACCEL, true);
        rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
    }

    setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
    setTaskEnabled(TASK_SERIAL, true);
    rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz));

    bool useBatteryVoltage = batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
    setTaskEnabled(TASK_BATTERY_VOLTAGE, useBatteryVoltage);
    bool useBatteryCurrent = batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
    setTaskEnabled(TASK_BATTERY_CURRENT, useBatteryCurrent);

    bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || feature(FEATURE_OSD);
    setTaskEnabled(TASK_BATTERY_ALERTS, (useBatteryVoltage || useBatteryCurrent) && useBatteryAlerts);

    setTaskEnabled(TASK_RX, true);

    setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled());

#ifdef BEEPER
    setTaskEnabled(TASK_BEEPER, true);
#endif
#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, TASK_PERIOD_HZ(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 USE_DASHBOARD
    setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
#endif
#ifdef TELEMETRY
    setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
    if (feature(FEATURE_TELEMETRY)) {
        if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) {
            // Reschedule telemetry to 500hz for Jeti Exbus
            rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
        } else if (rxConfig()->serialrx_provider == SERIALRX_CRSF) {
            // Reschedule telemetry to 500hz, 2ms for CRSF
            rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
        }
    }
#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_OSD_SLAVE
    setTaskEnabled(TASK_OSD_SLAVE, true);
#endif
#ifdef USE_BST
    setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
#endif
#ifdef USE_ESC_SENSOR
    setTaskEnabled(TASK_ESC_SENSOR, feature(FEATURE_ESC_SENSOR));
#endif
#ifdef CMS
#ifdef USE_MSP_DISPLAYPORT
    setTaskEnabled(TASK_CMS, true);
#else
    setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
#endif
#endif
#ifdef STACK_CHECK
    setTaskEnabled(TASK_STACK_CHECK, true);
#endif
#ifdef VTX_CONTROL
#if defined(VTX_RTC6705) || defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
    setTaskEnabled(TASK_VTXCTRL, true);
#endif
#endif
}