void osdSetFontCharacter(uint8_t address, sbuf_t *src)
{
if (sbufBytesRemaining(src) != MAX7456_CHARACTER_BUFFER_SIZE) {
return;
}
uint8_t characterBitmap[MAX7456_CHARACTER_BUFFER_SIZE];
for (int i = 0; i < MAX7456_CHARACTER_BUFFER_SIZE; i++) {
characterBitmap[i] = sbufReadU8(src);
}
max7456_setFontCharacter(address, characterBitmap);
}
// return positive for ACK, negative on error
int mspClientReplyHandler(mspPacket_t *reply)
{
sbuf_t * src = &reply->buf;
int len = sbufBytesRemaining(src);
mspClientStatus.lastReplyAt = micros();
UNUSED(len);
switch (reply->cmd) {
case MSP_STATUS:
fcStatus.cycleTime = sbufReadU16(src);
fcStatus.i2cErrors = sbufReadU16(src);
fcStatus.sensors = sbufReadU16(src);
fcStatus.fcState = sbufReadU32(src);
fcStatus.profile = sbufReadU8(src);
break;
case MSP_ANALOG:
fcStatus.vbat = sbufReadU8(src);
fcStatus.mAhDrawn = sbufReadU16(src);
fcStatus.rssi = scaleRange(sbufReadU16(src), 0, 1023, 0, 1000);
fcStatus.amperage = sbufReadU16(src);
break;
case MSP_MOTOR:
for (unsigned i = 0; i < 8 && i < OSD_MAX_MOTORS; i++) {
fcMotors[i] = sbufReadU16(src);
}
break;
default:
// we do not know how to handle the message
return -1;
}
return 1;
}
int getLtmFrame(uint8_t *frame, ltm_frame_e ltmFrameType)
{
static uint8_t ltmPayload[LTM_MAX_MESSAGE_SIZE];
sbuf_t ltmPayloadBuf = { .ptr = ltmPayload, .end =ARRAYEND(ltmPayload) };
sbuf_t * const sbuf = <mPayloadBuf;
switch (ltmFrameType) {
default:
case LTM_AFRAME:
ltm_aframe(sbuf);
break;
case LTM_SFRAME:
ltm_sframe(sbuf);
break;
#if defined(GPS)
case LTM_GFRAME:
ltm_gframe(sbuf);
break;
case LTM_OFRAME:
ltm_oframe(sbuf);
break;
case LTM_XFRAME:
ltm_xframe(sbuf);
break;
#endif
#if defined(NAV)
case LTM_NFRAME:
ltm_nframe(sbuf);
break;
#endif
}
sbufSwitchToReader(sbuf, ltmPayload);
const int frameSize = sbufBytesRemaining(sbuf);
for (int ii = 0; sbufBytesRemaining(sbuf); ++ii) {
frame[ii] = sbufReadU8(sbuf);
}
return frameSize;
}
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
}
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;
}
bool handleMspFrame(uint8_t *frameStart, int frameLength)
{
static uint8_t mspStarted = 0;
static uint8_t lastSeq = 0;
if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
mspStarted = 0;
}
if (mspStarted == 0) {
initSharedMsp();
}
mspPacket_t *packet = mspPackage.requestPacket;
sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameStart + (uint8_t)frameLength);
sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
const uint8_t header = sbufReadU8(frameBuf);
const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT;
if (version != TELEMETRY_MSP_VERSION) {
sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
return true;
}
if (header & TELEMETRY_MSP_START_FLAG) {
// first packet in sequence
uint8_t mspPayloadSize = sbufReadU8(frameBuf);
packet->cmd = sbufReadU8(frameBuf);
packet->result = 0;
packet->buf.ptr = mspPackage.requestBuffer;
packet->buf.end = mspPackage.requestBuffer + mspPayloadSize;
checksum = mspPayloadSize ^ packet->cmd;
mspStarted = 1;
} else if (!mspStarted) {
// no start packet yet, throw this one away
return false;
} else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) {
// packet loss detected!
mspStarted = 0;
return false;
}
const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf);
const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf);
uint8_t payload[frameBytesRemaining];
if (bufferBytesRemaining >= frameBytesRemaining) {
sbufReadData(frameBuf, payload, frameBytesRemaining);
sbufAdvance(frameBuf, frameBytesRemaining);
sbufWriteData(rxBuf, payload, frameBytesRemaining);
lastSeq = seqNumber;
return false;
} else {
sbufReadData(frameBuf, payload, bufferBytesRemaining);
sbufAdvance(frameBuf, bufferBytesRemaining);
sbufWriteData(rxBuf, payload, bufferBytesRemaining);
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
while (sbufBytesRemaining(rxBuf)) {
checksum ^= sbufReadU8(rxBuf);
}
if (checksum != *frameBuf->ptr) {
mspStarted = 0;
sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd);
return true;
}
}
mspStarted = 0;
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
processMspPacket();
return true;
}