bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs) { UNUSED(currentTimeUs); static uint8_t lineNo = 0; int lineCount = 0; // Skip already sent lines... while (lineSent[lineNo] && lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) { lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS; lineCount++; } sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU8(dst, lineNo); sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS); lineSent[lineNo] = true; lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS; // Always send something, Always one user frame after the two mandatory frames // I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true // too keep the "Waltz" sequence intact. return true; }
bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs) { uint16_t period_us = SPEKTRUM_RPM_UNUSED; #ifdef USE_ESC_SENSOR_TELEMETRY escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); if (escData != NULL) { period_us = 60000000 / escData->rpm; // revs/minute -> microSeconds } #endif int16_t coreTemp = SPEKTRUM_TEMP_UNUSED; #if defined(USE_ADC_INTERNAL) coreTemp = getCoreTemperatureCelsius(); coreTemp = coreTemp * 9 / 5 + 32; // C -> F #endif UNUSED(currentTimeUs); sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU16BigEndian(dst, period_us); // pulse leading edges if (telemetryConfig()->report_cell_voltage) { sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V } else { sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.01V } sbufWriteU16BigEndian(dst, coreTemp); // temperature sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT); // Mandatory frame, send it unconditionally. return true; }
bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs) { uint16_t amps = getAmperage() / 10; uint16_t mah = getMAhDrawn(); static uint16_t sentAmps; static uint16_t sentMah; static timeUs_t lastTimeSentFPmAh = 0; timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh; if ( amps != sentAmps || mah != sentMah || keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) { sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU16(dst, amps); sbufWriteU16(dst, mah); sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp A sbufWriteU16(dst, SPEKTRUM_AMPS_UNUSED); // Amps B sbufWriteU16(dst, SPEKTRUM_AMPH_UNUSED); // mAH B sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp B sbufFill(dst, STRU_TELE_FP_EMPTY_FIELDS_VALUE, STRU_TELE_FP_EMPTY_FIELDS_COUNT); sentAmps = amps; sentMah = mah; lastTimeSentFPmAh = currentTimeUs; return true; } return false; }
static void srxlInitializeFrame(sbuf_t *dst) { dst->ptr = srxlFrame; dst->end = ARRAYEND(srxlFrame); sbufWriteU8(dst, SRXL_ADDRESS_FIRST); sbufWriteU8(dst, SRXL_ADDRESS_SECOND); sbufWriteU8(dst, SRXL_PACKET_LENGTH); }
static void ltm_initialise_packet(sbuf_t *dst) { ltm_crc = 0; dst->ptr = ltmPayload; dst->end = ARRAYEND(ltmPayload); sbufWriteU8(dst, '$'); sbufWriteU8(dst, 'T'); }
bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs) { UNUSED(currentTimeUs); sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufFill(dst, STRU_TELE_QOS_EMPTY_FIELDS_VALUE, STRU_TELE_QOS_EMPTY_FIELDS_COUNT); // Clear remainder // Mandatory frame, send it unconditionally. return true; }
bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs) { UNUSED(currentTimeUs); gpsCoordinateDDDMMmmmm_t coordinate; uint32_t latitudeBcd, longitudeBcd, altitudeLo; uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint8_t hdopBcd, gpsFlags; if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { return false; } // lattitude GPStoDDDMM_MMMM(gpsSol.llh.lat, &coordinate); latitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm); // longitude GPStoDDDMM_MMMM(gpsSol.llh.lon, &coordinate); longitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm); // altitude (low order) altitudeLo = ABS(gpsSol.llh.altCm) / 10; altitudeLoBcd = dec2bcd(altitudeLo % 100000); // Ground course groundCourseBcd = dec2bcd(gpsSol.groundCourse); // HDOP hdop = gpsSol.hdop / 10; hdop = (hdop > 99) ? 99 : hdop; hdopBcd = dec2bcd(hdop); // flags gpsFlags = GPS_FLAGS_GPS_DATA_RECEIVED_BIT | GPS_FLAGS_GPS_FIX_VALID_BIT | GPS_FLAGS_3D_FIX_BIT; gpsFlags |= (gpsSol.llh.lat > 0) ? GPS_FLAGS_IS_NORTH_BIT : 0; gpsFlags |= (gpsSol.llh.lon > 0) ? GPS_FLAGS_IS_EAST_BIT : 0; gpsFlags |= (gpsSol.llh.altCm < 0) ? GPS_FLAGS_NEGATIVE_ALT_BIT : 0; gpsFlags |= (gpsSol.llh.lon / GPS_DEGREES_DIVIDER > 99) ? GPS_FLAGS_LONGITUDE_GREATER_99_BIT : 0; // SRXL frame sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_LOC); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU16(dst, altitudeLoBcd); sbufWriteU32(dst, latitudeBcd); sbufWriteU32(dst, longitudeBcd); sbufWriteU16(dst, groundCourseBcd); sbufWriteU8(dst, hdopBcd); sbufWriteU8(dst, gpsFlags); return true; }
bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs) { UNUSED(currentTimeUs); uint32_t timeBcd; uint16_t speedKnotsBcd, speedTmp; uint8_t numSatBcd, altitudeHighBcd; bool timeProvided = false; if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { return false; } // Number of sats and altitude (high bits) numSatBcd = (gpsSol.numSat > 99) ? dec2bcd(99) : dec2bcd(gpsSol.numSat); altitudeHighBcd = dec2bcd(gpsSol.llh.altCm / 100000); // Speed (knots) speedTmp = gpsSol.groundSpeed * 1944 / 1000; speedKnotsBcd = (speedTmp > 9999) ? dec2bcd(9999) : dec2bcd(speedTmp); #ifdef USE_RTC_TIME dateTime_t dt; // RTC if (rtcHasTime()) { rtcGetDateTime(&dt); timeBcd = dec2bcd(dt.hours); timeBcd = timeBcd << 8; timeBcd = timeBcd | dec2bcd(dt.minutes); timeBcd = timeBcd << 8; timeBcd = timeBcd | dec2bcd(dt.seconds); timeBcd = timeBcd << 4; timeBcd = timeBcd | dec2bcd(dt.millis / 100); timeProvided = true; } #endif timeBcd = (timeProvided) ? timeBcd : SPEKTRUM_TIME_UNKNOWN; // SRXL frame sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_STAT); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU16(dst, speedKnotsBcd); sbufWriteU32(dst, timeBcd); sbufWriteU8(dst, numSatBcd); sbufWriteU8(dst, altitudeHighBcd); sbufFill(dst, STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE, STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT); return true; }
/* * GPS G-frame 5Hhz at > 2400 baud * LAT LON SPD ALT SAT/FIX */ void ltm_gframe(sbuf_t *dst) { uint8_t gps_fix_type = 0; int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0; if (sensors(SENSOR_GPS)) { if (gpsSol.fixType == GPS_NO_FIX) gps_fix_type = 1; else if (gpsSol.fixType == GPS_FIX_2D) gps_fix_type = 2; else if (gpsSol.fixType == GPS_FIX_3D) gps_fix_type = 3; ltm_lat = gpsSol.llh.lat; ltm_lon = gpsSol.llh.lon; ltm_gs = gpsSol.groundSpeed / 100; } #if defined(NAV) ltm_alt = getEstimatedActualPosition(Z); // cm #else ltm_alt = sensors(SENSOR_GPS) ? gpsSol.llh.alt : 0; // cm #endif sbufWriteU8(dst, 'G'); ltm_serialise_32(dst, ltm_lat); ltm_serialise_32(dst, ltm_lon); ltm_serialise_8(dst, (uint8_t)ltm_gs); ltm_serialise_32(dst, ltm_alt); ltm_serialise_8(dst, (gpsSol.numSat << 2) | gps_fix_type); }
/* * Attitude A-frame - 10 Hz at > 2400 baud * PITCH ROLL HEADING */ void ltm_aframe(sbuf_t *dst) { sbufWriteU8(dst, 'A'); ltm_serialise_16(dst, DECIDEGREES_TO_DEGREES(attitude.values.pitch)); ltm_serialise_16(dst, DECIDEGREES_TO_DEGREES(attitude.values.roll)); ltm_serialise_16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); }
void ltm_sframe(sbuf_t *dst) { uint8_t lt_flightmode; if (FLIGHT_MODE(PASSTHRU_MODE)) lt_flightmode = 0; else if (FLIGHT_MODE(NAV_WP_MODE)) lt_flightmode = 10; else if (FLIGHT_MODE(NAV_RTH_MODE)) lt_flightmode = 13; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) lt_flightmode = 9; else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE)) lt_flightmode = 11; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) lt_flightmode = 8; else if (FLIGHT_MODE(ANGLE_MODE)) lt_flightmode = 2; else if (FLIGHT_MODE(HORIZON_MODE)) lt_flightmode = 3; else lt_flightmode = 1; // Rate mode uint8_t lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0; if (failsafeIsActive()) lt_statemode |= 2; sbufWriteU8(dst, 'S'); ltm_serialise_16(dst, vbat * 100); //vbat converted to mv ltm_serialise_16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // current mAh (65535 mAh max) ltm_serialise_8(dst, (uint8_t)((rssi * 254) / 1023)); // scaled RSSI (uchar) ltm_serialise_8(dst, 0); // no airspeed ltm_serialise_8(dst, (lt_flightmode << 2) | lt_statemode); }
static void crsfInitializeFrame(sbuf_t *dst) { crsfCrc = 0; dst->ptr = crsfFrame; dst->end = ARRAYEND(crsfFrame); sbufWriteU8(dst, CRSF_TELEMETRY_SYNC_BYTE); }
static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs) { static timeUs_t lastTimeSentVtx = 0; static spektrumVtx_t vtxSent; spektrumVtx_t vtx; collectVtxTmData(&vtx); if ((vtxDeviceType != VTXDEV_UNKNOWN) && vtxDeviceType != VTXDEV_UNSUPPORTED) { convertVtxTmData(&vtx); if ((memcmp(&vtxSent, &vtx, sizeof(spektrumVtx_t)) != 0) || ((currentTimeUs - lastTimeSentVtx) > VTX_KEEPALIVE_TIME_OUT) ) { // Fill in the VTX tm structure sbufWriteU8(dst, TELE_DEVICE_VTX); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU8(dst, vtx.band); sbufWriteU8(dst, vtx.channel); sbufWriteU8(dst, vtx.pitMode); sbufWriteU8(dst, vtx.power); sbufWriteU16(dst, vtx.powerValue); sbufWriteU8(dst, vtx.region); sbufFill(dst, STRU_TELE_VTX_EMPTY_VALUE, STRU_TELE_VTX_EMPTY_COUNT); memcpy(&vtxSent, &vtx, sizeof(spektrumVtx_t)); lastTimeSentVtx = currentTimeUs; return true; } } return false; }
void crc8_dvb_s2_sbuf_append(sbuf_t *dst, uint8_t *start) { uint8_t crc = 0; const uint8_t * const end = dst->ptr; for (const uint8_t *ptr = start; ptr < end; ++ptr) { crc = crc8_dvb_s2(crc, *ptr); } sbufWriteU8(dst, crc); }
void crc8_xor_sbuf_append(sbuf_t *dst, uint8_t *start) { uint8_t crc = 0; const uint8_t *end = dst->ptr; for (uint8_t *ptr = start; ptr < end; ++ptr) { crc ^= *ptr; } sbufWriteU8(dst, crc); }
/* * OSD additional data frame, 1 Hz rate * This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD * home pos, home alt, direction to home */ void ltm_oframe(sbuf_t *dst) { sbufWriteU8(dst, 'O'); ltm_serialise_32(dst, GPS_home.lat); ltm_serialise_32(dst, GPS_home.lon); ltm_serialise_32(dst, GPS_home.alt); ltm_serialise_8(dst, 1); // OSD always ON ltm_serialise_8(dst, STATE(GPS_FIX_HOME) ? 1 : 0); }
void sendMspErrorResponse(uint8_t error, int16_t cmd) { mspPackage.responsePacket->cmd = cmd; mspPackage.responsePacket->result = 0; mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; sbufWriteU8(&mspPackage.responsePacket->buf, error); mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR; sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); }
/** OSD additional data frame, ~4 Hz rate, navigation system status */ void ltm_nframe(sbuf_t *dst) { sbufWriteU8(dst, 'N'); ltm_serialise_8(dst, NAV_Status.mode); ltm_serialise_8(dst, NAV_Status.state); ltm_serialise_8(dst, NAV_Status.activeWpAction); ltm_serialise_8(dst, NAV_Status.activeWpNumber); ltm_serialise_8(dst, NAV_Status.error); ltm_serialise_8(dst, NAV_Status.flags); }
/* * Extended information data frame, 1 Hz rate * This frame is intended to report extended GPS and NAV data, however at the moment it contains only HDOP value */ void ltm_xframe(sbuf_t *dst) { uint8_t sensorStatus = (isHardwareHealthy() ? 0 : 1) << 0; // bit 0 - hardware failure indication (1 - something is wrong with the hardware sensors) sbufWriteU8(dst, 'X'); ltm_serialise_16(dst, gpsSol.hdop); ltm_serialise_8(dst, sensorStatus); ltm_serialise_8(dst, 0); ltm_serialise_8(dst, 0); ltm_serialise_8(dst, 0); }
static void processMspPacket(void) { mspPackage.responsePacket->cmd = 0; mspPackage.responsePacket->result = 0; mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; mspPostProcessFnPtr mspPostProcessFn = NULL; if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR); } if (mspPostProcessFn) { mspPostProcessFn(NULL); } sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); }
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) { static uint8_t checksum = 0; static uint8_t seq = 0; uint8_t payloadOut[payloadSize]; sbuf_t payload; sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize); sbuf_t *txBuf = &mspPackage.responsePacket->buf; // detect first reply packet if (txBuf->ptr == mspPackage.responseBuffer) { // header uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK); if (mspPackage.responsePacket->result < 0) { head |= TELEMETRY_MSP_ERROR_FLAG; } sbufWriteU8(payloadBuf, head); uint8_t size = sbufBytesRemaining(txBuf); sbufWriteU8(payloadBuf, size); } else { // header sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK)); } const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); uint8_t frame[payloadBytesRemaining]; if (bufferBytesRemaining >= payloadBytesRemaining) { sbufReadData(txBuf, frame, payloadBytesRemaining); sbufAdvance(txBuf, payloadBytesRemaining); sbufWriteData(payloadBuf, frame, payloadBytesRemaining); responseFn(payloadOut); return true; } else { sbufReadData(txBuf, frame, bufferBytesRemaining); sbufAdvance(txBuf, bufferBytesRemaining); sbufWriteData(payloadBuf, frame, bufferBytesRemaining); sbufSwitchToReader(txBuf, mspPackage.responseBuffer); checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd; while (sbufBytesRemaining(txBuf)) { checksum ^= sbufReadU8(txBuf); } sbufWriteU8(payloadBuf, checksum); while (sbufBytesRemaining(payloadBuf)>1) { sbufWriteU8(payloadBuf, 0); } } responseFn(payloadOut); return false; }
static void crsfSerialize8(sbuf_t *dst, uint8_t v) { sbufWriteU8(dst, v); crsfCrc = crc8_dvb_s2(crsfCrc, v); }
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 }
static void ltm_serialise_8(sbuf_t *dst, uint8_t v) { sbufWriteU8(dst, v); ltm_crc ^= v; }
static void ltm_finalise(sbuf_t *dst) { sbufWriteU8(dst, ltm_crc); sbufSwitchToReader(dst, ltmPayload); serialWriteBuf(ltmPort, sbufPtr(dst), sbufBytesRemaining(dst)); }