static void showBatteryPage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) { tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount()); padLineBuffer(); i2c_OLED_set_line(bus, rowIndex++); i2c_OLED_send_string(bus, lineBuffer); uint8_t batteryPercentage = calculateBatteryPercentageRemaining(); i2c_OLED_set_line(bus, rowIndex++); drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage); } if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { int32_t amperage = getAmperage(); tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, getMAhDrawn()); padLineBuffer(); i2c_OLED_set_line(bus, rowIndex++); i2c_OLED_send_string(bus, lineBuffer); uint8_t capacityPercentage = calculateBatteryPercentageRemaining(); i2c_OLED_set_line(bus, rowIndex++); drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage); } }
static void sendFuelLevel(void) { sendDataHead(ID_FUEL_LEVEL); if (batteryConfig()->batteryCapacity > 0) { serialize16((uint16_t)batteryCapacityRemainingPercentage()); } else { amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource); serialize16((uint16_t)constrain(state->mAhDrawn, 0, 0xFFFF)); } }
void taskUpdateBattery(void) { static uint32_t vbatLastServiced = 0; static uint32_t ibatLastServiced = 0; if (feature(FEATURE_VBAT)) { if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { vbatLastServiced = currentTime; voltageMeterUpdate(); batteryUpdate(); } } if (feature(FEATURE_AMPERAGE_METER)) { int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTime; if (batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC) { amperageUpdateMeter(ibatTimeSinceLastServiced); } else { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle); bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)); int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; amperageUpdateVirtualMeter(ibatTimeSinceLastServiced, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset); } } } }
static void sendAmperage(void) { amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource); sendDataHead(ID_CURRENT); serialize16((uint16_t)(state->amperage / 10)); }
static long cmsx_menuMiscOnEnter(void) { motorConfig_minthrottle = motorConfig()->minthrottle; motorConfig_digitalIdleOffsetValue = motorConfig()->digitalIdleOffsetValue / 10; voltageSensorADCConfig_vbatscale = voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale; batteryConfig_vbatmaxcellvoltage = batteryConfig()->vbatmaxcellvoltage; return 0; }
static void sendFuelLevel(void) { sendDataHead(ID_FUEL_LEVEL); if (batteryConfig()->batteryCapacity > 0) { serialize16((uint16_t)calculateBatteryCapacityRemainingPercentage()); } else { serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); } }
static long cmsx_Power_onEnter(void) { batteryConfig_voltageMeterSource = batteryConfig()->voltageMeterSource; batteryConfig_currentMeterSource = batteryConfig()->currentMeterSource; batteryConfig_vbatmaxcellvoltage = batteryConfig()->vbatmaxcellvoltage; voltageSensorADCConfig_vbatscale = voltageSensorADCConfig(0)->vbatscale; currentSensorADCConfig_scale = currentSensorADCConfig()->scale; currentSensorADCConfig_offset = currentSensorADCConfig()->offset; #ifdef USE_VIRTUAL_CURRENT_METER currentSensorVirtualConfig_scale = currentSensorVirtualConfig()->scale; currentSensorVirtualConfig_offset = currentSensorVirtualConfig()->offset; #endif return 0; }
static char osdGetBatterySymbol(int cellVoltage) { if (getBatteryState() == BATTERY_CRITICAL) { return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark } else { // Calculate a symbol offset using cell voltage over full cell voltage range const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage * 10, batteryConfig()->vbatmaxcellvoltage * 10, 0, 7); return SYM_BATT_EMPTY - constrain(symOffset, 0, 6); } }
static void applyLedWarningLayer(bool updateNow, timeUs_t *timer) { static uint8_t warningFlashCounter = 0; static uint8_t warningFlags = 0; // non-zero during blinks if (updateNow) { // keep counter running, so it stays in sync with blink warningFlashCounter++; warningFlashCounter &= 0xF; if (warningFlashCounter == 0) { // update when old flags was processed warningFlags = 0; if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryState() != BATTERY_OK) warningFlags |= 1 << WARNING_LOW_BATTERY; if (failsafeIsActive()) warningFlags |= 1 << WARNING_FAILSAFE; if (!ARMING_FLAG(ARMED) && isArmingDisabled()) warningFlags |= 1 << WARNING_ARMING_DISABLED; } *timer += HZ_TO_US(10); } const hsvColor_t *warningColor = NULL; if (warningFlags) { bool colorOn = (warningFlashCounter % 2) == 0; // w_w_ warningFlags_e warningId = warningFlashCounter / 4; if (warningFlags & (1 << warningId)) { switch (warningId) { case WARNING_ARMING_DISABLED: warningColor = colorOn ? &HSV(GREEN) : &HSV(BLACK); break; case WARNING_LOW_BATTERY: warningColor = colorOn ? &HSV(RED) : &HSV(BLACK); break; case WARNING_FAILSAFE: warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE); break; default:; } } } else { if (isBeeperOn()) { warningColor = &HSV(ORANGE); } } if (warningColor) { applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); } }
void osdSlaveTasksInit(void) { schedulerInit(); setTaskEnabled(TASK_SERIAL, true); 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); setTaskEnabled(TASK_BATTERY_ALERTS, (useBatteryVoltage || useBatteryCurrent) && useBatteryAlerts); #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif setTaskEnabled(TASK_OSD_SLAVE, true); #ifdef STACK_CHECK setTaskEnabled(TASK_STACK_CHECK, true); #endif }
static void sendThrottleOrBatterySizeAsRpm(uint16_t deadband3d_throttle) { uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; sendDataHead(ID_RPM); if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), deadband3d_throttle); if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; serialize16(throttleForRPM); } else { serialize16((batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER)); } }
void init(void) { drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #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(systemConfig()->emf_avoidance); #endif i2cSetOverclock(systemConfig()->i2c_highspeed); systemInit(); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); // initialize IO (needed for all IO operations) IOInitGlobal(); debugMode = debugConfig()->debug_mode; #ifdef USE_EXTI EXTIInit(); #endif #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_1) { ledInit(false); } else { ledInit(true); } #else ledInit(false); #endif #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPeripheral = BEEP_PERIPHERAL, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #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 BUTTONS buttonsInit(); if (!isMPUSoftReset()) { buttonsHandleColdBootButtonPresses(); } #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (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(rxConfig()); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated dmaInit(); serialInit(feature(FEATURE_SOFTSERIAL)); mixerInit(customMotorMixer(0)); #ifdef USE_SERVOS mixerInitServos(customServoMixer(0)); #endif memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR const sonarHardware_t *sonarHardware = NULL; sonarGPIOConfig_t sonarGPIOConfig; if (feature(FEATURE_SONAR)) { bool usingCurrentMeterIOPins = (feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC); sonarHardware = sonarGetHardwareConfiguration(usingCurrentMeterIOPins); sonarGPIOConfig.triggerGPIO = sonarHardware->trigger_gpio; sonarGPIOConfig.triggerPin = sonarHardware->trigger_pin; sonarGPIOConfig.echoGPIO = sonarHardware->echo_gpio; sonarGPIOConfig.echoPin = sonarHardware->echo_pin; pwm_params.sonarGPIOConfig = &sonarGPIOConfig; } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_UART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2); #endif #if defined(USE_UART3) pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3); #endif #if defined(USE_UART4) pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4); #endif #if defined(USE_UART5) pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5); #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_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC ); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse; pwm_params.servoPwmRate = servoConfig()->servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = motorConfig()->motor_pwm_rate; pwm_params.idlePulse = calculateMotorOff(); if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(); // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params); mixerUsePWMIOConfiguration(pwmIOConfiguration); #ifdef DEBUG_PWM_CONFIGURATION debug[2] = pwmIOConfiguration->pwmInputCount; debug[3] = pwmIOConfiguration->ppmInputCount; #endif if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #ifdef STM32F303xC #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_2) { spiInit(SPI3); } #else spiInit(SPI3); #endif #endif #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_UART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.channelMask = 0; #ifdef ADC_BATTERY adc_params.channelMask = (feature(FEATURE_VBAT) ? ADC_CHANNEL_MASK(ADC_BATTERY) : 0); #endif #ifdef ADC_RSSI adc_params.channelMask |= (feature(FEATURE_RSSI_ADC) ? ADC_CHANNEL_MASK(ADC_RSSI) : 0); #endif #ifdef ADC_AMPERAGE adc_params.channelMask |= (feature(FEATURE_AMPERAGE_METER) ? ADC_CHANNEL_MASK(ADC_AMPERAGE) : 0); #endif #ifdef ADC_POWER_12V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_12V); #endif #ifdef ADC_POWER_5V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_5V); #endif #ifdef ADC_POWER_3V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_3V); #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.channelMask |= (hardwareRevision >= NAZE32_REV5) ? ADC_CHANNEL_MASK(ADC_EXTERNAL) : 0; #endif adcInit(&adc_params); #endif initBoardAlignment(); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(); } #endif #ifdef NAZE if (hardwareRevision < NAZE32_REV5) { gyroConfig()->gyro_sync = 0; } #endif if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; flashLedsAndBeep(); mspInit(); mspSerialInit(); const uint16_t pidPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz); pidSetTargetLooptime(pidPeriodUs * gyroConfig()->pid_process_denom); pidInitFilters(pidProfile()); #ifdef USE_SERVOS mixerInitialiseServoFiltering(targetPidLooptime); #endif imuInit(); #ifdef USE_CLI cliInit(); #endif failsafeInit(); rxInit(modeActivationProfile()->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit(); navigationInit(pidProfile()); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(sonarHardware); } #endif #ifdef LED_STRIP ledStripInit(); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(transponderConfig()->data); transponderEnable(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif #ifdef BLACKBOX initBlackbox(); #endif if (mixerConfig()->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 if (feature(FEATURE_VBAT)) { // Now that everything has powered up the voltage and cell count be determined. voltageMeterInit(); batteryInit(); } if (feature(FEATURE_AMPERAGE_METER)) { amperageMeterInit(); } #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif void configureScheduler(void) { schedulerInit(); setTaskEnabled(TASK_SYSTEM, true); uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz); rescheduleTask(TASK_GYRO, gyroPeriodUs); setTaskEnabled(TASK_GYRO, true); rescheduleTask(TASK_PID, gyroPeriodUs); setTaskEnabled(TASK_PID, true); if (sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); } setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_AMPERAGE_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, 1000000 / 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 DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif }
static void osdShowStats(uint16_t endBatteryVoltage) { uint8_t top = 2; char buff[OSD_ELEMENT_BUFFER_LENGTH]; displayClearScreen(osdDisplayPort); displayWrite(osdDisplayPort, 2, top++, " --- STATS ---"); if (osdStatGetState(OSD_STAT_RTC_DATE_TIME)) { bool success = false; #ifdef USE_RTC_TIME success = osdFormatRtcDateTime(&buff[0]); #endif if (!success) { tfp_sprintf(buff, "NO RTC"); } displayWrite(osdDisplayPort, 2, top++, buff); } if (osdStatGetState(OSD_STAT_TIMER_1)) { osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1); osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff); } if (osdStatGetState(OSD_STAT_TIMER_2)) { osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2); osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff); } if (osdStatGetState(OSD_STAT_MAX_SPEED) && STATE(GPS_FIX)) { itoa(stats.max_speed, buff, 10); osdDisplayStatisticLabel(top++, "MAX SPEED", buff); } if (osdStatGetState(OSD_STAT_MAX_DISTANCE)) { tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol()); osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff); } if (osdStatGetState(OSD_STAT_MIN_BATTERY)) { tfp_sprintf(buff, "%d.%1d%c", stats.min_voltage / 10, stats.min_voltage % 10, SYM_VOLT); osdDisplayStatisticLabel(top++, "MIN BATTERY", buff); } if (osdStatGetState(OSD_STAT_END_BATTERY)) { tfp_sprintf(buff, "%d.%1d%c", endBatteryVoltage / 10, endBatteryVoltage % 10, SYM_VOLT); osdDisplayStatisticLabel(top++, "END BATTERY", buff); } if (osdStatGetState(OSD_STAT_BATTERY)) { tfp_sprintf(buff, "%d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT); osdDisplayStatisticLabel(top++, "BATTERY", buff); } if (osdStatGetState(OSD_STAT_MIN_RSSI)) { itoa(stats.min_rssi, buff, 10); strcat(buff, "%"); osdDisplayStatisticLabel(top++, "MIN RSSI", buff); } if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { if (osdStatGetState(OSD_STAT_MAX_CURRENT)) { itoa(stats.max_current, buff, 10); strcat(buff, "A"); osdDisplayStatisticLabel(top++, "MAX CURRENT", buff); } if (osdStatGetState(OSD_STAT_USED_MAH)) { tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH); osdDisplayStatisticLabel(top++, "USED MAH", buff); } } if (osdStatGetState(OSD_STAT_MAX_ALTITUDE)) { osdFormatAltitudeString(buff, stats.max_altitude); osdDisplayStatisticLabel(top++, "MAX ALTITUDE", buff); } #ifdef USE_BLACKBOX if (osdStatGetState(OSD_STAT_BLACKBOX) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) { osdGetBlackboxStatusString(buff); osdDisplayStatisticLabel(top++, "BLACKBOX", buff); } if (osdStatGetState(OSD_STAT_BLACKBOX_NUMBER) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) { itoa(blackboxGetLogNumber(), buff, 10); osdDisplayStatisticLabel(top++, "BB LOG NUM", buff); } #endif }
CMS_Menu cmsx_menuRcPreview = { .GUARD_text = "XRCPREV", .GUARD_type = OME_MENU, .onEnter = NULL, .onExit = cmsx_menuRcConfirmBack, .onGlobalExit = NULL, .entries = cmsx_menuRcEntries }; static OSD_Entry menuMiscEntries[]= { { "-- MISC --", OME_Label, NULL, NULL, 0 }, { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig()->minthrottle, 1000, 2000, 1 }, 0 }, { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatscale, 1, 250, 1 }, 0 }, { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage, 10, 50, 1 }, 0 }, { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, { "BACK", OME_Back, NULL, NULL, 0}, { NULL, OME_END, NULL, NULL, 0} }; CMS_Menu cmsx_menuMisc = { .GUARD_text = "XMISC", .GUARD_type = OME_MENU, .onEnter = NULL, .onExit = NULL, .onGlobalExit = NULL, .entries = menuMiscEntries };
static bool osdDrawSingleElement(uint8_t item) { if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) { return false; } uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]); uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]); char buff[OSD_ELEMENT_BUFFER_LENGTH] = ""; switch (item) { case OSD_RSSI_VALUE: { uint16_t osdRssi = getRssi() * 100 / 1024; // change range if (osdRssi >= 100) osdRssi = 99; tfp_sprintf(buff, "%c%2d", SYM_RSSI, osdRssi); break; } case OSD_MAIN_BATT_VOLTAGE: buff[0] = osdGetBatterySymbol(osdGetBatteryAverageCellVoltage()); tfp_sprintf(buff + 1, "%2d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT); break; case OSD_CURRENT_DRAW: { const int32_t amperage = getAmperage(); tfp_sprintf(buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP); break; } case OSD_MAH_DRAWN: tfp_sprintf(buff, "%4d%c", getMAhDrawn(), SYM_MAH); break; #ifdef USE_GPS case OSD_GPS_SATS: tfp_sprintf(buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat); break; case OSD_GPS_SPEED: // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K (M for MPH) switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: tfp_sprintf(buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed)); break; default: tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed)); break; } break; case OSD_GPS_LAT: // The SYM_LAT symbol in the actual font contains only blank, so we use the SYM_ARROW_NORTH osdFormatCoordinate(buff, SYM_ARROW_NORTH, gpsSol.llh.lat); break; case OSD_GPS_LON: // The SYM_LON symbol in the actual font contains only blank, so we use the SYM_ARROW_EAST osdFormatCoordinate(buff, SYM_ARROW_EAST, gpsSol.llh.lon); break; case OSD_HOME_DIR: if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { if (GPS_distanceToHome > 0) { const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw); buff[0] = osdGetDirectionSymbolFromHeading(h); } else { // We don't have a HOME symbol in the font, by now we use this buff[0] = SYM_THR1; } } else { // We use this symbol when we don't have a FIX buff[0] = SYM_COLON; } buff[1] = 0; break; case OSD_HOME_DIST: if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome); tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol()); } else { // We use this symbol when we don't have a FIX buff[0] = SYM_COLON; // overwrite any previous distance with blanks memset(buff + 1, SYM_BLANK, 6); buff[7] = '\0'; } break; #endif // GPS case OSD_COMPASS_BAR: memcpy(buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9); buff[9] = 0; break; case OSD_ALTITUDE: osdFormatAltitudeString(buff, getEstimatedAltitude()); break; case OSD_ITEM_TIMER_1: case OSD_ITEM_TIMER_2: osdFormatTimer(buff, true, true, item - OSD_ITEM_TIMER_1); break; case OSD_REMAINING_TIME_ESTIMATE: { const int mAhDrawn = getMAhDrawn(); const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn); if (mAhDrawn < 0.1 * osdConfig()->cap_alarm) { tfp_sprintf(buff, "--:--"); } else if (mAhDrawn > osdConfig()->cap_alarm) { tfp_sprintf(buff, "00:00"); } else { osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time); } break; } case OSD_FLYMODE: { if (FLIGHT_MODE(FAILSAFE_MODE)) { strcpy(buff, "!FS!"); } else if (FLIGHT_MODE(ANGLE_MODE)) { strcpy(buff, "STAB"); } else if (FLIGHT_MODE(HORIZON_MODE)) { strcpy(buff, "HOR "); } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) { strcpy(buff, "RESC"); } else if (isAirmodeActive()) { strcpy(buff, "AIR "); } else { strcpy(buff, "ACRO"); } break; } case OSD_ANTI_GRAVITY: { if (pidItermAccelerator() > 1.0f) { strcpy(buff, "AG"); } break; } case OSD_CRAFT_NAME: // This does not strictly support iterative updating if the craft name changes at run time. But since the craft name is not supposed to be changing this should not matter, and blanking the entire length of the craft name string on update will make it impossible to configure elements to be displayed on the right hand side of the craft name. //TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered. if (strlen(pilotConfig()->name) == 0) { strcpy(buff, "CRAFT_NAME"); } else { unsigned i; for (i = 0; i < MAX_NAME_LENGTH; i++) { if (pilotConfig()->name[i]) { buff[i] = toupper((unsigned char)pilotConfig()->name[i]); } else { break; } } buff[i] = '\0'; } break; case OSD_THROTTLE_POS: buff[0] = SYM_THR; buff[1] = SYM_THR1; tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); break; #if defined(USE_VTX_COMMON) case OSD_VTX_CHANNEL: { const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band]; const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel]; uint8_t vtxPower = vtxSettingsConfig()->power; const vtxDevice_t *vtxDevice = vtxCommonDevice(); if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) { vtxCommonGetPowerIndex(vtxDevice, &vtxPower); } tfp_sprintf(buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower); break; } #endif case OSD_CROSSHAIRS: buff[0] = SYM_AH_CENTER_LINE; buff[1] = SYM_AH_CENTER; buff[2] = SYM_AH_CENTER_LINE_RIGHT; buff[3] = 0; break; case OSD_ARTIFICIAL_HORIZON: { // Get pitch and roll limits in tenths of degrees const int maxPitch = osdConfig()->ahMaxPitch * 10; const int maxRoll = osdConfig()->ahMaxRoll * 10; const int rollAngle = constrain(attitude.values.roll, -maxRoll, maxRoll); int pitchAngle = constrain(attitude.values.pitch, -maxPitch, maxPitch); // Convert pitchAngle to y compensation value // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees pitchAngle = ((pitchAngle * 25) / maxPitch) - 41; // 41 = 4 * AH_SYMBOL_COUNT + 5 for (int x = -4; x <= 4; x++) { const int y = ((-rollAngle * x) / 64) - pitchAngle; if (y >= 0 && y <= 81) { displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT))); } } return true; } case OSD_HORIZON_SIDEBARS: { // Draw AH sides const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; for (int y = -hudheight; y <= hudheight; y++) { displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION); displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION); } // AH level indicators displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT); displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT); return true; } case OSD_ROLL_PIDS: osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]); break; case OSD_PITCH_PIDS: osdFormatPID(buff, "PIT", ¤tPidProfile->pid[PID_PITCH]); break; case OSD_YAW_PIDS: osdFormatPID(buff, "YAW", ¤tPidProfile->pid[PID_YAW]); break; case OSD_POWER: tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000); break; case OSD_PIDRATE_PROFILE: tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1); break; case OSD_WARNINGS: { #define OSD_WARNINGS_MAX_SIZE 11 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1) STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size); const batteryState_e batteryState = getBatteryState(); if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW"); break; } #ifdef USE_ADC_INTERNAL uint8_t coreTemperature = getCoreTemperatureCelsius(); if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) { char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg); break; } #endif #ifdef USE_ESC_SENSOR // Show warning if we lose motor output, the ESC is overheating or excessive current draw if (feature(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) { char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; unsigned pos = 0; const char *title = "ESC"; // center justify message while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) { escWarningMsg[pos++] = ' '; } strcpy(escWarningMsg + pos, title); pos += strlen(title); unsigned i = 0; unsigned escWarningCount = 0; while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) { escSensorData_t *escData = getEscSensorData(i); const char motorNumber = '1' + i; // if everything is OK just display motor number else R, T or C char warnFlag = motorNumber; if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) { warnFlag = 'R'; } if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) { warnFlag = 'T'; } if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) { warnFlag = 'C'; } escWarningMsg[pos++] = warnFlag; if (warnFlag != motorNumber) { escWarningCount++; } i++; } escWarningMsg[pos] = '\0'; if (escWarningCount > 0) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg); } break; } #endif // Warn when in flip over after crash mode if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashMode()) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP"); break; } // Show most severe reason for arming being disabled if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE) && IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) { const armingDisableFlags_e flags = getArmingDisableFlags(); for (int i = 0; i < ARMING_DISABLE_FLAGS_COUNT; i++) { if (flags & (1 << i)) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[i]); break; } } break; } if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY"); break; } // Show warning if battery is not fresh if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK) && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL"); break; } // Visual beeper if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && showVisualBeeper) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " * * * *"); break; } osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL); break; } case OSD_AVG_CELL_VOLTAGE: { const int cellV = osdGetBatteryAverageCellVoltage(); buff[0] = osdGetBatterySymbol(cellV); tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT); break; } case OSD_DEBUG: tfp_sprintf(buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]); break; case OSD_PITCH_ANGLE: case OSD_ROLL_ANGLE: { const int angle = (item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll; tfp_sprintf(buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10)); break; } case OSD_MAIN_BATT_USAGE: { // Set length of indicator bar #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered. // Calculate constrained value const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity); // Calculate mAh used progress const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS))); // Create empty battery indicator bar buff[0] = SYM_PB_START; for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) { buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY; } buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE; if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) { buff[1 + mAhUsedProgress] = SYM_PB_END; } buff[MAIN_BATT_USAGE_STEPS+2] = '\0'; break; } case OSD_DISARMED: if (!ARMING_FLAG(ARMED)) { tfp_sprintf(buff, "DISARMED"); } else { if (!lastArmState) { // previously disarmed - blank out the message one time tfp_sprintf(buff, " "); } } break; case OSD_NUMERICAL_HEADING: { const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw); tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading); break; } case OSD_NUMERICAL_VARIO: { const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario()); const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH; tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10)); break; } #ifdef USE_ESC_SENSOR case OSD_ESC_TMP: if (feature(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); } break; case OSD_ESC_RPM: if (feature(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm)); } break; #endif #ifdef USE_RTC_TIME case OSD_RTC_DATETIME: osdFormatRtcDateTime(&buff[0]); break; #endif #ifdef USE_OSD_ADJUSTMENTS case OSD_ADJUSTMENT_RANGE: tfp_sprintf(buff, "%s: %3d", adjustmentRangeName, adjustmentRangeValue); break; #endif #ifdef USE_ADC_INTERNAL case OSD_CORE_TEMPERATURE: tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); break; #endif default: return false; } displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); return true; }
void handleSmartPortTelemetry(void) { uint32_t smartPortLastServiceTime = millis(); if (!smartPortTelemetryEnabled) { return; } if (!canSendSmartPortTelemetry()) { return; } while (serialRxBytesWaiting(smartPortSerialPort) > 0) { uint8_t c = serialRead(smartPortSerialPort); smartPortDataReceive(c); } uint32_t now = millis(); // if timed out, reconfigure the UART back to normal so the GUI or CLI works if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) { smartPortState = SPSTATE_TIMEDOUT; return; } while (smartPortHasRequest) { // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long. if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) { smartPortHasRequest = 0; return; } // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; if (id == 0) { // end of table reached, loop back smartPortIdCnt = 0; id = frSkyDataIdTable[smartPortIdCnt]; } smartPortIdCnt++; int32_t tmpi; switch(id) { #ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H smartPortHasRequest = 0; } break; #endif case FSSP_DATAID_VFAS : if (feature(FEATURE_VBAT)) { smartPortSendPackage(id, vbat * 10); // given in 0.1V, convert to volts smartPortHasRequest = 0; } break; case FSSP_DATAID_CELLS : if (feature(FEATURE_VBAT) && telemetryConfig()->telemetry_send_cells) { /* * A cell packet is formated this way: https://github.com/jcheger/frsky-arduino/blob/master/FrskySP/FrskySP.cpp * content | length * ---------- | ------ * volt[id] | 12-bits * celltotal | 4 bits * cellid | 4 bits */ static uint8_t currentCell = 0; // Track current cell index number // Cells Data Payload uint32_t payload = 0; payload |= ((uint16_t)(vbat * 100 + batteryCellCount) / (batteryCellCount * 2)) & 0x0FFF; // Cell Voltage formatted for payload, modified code from cleanflight Frsky.c, TESTING NOTE: (uint16_t)(4.2 * 500.0) & 0x0FFF; payload <<= 4; payload |= (uint8_t)batteryCellCount & 0x0F; // Cell Total Count formatted for payload payload <<= 4; payload |= (uint8_t)currentCell & 0x0F; // Current Cell Index Number formatted for payload // Send Payload smartPortSendPackage(id, payload); smartPortHasRequest = 0; // Incremental Counter currentCell++; currentCell %= batteryCellCount; // Reset counter @ max index } break; case FSSP_DATAID_CURRENT : if (feature(FEATURE_AMPERAGE_METER)) { amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource); smartPortSendPackage(id, state->amperage / 10); // given in 10mA steps, unknown requested unit smartPortHasRequest = 0; } break; //case FSSP_DATAID_RPM : case FSSP_DATAID_ALTITUDE : if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter smartPortHasRequest = 0; } break; case FSSP_DATAID_FUEL : if (feature(FEATURE_AMPERAGE_METER)) { amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource); smartPortSendPackage(id, state->mAhDrawn); // given in mAh, unknown requested unit smartPortHasRequest = 0; } break; //case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC2 : #ifdef GPS case FSSP_DATAID_LATLONG : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = 0; // the same ID is sent twice, one for longitude, one for latitude // the MSB of the sent uint32_t helps FrSky keep track // the even/odd bit of our counter helps us keep track if (smartPortIdCnt & 1) { tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast if (GPS_coord[LON] < 0) tmpui |= 0x40000000; } else { tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast if (GPS_coord[LAT] < 0) tmpui |= 0x40000000; } smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; } break; #endif //case FSSP_DATAID_CAP_USED : case FSSP_DATAID_VARIO : if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s smartPortHasRequest = 0; } break; case FSSP_DATAID_HEADING : smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg smartPortHasRequest = 0; break; case FSSP_DATAID_ACCX : smartPortSendPackage(id, accSmooth[X] / 44); // unknown input and unknown output unit // we can only show 00.00 format, another digit won't display right on Taranis // dividing by roughly 44 will give acceleration in G units smartPortHasRequest = 0; break; case FSSP_DATAID_ACCY : smartPortSendPackage(id, accSmooth[Y] / 44); smartPortHasRequest = 0; break; case FSSP_DATAID_ACCZ : smartPortSendPackage(id, accSmooth[Z] / 44); smartPortHasRequest = 0; break; case FSSP_DATAID_T1 : // we send all the flags as decimal digits for easy reading tmpi = 10000; // start off with at least one digit so the most significant 0 won't be cut off // the Taranis seems to be able to fit 5 digits on the screen // the Taranis seems to consider this number a signed 16 bit integer if (ARMING_FLAG(OK_TO_ARM)) tmpi += 1; if (ARMING_FLAG(PREVENT_ARMING)) tmpi += 2; if (ARMING_FLAG(ARMED)) tmpi += 4; if (FLIGHT_MODE(ANGLE_MODE)) tmpi += 10; if (FLIGHT_MODE(HORIZON_MODE)) tmpi += 20; if (FLIGHT_MODE(GTUNE_MODE) || FLIGHT_MODE(PASSTHRU_MODE)) tmpi += 40; if (FLIGHT_MODE(MAG_MODE)) tmpi += 100; if (FLIGHT_MODE(BARO_MODE)) tmpi += 200; if (FLIGHT_MODE(SONAR_MODE)) tmpi += 400; if (FLIGHT_MODE(GPS_HOLD_MODE)) tmpi += 1000; if (FLIGHT_MODE(GPS_HOME_MODE)) tmpi += 2000; if (FLIGHT_MODE(HEADFREE_MODE)) tmpi += 4000; smartPortSendPackage(id, (uint32_t)tmpi); smartPortHasRequest = 0; break; case FSSP_DATAID_T2 : if (sensors(SENSOR_GPS)) { #ifdef GPS // provide GPS lock status smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; #endif } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; } break; #ifdef GPS case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) smartPortHasRequest = 0; } break; #endif case FSSP_DATAID_A4 : if (feature(FEATURE_VBAT)) { smartPortSendPackage(id, vbat * 10 / batteryCellCount ); //sending calculated average cell value with 0.01 precision smartPortHasRequest = 0; } break; default: break; // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start } } }
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 }
intptr_t osdElementData_amperage(void) { return (intptr_t)getAmperageMeter(batteryConfig()->amperageMeterSource)->amperage; }
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 }
// 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 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 }
// 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); } }