rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3166 from ledvinap/improvement-box-serialization-bf 

Improve box serialization
This commit is contained in:
Michael Keller 2017-05-29 21:18:30 +12:00 committed by GitHub
parent 7a821b7bc4 7582cf86b4
commit 3042e58957
5 changed files with 126 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/common/streambuf.c
View File

@ -62,7 +62,7 @@ void sbufWriteData(sbuf_t *dst, const void *data, int len)
void sbufWriteString(sbuf_t *dst, const char *string)
{
sbufWriteData(dst, string, strlen(string) + 1); // include zero terminator
sbufWriteData(dst, string, strlen(string));
}
uint8_t sbufReadU8(sbuf_t *src)

213
src/main/fc/fc_msp.c
View File

@ -109,11 +109,14 @@
#include "hardware_revision.h"
#endif
#define STATIC_ASSERT(condition, name) \
typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
#ifndef USE_OSD_SLAVE
static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOXARM, "ARM", 0 },
{ BOXANGLE, "ANGLE", 1 },
{ BOXHORIZON, "HORIZON", 2 },
@ -146,13 +149,12 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
// this is calculated at startup based on enabled features.
static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
// this is the number of filled indexes in above array
static uint8_t activeBoxIdCount = 0;
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
static uint32_t activeBoxIds;
// check that all boxId_e values fit
STATIC_ASSERT(sizeof(activeBoxIds) * 8 >= CHECKBOX_ITEM_COUNT, CHECKBOX_ITEMS_wont_fit_in_activeBoxIds);
static const char pidnames[] =
"ROLL;"
@ -254,188 +256,207 @@ static void mspRebootFn(serialPort_t *serialPort)
}
#ifndef USE_OSD_SLAVE
const box_t *findBoxByBoxId(uint8_t boxId)
const box_t *findBoxByBoxId(boxId_e boxId)
{
for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
const box_t *candidate = &boxes[boxIndex];
if (candidate->boxId == boxId) {
for (unsigned i = 0; i < ARRAYLEN(boxes); i++) {
const box_t *candidate = &boxes[i];
if (candidate->boxId == boxId)
return candidate;
}
}
return NULL;
}
const box_t *findBoxByPermanentId(uint8_t permenantId)
const box_t *findBoxByPermanentId(uint8_t permanentId)
{
for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
const box_t *candidate = &boxes[boxIndex];
if (candidate->permanentId == permenantId) {
for (unsigned i = 0; i < ARRAYLEN(boxes); i++) {
const box_t *candidate = &boxes[i];
if (candidate->permanentId == permanentId)
return candidate;
}
}
return NULL;
}
static void serializeBoxNamesReply(sbuf_t *dst)
{
for (int i = 0; i < activeBoxIdCount; i++) {
const int activeBoxId = activeBoxIds[i];
const box_t *box = findBoxByBoxId(activeBoxId);
if (!box) {
continue;
for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) {
if(activeBoxIds & (1 << id)) {
const box_t *box = findBoxByBoxId(id);
sbufWriteString(dst, box->boxName);
sbufWriteU8(dst, ';');
}
}
}
sbufWriteData(dst, box->boxName, strlen(box->boxName));
sbufWriteU8(dst, ';');
static void serializeBoxIdsReply(sbuf_t *dst)
{
for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) {
if(activeBoxIds & (1 << id)) {
const box_t *box = findBoxByBoxId(id);
sbufWriteU8(dst, box->permanentId);
}
}
}
void initActiveBoxIds(void)
{
// calculate used boxes based on features and fill availableBoxes[] array
memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
activeBoxIdCount = 0;
activeBoxIds[activeBoxIdCount++] = BOXARM;
// calculate used boxes based on features and set corresponding activeBoxIds bits
uint32_t ena = 0; // temporary variable to collect result
// macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only
#define BME(boxId) do { ena |= (1 << (boxId)); } while(0)
BME(BOXARM);
if (!feature(FEATURE_AIRMODE)) {
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
BME(BOXAIRMODE);
}
if (!feature(FEATURE_ANTI_GRAVITY)) {
activeBoxIds[activeBoxIdCount++] = BOXANTIGRAVITY;
BME(BOXANTIGRAVITY);
}
if (sensors(SENSOR_ACC)) {
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
BME(BOXANGLE);
BME(BOXHORIZON);
BME(BOXHEADFREE);
}
#ifdef BARO
if (sensors(SENSOR_BARO)) {
activeBoxIds[activeBoxIdCount++] = BOXBARO;
BME(BOXBARO);
}
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
activeBoxIds[activeBoxIdCount++] = BOXMAG;
activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
BME(BOXMAG);
BME(BOXHEADADJ);
}
#endif
#ifdef GPS
if (feature(FEATURE_GPS)) {
activeBoxIds[activeBoxIdCount++] = BOXGPSHOME;
activeBoxIds[activeBoxIdCount++] = BOXGPSHOLD;
BME(BOXGPSHOME);
BME(BOXGPSHOLD);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
activeBoxIds[activeBoxIdCount++] = BOXSONAR;
BME(BOXSONAR);
}
#endif
if (feature(FEATURE_FAILSAFE)) {
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
BME(BOXFAILSAFE);
}
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
BME(BOXPASSTHRU);
}
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
BME(BOXBEEPERON);
#ifdef LED_STRIP
if (feature(FEATURE_LED_STRIP)) {
activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
BME(BOXLEDLOW);
}
#endif
#ifdef BLACKBOX
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
BME(BOXBLACKBOX);
#ifdef USE_FLASHFS
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE;
BME(BOXBLACKBOXERASE);
#endif
#endif
activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
BME(BOXFPVANGLEMIX);
if (feature(FEATURE_3D)) {
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
BME(BOX3DDISABLESWITCH);
}
if (feature(FEATURE_SERVO_TILT)) {
activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
BME(BOXCAMSTAB);
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
activeBoxIds[activeBoxIdCount++] = BOXCALIB;
BME(BOXCALIB);
}
activeBoxIds[activeBoxIdCount++] = BOXOSD;
BME(BOXOSD);
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch) {
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
BME(BOXTELEMETRY);
}
#endif
#ifdef USE_SERVOS
if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
BME(BOXSERVO1);
BME(BOXSERVO2);
BME(BOXSERVO3);
}
#endif
}
#define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
#undef BME
// check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
for(boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++)
if((ena & (1 << boxId))
&& findBoxByBoxId(boxId) == NULL)
ena &= ~ (1 << boxId); // this should not happen, but handle it gracefully
activeBoxIds = ena; // set global variable
}
static uint32_t packFlightModeFlags(void)
{
// Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
// Requires new Multiwii protocol version to fix
// It would be preferable to setting the enabled bits based on BOXINDEX.
const uint32_t tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO |
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG |
IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME |
IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD |
IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) << BOXBLACKBOXERASE |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY)) << BOXANTIGRAVITY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX;
uint32_t ret = 0;
for (int i = 0; i < activeBoxIdCount; i++) {
const uint32_t flag = (tmp & (1 << activeBoxIds[i]));
if (flag) {
ret |= 1 << i;
uint32_t boxEnabledMask = 0; // enabled BOXes, bits indexed by boxId_e
// enable BOXes dependent on FLIGHT_MODE, use mapping table (from runtime_config.h)
// flightMode_boxId_map[HORIZON_MODE] == BOXHORIZON
static const int8_t flightMode_boxId_map[] = FLIGHT_MODE_BOXID_MAP_INITIALIZER;
flightModeFlags_e flightModeCopyMask = ~0; // only modes with bit set will be copied
for(unsigned i = 0; i < ARRAYLEN(flightMode_boxId_map); i++) {
if(flightMode_boxId_map[i] != -1 // boxId_e does exist for this FLIGHT_MODE
&& (flightModeCopyMask & (1 << i)) // this flightmode is copy is enabled
&& FLIGHT_MODE(1 << i)) { // this flightmode is active
boxEnabledMask |= (1 << flightMode_boxId_map[i]);
}
}
return ret;
// enable BOXes dependent on rcMode bits, indexes are the same.
// only subset of BOXes depend on rcMode, use mask to select them
#define BM(x) (1 << (x))
const uint32_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON)
| BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD)
| BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE)
| BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX);
for(unsigned i = 0; i < sizeof(rcModeCopyMask) * 8; i++) {
if((rcModeCopyMask & BM(i)) // mode copy is enabled
&& IS_RC_MODE_ACTIVE(i)) { // mode is active
boxEnabledMask |= (1 << i);
}
}
#undef BM
// copy ARM state
if(ARMING_FLAG(ARMED))
boxEnabledMask |= (1 << BOXARM);
// map boxId_e enabled bits to MSP status indexes
// only active boxIds are sent in status over MSP, other bits are not counted
uint32_t mspBoxEnabledMask = 0;
unsigned mspBoxIdx = 0; // index of active boxId (matches sent permanentId and boxNames)
for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) {
if(activeBoxIds & (1 << boxId)) {
if (boxEnabledMask & (1 << boxId))
mspBoxEnabledMask |= 1 << mspBoxIdx; // box is enabled
mspBoxIdx++; // box is active, count it
}
}
return mspBoxEnabledMask;
}
static void serializeSDCardSummaryReply(sbuf_t *dst)
@ -1029,13 +1050,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_BOXIDS:
for (int i = 0; i < activeBoxIdCount; i++) {
const box_t *box = findBoxByBoxId(activeBoxIds[i]);
if (!box) {
continue;
}
sbufWriteU8(dst, box->permanentId);
}
serializeBoxIdsReply(dst);
break;
case MSP_MOTOR_CONFIG:

2
src/main/fc/fc_msp.h
View File

@ -26,7 +26,7 @@ typedef struct box_e {
const uint8_t permanentId; //
} box_t;
const box_t *findBoxByBoxId(uint8_t boxId);
const box_t *findBoxByBoxId(boxId_e boxId);
const box_t *findBoxByPermanentId(uint8_t permenantId);
void mspFcInit(void);

8
src/main/fc/runtime_config.h
View File

@ -51,6 +51,14 @@ extern uint16_t flightModeFlags;
#define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask)
#define FLIGHT_MODE(mask) (flightModeFlags & (mask))
// macro to initialize map from flightModeFlags to boxId_e. Keep it in sync with flightModeFlags_e enum.
// Each boxId_e is at index of flightModeFlags_e bit, value is -1 if boxId_e does not exist.
// It is much more memory efficient than full map (uint32_t -> uint8_t)
#define FLIGHT_MODE_BOXID_MAP_INITIALIZER { \
BOXANGLE, BOXHORIZON, BOXMAG, BOXBARO, BOXGPSHOME, BOXGPSHOLD, \
BOXHEADFREE, -1, BOXPASSTHRU, BOXSONAR, BOXFAILSAFE} \
/**/
typedef enum {
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),

3
src/main/telemetry/crsf.c
View File

@ -204,7 +204,7 @@ void crsfFrameAttitude(sbuf_t *dst)
/*
0x21 Flight mode text based
Payload:
char[] Flight mode ( Null­terminated string )
char[] Flight mode ( Null­terminated string )
*/
void crsfFrameFlightMode(sbuf_t *dst)
{
@ -226,6 +226,7 @@ void crsfFrameFlightMode(sbuf_t *dst)
flightMode = "HOR";
}
sbufWriteString(dst, flightMode);
sbufWriteU8(dst, '\0'); // zero-terminate string
// write in the frame length
*lengthPtr = sbufPtr(dst) - lengthPtr;
}