Exemple #1
0
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;
}
Exemple #3
0
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 = &ltmPayloadBuf;

    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
}
Exemple #5
0
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;
}
Exemple #6
0
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;
}