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
}
}
