Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Merge pull request #2274 from fiam/mspv2_settings

Browse source 
Add support for getting and setting arbitrary settings via MSPv2
This commit is contained in:
Konstantin Sharlaimov 2017-10-12 18:52:18 +10:00 • committed by GitHub
commit 773636ad2d
7 changed files with 237 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

129
src/main/fc/fc_msp.c
View file

@ -54,6 +54,7 @@
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
@ -1976,6 +1977,130 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ACK;
}
static const setting_t *mspReadSettingName(sbuf_t *src)
{
char name[SETTING_MAX_NAME_LENGTH];
uint8_t c;
size_t s = 0;
while (1) {
if (!sbufReadU8Safe(&c, src)) {
return NULL;
}
name[s++] = c;
if (c == '\0') {
break;
}
if (s == SETTING_MAX_NAME_LENGTH) {
// Name is too long
return NULL;
}
}
return setting_find(name);
}
static bool mspSettingCommand(sbuf_t *dst, sbuf_t *src)
{
const setting_t *setting = mspReadSettingName(src);
if (!setting) {
return false;
}
const void *ptr = setting_get_value_pointer(setting);
size_t size = setting_get_value_size(setting);
sbufWriteDataSafe(dst, ptr, size);
return true;
}
static bool mspSetSettingCommand(sbuf_t *dst, sbuf_t *src)
{
UNUSED(dst);
const setting_t *setting = mspReadSettingName(src);
if (!setting) {
return false;
}
setting_min_t min = setting_get_min(setting);
setting_max_t max = setting_get_max(setting);
void *ptr = setting_get_value_pointer(setting);
switch (SETTING_TYPE(setting)) {
case VAR_UINT8:
{
uint8_t val;
if (!sbufReadU8Safe(&val, src)) {
return false;
}
if (val > max) {
return false;
}
*((uint8_t*)ptr) = val;
}
break;
case VAR_INT8:
{
int8_t val;
if (!sbufReadI8Safe(&val, src)) {
return false;
}
if (val < min || val > (int8_t)max) {
return false;
}
*((int8_t*)ptr) = val;
}
break;
case VAR_UINT16:
{
uint16_t val;
if (!sbufReadU16Safe(&val, src)) {
return false;
}
if (val > max) {
return false;
}
*((uint16_t*)ptr) = val;
}
break;
case VAR_INT16:
{
int16_t val;
if (!sbufReadI16Safe(&val, src)) {
return false;
}
if (val < min || val > (int16_t)max) {
return false;
}
*((int16_t*)ptr) = val;
}
break;
case VAR_UINT32:
{
uint32_t val;
if (!sbufReadU32Safe(&val, src)) {
return false;
}
if (val > max) {
return false;
}
*((uint32_t*)ptr) = val;
}
break;
case VAR_FLOAT:
{
float val;
if (!sbufReadDataSafe(src, &val, sizeof(float))) {
return false;
}
if (val < (float)min || val > (float)max) {
return false;
}
*((float*)ptr) = val;
}
break;
}
return true;
}
/*
* Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
*/
@ -2005,6 +2130,10 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
mspFcDataFlashReadCommand(dst, src);
ret = MSP_RESULT_ACK;
#endif
} else if (cmdMSP == MSP2_COMMON_SETTING) {
ret = mspSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
} else if (cmdMSP == MSP2_COMMON_SET_SETTING) {
ret = mspSetSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
} else {
ret = mspFcProcessInCommand(cmdMSP, src);
}