static void buildTelemetryFrame(uint8_t *packet) { uint8_t a1Value; switch (rxFrSkySpiConfig()->a1Source) { case FRSKY_SPI_A1_SOURCE_VBAT: a1Value = (getBatteryVoltage() / 5) & 0xff; break; case FRSKY_SPI_A1_SOURCE_EXTADC: a1Value = (adcGetChannel(ADC_EXTERNAL1) & 0xff0) >> 4; break; case FRSKY_SPI_A1_SOURCE_CONST: a1Value = A1_CONST_D & 0xff; break; } const uint8_t a2Value = (adcGetChannel(ADC_RSSI)) >> 4; telemetryId = packet[4]; frame[0] = 0x11; // length frame[1] = rxFrSkySpiConfig()->bindTxId[0]; frame[2] = rxFrSkySpiConfig()->bindTxId[1]; frame[3] = a1Value; frame[4] = a2Value; frame[5] = (uint8_t)cc2500getRssiDbm(); uint8_t bytesUsed = 0; #if defined(USE_TELEMETRY_FRSKY_HUB) if (telemetryEnabled) { bytesUsed = appendFrSkyHubData(&frame[8]); } #endif frame[6] = bytesUsed; frame[7] = telemetryId; }
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; int32_t throttleFactor = 0; switch(batteryConfig->currentMeterType) { case CURRENT_SENSOR_ADC: amperageRaw -= amperageRaw / 8; amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); amperage = currentSensorToCentiamps(amperageRaw / 8); break; case CURRENT_SENSOR_VIRTUAL: amperage = (int32_t)batteryConfig->currentMeterOffset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleOffset = 0; throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; } break; case CURRENT_SENSOR_NONE: amperage = 0; break; } mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; mAhDrawn = mAhdrawnRaw / (3600 * 100); }
/** @Brief 电位器\旋钮获取中值 * @ 多次采用取平均 * * @Note * 1) 调用此程序前,各电位器拨到中间位置 */ void stickKnobCalibrationMiddle(void) { uint32_t currentTime; uint8_t i, count; uint16_t adc; uint32_t adcSum[STICK_KNOB_SUM]; for(i=0; i<STICK_KNOB_SUM; i++) { adcSum[i] = 0; StickKnob[i].adcLimitMiddle = 2048; } count = 0; //1) 每20ms采样一次,1s后计算均值 currentTime = micros() + 300000; while((int32_t)(currentTime - micros()) >= 0) { for(i=0; i<STICK_KNOB_SUM; i++) { adc = adcGetChannel(StickKnob[i].adcChannel); adcSum[i] += adc; } count++; delay(20); LED0_TOGGLE; } for(i=0; i<STICK_KNOB_SUM; i++) { StickKnob[i].adcLimitMiddle = adcSum[i]/count; } //2) 保存至EEPROM for(i = 0; i < STICK_KNOB_SUM; i++) { cfg.adcLimitMiddle[i] = StickKnob[i].adcLimitMiddle; } writeEEPROM(1, 1); }
void updateCurrentMeter(int32_t lastUpdateAt) { static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; int32_t throttleFactor = 0; switch(batteryConfig->currentMeterType) { case CURRENT_SENSOR_ADC: amperageRaw -= amperageRaw / 8; amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); amperage = currentSensorToCentiamps(amperageRaw / 8); break; case CURRENT_SENSOR_VIRTUAL: amperage = (int32_t)batteryConfig->currentMeterOffset; if(ARMING_FLAG(ARMED)) { throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; } break; case CURRENT_SENSOR_NONE: amperage = 0; break; } mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; mAhDrawn = mAhdrawnRaw / (3600 * 100); }
void voltageMeterADCRefresh(void) { for (uint8_t i = 0; i < MAX_VOLTAGE_SENSOR_ADC && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) { voltageMeterADCState_t *state = &voltageMeterADCStates[i]; #ifdef USE_ADC // store the battery voltage with some other recent battery voltage readings const voltageSensorADCConfig_t *config = voltageSensorADCConfig(i); uint8_t channel = voltageMeterAdcChannelMap[i]; uint16_t rawSample = adcGetChannel(channel); uint16_t filteredSample = biquadFilterApply(&state->filter, rawSample); // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand. state->voltageFiltered = voltageAdcToVoltage(filteredSample, config); state->voltageUnfiltered = voltageAdcToVoltage(rawSample, config); #else UNUSED(voltageAdcToVoltage); state->voltageFiltered = 0; state->voltageUnfiltered = 0; #endif } }
void updateRSSIADC(uint32_t currentTime) { #ifndef USE_ADC UNUSED(currentTime); #else static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT]; static uint8_t adcRssiSampleIndex = 0; static uint32_t rssiUpdateAt = 0; if ((int32_t)(currentTime - rssiUpdateAt) < 0) { return; } rssiUpdateAt = currentTime + DELAY_50_HZ; int16_t adcRssiMean = 0; uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); uint8_t rssiPercentage = adcRssiSample / rxConfig->rssi_scale; adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT; adcRssiSamples[adcRssiSampleIndex] = rssiPercentage; uint8_t sampleIndex; for (sampleIndex = 0; sampleIndex < RSSI_ADC_SAMPLE_COUNT; sampleIndex++) { adcRssiMean += adcRssiSamples[sampleIndex]; } adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT; rssi = (uint16_t)((constrain(adcRssiMean, 0, 100) / 100.0f) * 1023.0f); #endif }
/** @Brief 获取电位器\旋钮数值 * @Return StickKnob.output = 1000~2000 */ void stickKnobScan(void) { uint16_t adc, result; float def, range; uint8_t i; //1) 依次更新所有电位器 for(i=0; i<STICK_KNOB_SUM; i++) { adc = adcGetChannel(StickKnob[i].adcChannel); //2) 以中值为界,上下两段分别解算 if(adc >= StickKnob[i].adcLimitMiddle){ def = adc - StickKnob[i].adcLimitMiddle; range = StickKnob[i].adcLimitHigh - StickKnob[i].adcLimitMiddle; if(range <= 0) range = 1; result = def / range * 500.0f; if(result > 500) result = 500; StickKnob[i].output = 500 + result; } else { def = StickKnob[i].adcLimitMiddle - adc; range = StickKnob[i].adcLimitMiddle - StickKnob[i].adcLimitLow; if(range <= 0) range = 1; result = def / range * 500.0f; if(result > 500) result = 500; StickKnob[i].output = 500 - result; } //3) 若电位器反向,调整输出数据为(左小右大、后小前大) if(StickKnob[i].reverse) StickKnob[i].output = 1000 - StickKnob[i].output; StickKnob[i].output += 1000; } }
static void updateBatteryVoltage(void) { uint16_t vbatSample; // store the battery voltage with some other recent battery voltage readings vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState); vbat = batteryAdcToVoltage(vbatSample); }
static void updateBatteryVoltage(void) { uint16_t vbatSample; uint16_t vbatFiltered; // store the battery voltage with some other recent battery voltage readings vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); vbatFiltered = (uint16_t)lowpassFixed(&lowpassFilter, vbatSample, VBATT_LPF_FREQ); vbat = batteryAdcToVoltage(vbatFiltered); }
static void updateBatteryVoltage(uint32_t vbatTimeDelta) { uint16_t vbatSample; static pt1Filter_t vbatFilterState; // store the battery voltage with some other recent battery voltage readings vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); vbatSample = pt1FilterApply4(&vbatFilterState, vbatSample, VBATT_LPF_FREQ, vbatTimeDelta * 1e-6f); vbat = batteryAdcToVoltage(vbatSample); }
void updateBatteryVoltage(void) { static uint16_t vbatSamples[BATTERY_SAMPLE_COUNT]; static uint8_t currentSampleIndex = 0; uint8_t index; uint16_t vbatSampleTotal = 0; // store the battery voltage with some other recent battery voltage readings vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatestADC = adcGetChannel(ADC_BATTERY); // calculate vbat based on the average of recent readings for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) { vbatSampleTotal += vbatSamples[index]; } vbat = batteryAdcToVoltage(vbatSampleTotal / BATTERY_SAMPLE_COUNT); }
uint16_t RSSI_getValue(void) { uint16_t value = 0; if (mcfg.rssi_aux_channel > 0) { const int16_t rssiChannelData = rcData[AUX1 + mcfg.rssi_aux_channel - 1]; // Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023]; value = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f); } else if (mcfg.rssi_adc_channel > 0) { const int16_t rssiData = (((int32_t)(adcGetChannel(ADC_RSSI) - mcfg.rssi_adc_offset)) * 1023L) / mcfg.rssi_adc_max; // Set to correct range [0;1023] value = constrain(rssiData, 0, 1023); } // return range [0;1023] return value; }
void batteryInit(void) { uint32_t i; uint32_t voltage = 0; for (i = 0; i < 32; i++) // average up some voltage readings { voltage += adcGetChannel(ADC_BATTERY); delay(10); } voltage = batteryAdcToVoltage((uint16_t)(voltage / 32)); for (i = 2; i < 6; i++) // autodetect cell count, going from 2S..6S { if (voltage < i * cfg.vbatmaxcellvoltage) break; } batteryCellCount = i; batteryWarningVoltage = i * cfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI }
/** @Brief 电位器\旋钮矫正上下边缘 * */ void stickKnobCalibrationEdge(void) { uint32_t currentTime; uint8_t i; uint16_t adc; int16_t def; for(i=0; i<STICK_KNOB_SUM; i++) { StickKnob[i].adcLimitHigh = 2048; StickKnob[i].adcLimitLow = 2048; } //1) 读取电位器ADC数值,更新上下边缘 //2) 30s后退出 currentTime = micros() + 30000000; while((int32_t)(currentTime - micros()) >= 0) { for(i=0; i<STICK_KNOB_SUM; i++) { adc = adcGetChannel(StickKnob[i].adcChannel); if(adc > StickKnob[i].adcLimitHigh){ StickKnob[i].adcLimitHigh = adc; } if(adc < StickKnob[i].adcLimitLow){ StickKnob[i].adcLimitLow = adc; } } delay(20); LED0_TOGGLE; } //3) 上下边缘各留出5%的虚位 for(i=0; i<STICK_KNOB_SUM; i++) { def = StickKnob[i].adcLimitHigh - StickKnob[i].adcLimitLow; def /= 20; if(def < 0) def = 0; StickKnob[i].adcLimitHigh -= def; StickKnob[i].adcLimitLow += def; } //4) 保存至EEPROM for(i = 0; i < STICK_KNOB_SUM; i++) { cfg.adcLimitHigh[i] = StickKnob[i].adcLimitHigh; cfg.adcLimitLow[i] = StickKnob[i].adcLimitLow; } writeEEPROM(1, 1); }
void voltageMeterUpdate(void) { uint16_t vbatSample; for (uint8_t i = 0; i < MAX_VOLTAGE_METERS && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) { // store the battery voltage with some other recent battery voltage readings voltageMeterState_t *state = &voltageMeterStates[i]; voltageMeterConfig_t *config = voltageMeterConfig(i); uint8_t channel = voltageMeterAdcChannelMap[i]; vbatSample = state->vbatLatestADC = adcGetChannel(channel); vbatSample = biquadFilterApply(&state->vbatFilterState, vbatSample); // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand. state->vbat = voltageAdcToVoltage(vbatSample, config); } }
static void updateBatteryVoltage(void) { static biquadFilter_t vbatFilter; static bool vbatFilterIsInitialised; // store the battery voltage with some other recent battery voltage readings uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample; if (!vbatFilterIsInitialised) { biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update vbatFilterIsInitialised = true; } vbatSample = biquadFilterApply(&vbatFilter, vbatSample); vbat = batteryAdcToVoltage(vbatSample); if (debugMode == DEBUG_BATTERY) debug[1] = vbat; }
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 }
void annexCode(void) { static uint32_t calibratedAccTime; int32_t tmp, tmp2; int32_t axis, prop1, prop2; static uint8_t buzzerFreq; // delay between buzzer ring // vbat shit static uint8_t vbatTimer = 0; static int32_t vbatRaw = 0; static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; static int32_t vbatCycleTime = 0; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < cfg.tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpa_breakpoint) / (2000 - cfg.tpa_breakpoint); } else { prop2 = 100 - cfg.dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = min(abs(rcData[axis] - mcfg.midrc), 500); if (axis != 2) { // ROLL & PITCH if (cfg.deadband) { if (tmp > cfg.deadband) { tmp -= cfg.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)cfg.rollPitchRate * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else { // YAW if (cfg.yawdeadband) { if (tmp > cfg.yawdeadband) { tmp -= cfg.yawdeadband; } else { tmp = 0; } } rcCommand[axis] = tmp * -mcfg.yaw_control_direction; prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500; } dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100; if (rcData[axis] < mcfg.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000); tmp = (uint32_t)(tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (f.HEADFREE_MODE) { float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f; float cosDiff = cosf(radDiff); float sinDiff = sinf(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT)) { vbatCycleTime += cycleTime; if (!(++vbatTimer % VBATFREQ)) { vbatRaw -= vbatRaw / 8; vbatRaw += adcGetChannel(ADC_BATTERY); vbat = batteryAdcToVoltage(vbatRaw / 8); if (mcfg.power_adc_channel > 0) { amperageRaw -= amperageRaw / 8; amperageRaw += adcGetChannel(ADC_EXTERNAL_CURRENT); amperage = currentSensorToCentiamps(amperageRaw / 8); mAhdrawnRaw += (amperage * vbatCycleTime) / 1000; mAhdrawn = mAhdrawnRaw / (3600 * 100); vbatCycleTime = 0; } } if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off buzzerFreq = 0; } else buzzerFreq = 4; // low battery } buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis LED0_TOGGLE; } else { if (f.ACC_CALIBRATED) LED0_OFF; if (f.ARMED) LED0_ON; #ifndef CJMCU checkTelemetryState(); #endif } #ifdef LEDRING if (feature(FEATURE_LED_RING)) { static uint32_t LEDTime; if ((int32_t)(currentTime - LEDTime) >= 0) { LEDTime = currentTime + 50000; ledringState(); } } #endif if ((int32_t)(currentTime - calibratedAccTime) >= 0) { if (!f.SMALL_ANGLE) { f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated LED0_TOGGLE; calibratedAccTime = currentTime + 500000; } else { f.ACC_CALIBRATED = 1; } } serialCom(); #ifndef CJMCU if (!cliMode && feature(FEATURE_TELEMETRY)) { handleTelemetry(); } #endif if (sensors(SENSOR_GPS)) { static uint32_t GPSLEDTime; if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { GPSLEDTime = currentTime + 150000; LED1_TOGGLE; } } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); else { // TODO MCU temp } }
void annexCode(void) { static uint32_t calibratedAccTime; int32_t tmp, tmp2; int32_t axis, prop1, prop2; static uint8_t buzzerFreq; // delay between buzzer ring // vbat shit static uint8_t vbatTimer = 0; static uint8_t ind = 0; uint16_t vbatRaw = 0; static uint16_t vbatRawArray[8]; int i; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < BREAKPOINT) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT); } else { prop2 = 100 - cfg.dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = min(abs(rcData[axis] - mcfg.midrc), 500); if (axis != 2) { // ROLL & PITCH if (cfg.deadband) { if (tmp > cfg.deadband) { tmp -= cfg.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500; prop1 = (uint16_t) prop1 *prop2 / 100; } else { // YAW if (cfg.yawdeadband) { if (tmp > cfg.yawdeadband) { tmp -= cfg.yawdeadband; } else { tmp = 0; } } rcCommand[axis] = tmp * -mcfg.yaw_control_direction; prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500; } dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100; if (rcData[axis] < mcfg.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000); tmp = (uint32_t) (tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (f.HEADFREE_MODE) { float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f; float cosDiff = cosf(radDiff); float sinDiff = sinf(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT)) { if (!(++vbatTimer % VBATFREQ)) { vbatRawArray[(ind++) % 8] = adcGetChannel(ADC_BATTERY); for (i = 0; i < 8; i++) vbatRaw += vbatRawArray[i]; vbat = batteryAdcToVoltage(vbatRaw / 8); } if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off buzzerFreq = 0; } else buzzerFreq = 4; // low battery } buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis LED0_TOGGLE; } else { if (f.ACC_CALIBRATED) LED0_OFF; if (f.ARMED) LED0_ON; // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. With telemetry_softserial>0 telemetry is always enabled, also see updateTelemetryState() if (feature(FEATURE_TELEMETRY)) updateTelemetryState(); } #ifdef LEDRING if (feature(FEATURE_LED_RING)) { static uint32_t LEDTime; if ((int32_t)(currentTime - LEDTime) >= 0) { LEDTime = currentTime + 50000; ledringState(); } } #endif if ((int32_t)(currentTime - calibratedAccTime) >= 0) { if (!f.SMALL_ANGLES_25) { f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated LED0_TOGGLE; //DEBUG_PRINT("LED\r\n"); calibratedAccTime = currentTime + 500000; } else { f.ACC_CALIBRATED = 1; } } serialCom(); if (sensors(SENSOR_GPS)) { static uint32_t GPSLEDTime; if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { GPSLEDTime = currentTime + 150000; LED1_TOGGLE; } } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); else { // TODO MCU temp } }