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