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;
}
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;
}
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;
}
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();
}
}
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;
}
}
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;
}
}
}
}
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;
}
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
}
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;
}
// 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);
}
}
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
}
// 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);
}
}
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
}
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
}
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
}
