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);
}
void updateBattery(void)
{
uint16_t vbatPreviousADC = vbatLatestADC;
updateBatteryVoltage();
uint16_t vbatMeasured = batteryAdcToVoltage(vbatLatestADC);
/* battery has just been connected*/
if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - batteryAdcToVoltage(vbatPreviousADC)) <= VBAT_STABLE_MAX_DELTA))) {
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
batteryState = BATTERY_OK;
unsigned cells = (vbatMeasured / batteryConfig->vbatmaxcellvoltage) + 1;
if (cells > 8) {
// something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
cells = 8;
}
batteryCellCount = cells;
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */
} else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - batteryAdcToVoltage(vbatPreviousADC)) <= VBAT_STABLE_MAX_DELTA) {
batteryState = BATTERY_NOT_PRESENT;
batteryCellCount = 0;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}
switch(batteryState)
{
case BATTERY_OK:
if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) {
batteryState = BATTERY_WARNING;
beeper(BEEPER_BAT_LOW);
}
break;
case BATTERY_WARNING:
if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) {
batteryState = BATTERY_CRITICAL;
beeper(BEEPER_BAT_CRIT_LOW);
} else if (vbat > batteryWarningVoltage) {
batteryState = BATTERY_OK;
} else {
beeper(BEEPER_BAT_LOW);
}
break;
case BATTERY_CRITICAL:
if (vbat > batteryCriticalVoltage) {
batteryState = BATTERY_WARNING;
beeper(BEEPER_BAT_LOW);
} else {
beeper(BEEPER_BAT_CRIT_LOW);
}
break;
case BATTERY_NOT_PRESENT:
break;
}
}
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);
}
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
}
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;
}
void batterySample(void)
{
static uint8_t ind;
static uint16_t batSamples[8];
uint16_t batAccum = 0;
uint8_t i;
batSamples[(ind++) % 8] = adcGet();
for(i = 0; i < 8; ++i)
batAccum += batSamples[i];
sensorData.batteryVoltage = batteryAdcToVoltage(batAccum / 8.0f);
// TODO - add buzzer stuff
/*
if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 0;
} else
buzzerFreq = 4; // low battery
*/
}
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
}
}
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
void annexCode(void)
{
static uint32_t calibratedAccTime;
uint16_t tmp, tmp2;
static uint8_t buzzerFreq; //delay between buzzer ring
static uint8_t vbatTimer = 0;
uint8_t axis, prop1, prop2;
static uint8_t ind = 0;
uint16_t vbatRaw = 0;
static uint16_t vbatRawArray[8];
uint8_t 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] - cfg.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;
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < cfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000);
tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.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] = adcGetBattery();
for (i = 0; i < 8; i++)
vbatRaw += vbatRawArray[i];
vbat = batteryAdcToVoltage(vbatRaw / 8);
}
if ((vbat > batteryWarningVoltage) || (vbat < cfg.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.
if (feature(FEATURE_TELEMETRY))
initTelemetry(f.ARMED);
}
#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;
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
}
}
void updateBattery(void)
{
updateBatteryVoltage();
/* battery has just been connected*/
if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD_MV)
{
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
batteryState = BATTERY_OK;
/* wait for VBatt to stabilise then we can calc number of cells
(using the filtered value takes a long time to ramp up)
We only do this on the ground so don't care if we do block, not
worse than original code anyway*/
delay(VBATTERY_STABLE_DELAY);
updateBatteryVoltage();
unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1;
if (cells > 8) {
// something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
cells = 8;
}
batteryCellCount = cells;
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
}
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD_MV */
else if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD_MV)
{
batteryState = BATTERY_NOT_PRESENT;
batteryCellCount = 0;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}
switch(batteryState)
{
case BATTERY_OK:
if (vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)) {
batteryState = BATTERY_WARNING;
beeper(BEEPER_BAT_LOW);
}
break;
case BATTERY_WARNING:
if (vbat <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) {
batteryState = BATTERY_CRITICAL;
beeper(BEEPER_BAT_CRIT_LOW);
} else if (vbat > (batteryWarningVoltage + VBATT_HYSTERESIS)){
batteryState = BATTERY_OK;
} else {
beeper(BEEPER_BAT_LOW);
}
break;
case BATTERY_CRITICAL:
if (vbat > (batteryCriticalVoltage + VBATT_HYSTERESIS)){
batteryState = BATTERY_WARNING;
beeper(BEEPER_BAT_LOW);
} else {
beeper(BEEPER_BAT_CRIT_LOW);
}
break;
case BATTERY_NOT_PRESENT:
break;
}
}
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
}
