Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

Merge pull request #9332 from fiam/agh_cf_serial_config

Browse source 
[MSP] Add support for MSP2_COMMON_SERIAL_CONFIG / MSP2_COMMON_SET_SERIAL_CONFIG
This commit is contained in:
Michael Keller 2020-01-26 12:44:00 +13:00 • committed by GitHub
commit 3e752af4ac
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 82 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

66
src/main/msp/msp.c
View file

@ -103,6 +103,7 @@
#include "msp/msp_box.h"
#include "msp/msp_protocol.h"
#include "msp/msp_protocol_v2_common.h"
#include "msp/msp_serial.h"
#include "osd/osd.h"
@ -557,7 +558,7 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin
* Returns true if the command was processd, false otherwise.
* May set mspPostProcessFunc to a function to be called once the command has been processed
*/
static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{
UNUSED(mspPostProcessFn);
@ -959,7 +960,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
return true;
}
static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
{
bool unsupportedCommand = false;
@ -1478,6 +1479,27 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
}
break;
case MSP2_COMMON_SERIAL_CONFIG: {
uint8_t count = 0;
for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
if (serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
count++;
}
}
sbufWriteU8(dst, count);
for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
continue;
};
sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
sbufWriteU32(dst, serialConfig()->portConfigs[i].functionMask);
sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
}
break;
}
#ifdef USE_LED_STRIP_STATUS_MODE
case MSP_LED_COLORS:
@ -1853,7 +1875,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
return !unsupportedCommand;
}
static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{
switch (cmdMSP) {
@ -2039,7 +2061,7 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
}
#endif
static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src)
static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src)
{
uint32_t i;
uint8_t value;
@ -3048,6 +3070,38 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP,
}
}
break;
case MSP2_COMMON_SET_SERIAL_CONFIG: {
if (dataSize < 1) {
return MSP_RESULT_ERROR;
}
unsigned count = sbufReadU8(src);
unsigned portConfigSize = (dataSize - 1) / count;
unsigned expectedPortSize = sizeof(uint8_t) + sizeof(uint32_t) + (sizeof(uint8_t) * 4);
if (portConfigSize < expectedPortSize) {
return MSP_RESULT_ERROR;
}
for (unsigned ii = 0; ii < count; ii++) {
unsigned start = sbufBytesRemaining(src);
uint8_t identifier = sbufReadU8(src);
serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier);
if (!portConfig) {
return MSP_RESULT_ERROR;
}
portConfig->identifier = identifier;
portConfig->functionMask = sbufReadU32(src);
portConfig->msp_baudrateIndex = sbufReadU8(src);
portConfig->gps_baudrateIndex = sbufReadU8(src);
portConfig->telemetry_baudrateIndex = sbufReadU8(src);
portConfig->blackbox_baudrateIndex = sbufReadU8(src);
// Skip unknown bytes
while (start - sbufBytesRemaining(src) < portConfigSize && sbufBytesRemaining(src)) {
sbufReadU8(src);
}
}
break;
}
#ifdef USE_LED_STRIP_STATUS_MODE
case MSP_SET_LED_COLORS:
@ -3170,7 +3224,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP,
return MSP_RESULT_ACK;
}
static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
{
UNUSED(mspPostProcessFn);
const unsigned int dataSize = sbufBytesRemaining(src);
@ -3425,7 +3479,7 @@ mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPa
int ret = MSP_RESULT_ACK;
sbuf_t *dst = &reply->buf;
sbuf_t *src = &cmd->buf;
const uint8_t cmdMSP = cmd->cmd;
const int16_t cmdMSP = cmd->cmd;
// initialize reply by default
reply->cmd = cmd->cmd;

22
src/main/msp/msp_protocol_v2_common.h Normal file
View file

@ -0,0 +1,22 @@
/*
* This file is part of Cleanflight, Betaflight and INAV.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight, Betaflight and INAV are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#define MSP2_COMMON_SERIAL_CONFIG 0x1009
#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A