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

Merge branch 'master' of https://github.com/borisbstyle/betaflight into sharper_rc_response

This commit is contained in:
rav 2017-05-04 19:48:22 +02:00
parent 33043d79dc 61d66c8a88
commit 10b88ed145
42 changed files with 707 additions and 282 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
src/main/blackbox/blackbox.c
View File

@ -449,6 +449,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_NEVER:
return false;
default:
return false;
}
@ -456,11 +457,8 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
static void blackboxBuildConditionCache(void)
{
FlightLogFieldCondition cond;
blackboxConditionCache = 0;
for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
for (FlightLogFieldCondition cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
if (testBlackboxConditionUncached(cond)) {
blackboxConditionCache |= 1 << cond;
}
@ -765,10 +763,10 @@ static void loadSlowState(blackboxSlowState_t *slow)
* If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations
* since the field was last logged.
*/
static void writeSlowFrameIfNeeded(bool allowPeriodicWrite)
static bool writeSlowFrameIfNeeded(void)
{
// Write the slow frame peridocially so it can be recovered if we ever lose sync
bool shouldWrite = allowPeriodicWrite && blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
bool shouldWrite = blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
if (shouldWrite) {
loadSlowState(&slowHistory);
@ -788,15 +786,7 @@ static void writeSlowFrameIfNeeded(bool allowPeriodicWrite)
if (shouldWrite) {
writeSlowFrame();
}
}
static int gcd(int num, int denom)
{
if (denom == 0) {
return num;
}
return gcd(denom, num % denom);
return shouldWrite;
}
void blackboxValidateConfig(void)
@ -832,6 +822,13 @@ void blackboxValidateConfig(void)
}
}
static void blackboxResetIterationTimers(void)
{
blackboxIteration = 0;
blackboxPFrameIndex = 0;
blackboxIFrameIndex = 0;
}
/**
* Start Blackbox logging if it is not already running. Intended to be called upon arming.
*/
@ -861,11 +858,9 @@ static void blackboxStart(void)
*/
blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX);
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX);
blackboxIteration = 0;
blackboxPFrameIndex = 0;
blackboxIFrameIndex = 0;
blackboxResetIterationTimers();
/*
* Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
@ -1011,50 +1006,36 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
static void loadMainState(timeUs_t currentTimeUs)
{
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
int i;
blackboxCurrent->time = currentTimeUs;
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->axisPID_P[i] = axisPID_P[i];
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->axisPID_I[i] = axisPID_I[i];
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->axisPID_D[i] = axisPID_D[i];
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
#ifdef MAG
blackboxCurrent->magADC[i] = mag.magADC[i];
#endif
}
for (i = 0; i < 4; i++) {
for (int i = 0; i < 4; i++) {
blackboxCurrent->rcCommand[i] = rcCommand[i];
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
}
for (i = 0; i < 4; i++) {
for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
blackboxCurrent->debug[i] = debug[i];
}
const int motorCount = getMotorCount();
for (i = 0; i < motorCount; i++) {
for (int i = 0; i < motorCount; i++) {
blackboxCurrent->motor[i] = motor[i];
}
blackboxCurrent->vbatLatest = getBatteryVoltageLatest();
blackboxCurrent->amperageLatest = getAmperageLatest();
#ifdef MAG
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->magADC[i] = mag.magADC[i];
}
#endif
#ifdef BARO
blackboxCurrent->BaroAlt = baro.BaroAlt;
#endif
@ -1425,6 +1406,24 @@ static bool blackboxShouldLogIFrame(void)
return blackboxPFrameIndex == 0;
}
/*
* If the GPS home point has been updated, or every 128 I-frames (~10 seconds), write the
* GPS home position.
*
* We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
* still be interpreted correctly.
*/
#ifdef GPS
STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void)
{
if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
|| (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
return true;
}
return false;
}
#endif
// Called once every FC loop in order to keep track of how many FC loop iterations have passed
static void blackboxAdvanceIterationTimers(void)
{
@ -1447,7 +1446,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
* Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
* an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
*/
writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
if (blackboxIsOnlyLoggingIntraframes()) {
writeSlowFrameIfNeeded();
}
loadMainState(currentTimeUs);
writeIntraframe();
@ -1460,23 +1461,14 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
* We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
* So only log slow frames during loop iterations where we log a main frame.
*/
writeSlowFrameIfNeeded(true);
writeSlowFrameIfNeeded();
loadMainState(currentTimeUs);
writeInterframe();
}
#ifdef GPS
if (feature(FEATURE_GPS)) {
/*
* If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
* GPS home position.
*
* We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
* still be interpreted correctly.
*/
if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
|| (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
if (blackboxShouldLogGpsHomeFrame()) {
writeGPSHomeFrame();
writeGPSFrame(currentTimeUs);
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
@ -1497,8 +1489,6 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
*/
void blackboxUpdate(timeUs_t currentTimeUs)
{
int i;
switch (blackboxState) {
case BLACKBOX_STATE_STOPPED:
if (ARMING_FLAG(ARMED)) {
@ -1526,7 +1516,7 @@ void blackboxUpdate(timeUs_t currentTimeUs)
*/
if (millis() > xmitState.u.startTime + 100) {
if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) {
for (i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
blackboxWrite(blackboxHeader[xmitState.headerIndex]);
blackboxHeaderBudget--;
}

123
src/main/blackbox/blackbox_encoding.c
View File

@ -231,6 +231,129 @@ void blackboxWriteTag2_3S32(int32_t *values)
}
}
/**
* Write a 2 bit tag followed by 3 signed fields of 2, 554, 877 or 32 bits
*/
int blackboxWriteTag2_3SVariable(int32_t *values)
{
static const int FIELD_COUNT = 3;
enum {
BITS_2 = 0,
BITS_554 = 1,
BITS_877 = 2,
BITS_32 = 3
};
enum {
BYTES_1 = 0,
BYTES_2 = 1,
BYTES_3 = 2,
BYTES_4 = 3
};
/*
* Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes
* below:
*
* Selector possibilities
*
* 2 bits per field ss11 2233,
* 554 bits per field ss11 1112 2222 3333
* 877 bits per field ss11 1111 1122 2222 2333 3333
* 32 bits per field sstt tttt followed by fields of various byte counts
*/
int selector = BITS_2;
int selector2 = 0;
// Require more than 877 bits?
if (values[0] >= 256 || values[0] < -256
|| values[1] >= 128 || values[1] < -128
|| values[2] >= 128 || values[2] < -128) {
selector = BITS_32;
// Require more than 554 bits?
} else if (values[0] >= 16 || values[0] < -16
|| values[1] >= 16 || values[1] < -16
|| values[2] >= 8 || values[2] < -8) {
selector = BITS_877;
// Require more than 2 bits?
} else if (values[0] >= 2 || values[0] < -2
|| values[1] >= 2 || values[1] < -2
|| values[2] >= 2 || values[2] < -2) {
selector = BITS_554;
}
switch (selector) {
case BITS_2:
blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03));
break;
case BITS_554:
// 554 bits per field ss11 1112 2222 3333
blackboxWrite((selector << 6) | ((values[0] & 0x1F) << 1) | ((values[1] & 0x1F) >> 4));
blackboxWrite(((values[1] & 0x0F) << 4) | (values[2] & 0x0F));
break;
case BITS_877:
// 877 bits per field ss11 1111 1122 2222 2333 3333
blackboxWrite((selector << 6) | ((values[0] & 0xFF) >> 2));
blackboxWrite(((values[0] & 0x03) << 6) | ((values[1] & 0x7F) >> 1));
blackboxWrite(((values[1] & 0x01) << 7) | (values[2] & 0x7F));
break;
case BITS_32:
/*
* Do another round to compute a selector for each field, assuming that they are at least 8 bits each
*
* Selector2 field possibilities
* 0 - 8 bits
* 1 - 16 bits
* 2 - 24 bits
* 3 - 32 bits
*/
selector2 = 0;
//Encode in reverse order so the first field is in the low bits:
for (int x = FIELD_COUNT - 1; x >= 0; x--) {
selector2 <<= 2;
if (values[x] < 128 && values[x] >= -128) {
selector2 |= BYTES_1;
} else if (values[x] < 32768 && values[x] >= -32768) {
selector2 |= BYTES_2;
} else if (values[x] < 8388608 && values[x] >= -8388608) {
selector2 |= BYTES_3;
} else {
selector2 |= BYTES_4;
}
}
//Write the selectors
blackboxWrite((selector << 6) | selector2);
//And now the values according to the selectors we picked for them
for (int x = 0; x < FIELD_COUNT; x++, selector2 >>= 2) {
switch (selector2 & 0x03) {
case BYTES_1:
blackboxWrite(values[x]);
break;
case BYTES_2:
blackboxWrite(values[x]);
blackboxWrite(values[x] >> 8);
break;
case BYTES_3:
blackboxWrite(values[x]);
blackboxWrite(values[x] >> 8);
blackboxWrite(values[x] >> 16);
break;
case BYTES_4:
blackboxWrite(values[x]);
blackboxWrite(values[x] >> 8);
blackboxWrite(values[x] >> 16);
blackboxWrite(values[x] >> 24);
break;
}
}
break;
}
return selector;
}
/**
* Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits.
*/

1
src/main/blackbox/blackbox_encoding.h
View File

@ -27,6 +27,7 @@ void blackboxWriteSignedVBArray(int32_t *array, int count);
void blackboxWriteSigned16VBArray(int16_t *array, int count);
void blackboxWriteS16(int16_t value);
void blackboxWriteTag2_3S32(int32_t *values);
int blackboxWriteTag2_3SVariable(int32_t *values);
void blackboxWriteTag8_4S16(int32_t *values);
void blackboxWriteTag8_8SVB(int32_t *values, int valueCount);
void blackboxWriteU32(int32_t value);

3
src/main/blackbox/blackbox_fielddefs.h
View File

@ -97,7 +97,8 @@ typedef enum FlightLogFieldEncoding {
FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6,
FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7,
FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8,
FLIGHT_LOG_FIELD_ENCODING_NULL = 9 // Nothing is written to the file, take value to be zero
FLIGHT_LOG_FIELD_ENCODING_NULL = 9, // Nothing is written to the file, take value to be zero
FLIGHT_LOG_FIELD_ENCODING_TAG2_3SVARIABLE = 10
} FlightLogFieldEncoding;
typedef enum FlightLogFieldSign {

283
src/main/cms/cms.c
View File

@ -21,6 +21,7 @@
CMS-displayPort separation by jflyper and martinbudden
*/
//#define CMS_PAGE_DEBUG // For multi-page/menu debugging
//#define CMS_MENU_DEBUG // For external menu content creators
#include <stdbool.h>
@ -129,19 +130,20 @@ static displayPort_t *cmsDisplayPortSelectNext(void)
static bool cmsInMenu = false;
STATIC_UNIT_TESTED const CMS_Menu *currentMenu; // Points to top entry of the current page
typedef struct cmsCtx_s {
const CMS_Menu *menu; // menu for this context
uint8_t page; // page in the menu
int8_t cursorRow; // cursorRow in the page
} cmsCtx_t;
// XXX Does menu backing support backing into second page???
static const CMS_Menu *menuStack[10]; // Stack to save menu transition
static uint8_t menuStackHistory[10];// cursorRow in a stacked menu
static cmsCtx_t menuStack[10];
static uint8_t menuStackIdx = 0;
static OSD_Entry *pageTop; // Points to top entry of the current page
static OSD_Entry *pageTopAlt; // Only 2 pages are allowed (for now)
static uint8_t maxRow; // Max row in the current page
static int8_t pageCount; // Number of pages in the current menu
static OSD_Entry *pageTop; // First entry for the current page
static uint8_t pageMaxRow; // Max row in the current page
static int8_t cursorRow;
static cmsCtx_t currentCtx;
#ifdef CMS_MENU_DEBUG // For external menu content creators
@ -164,19 +166,52 @@ static CMS_Menu menuErr = {
};
#endif
#ifdef CMS_PAGE_DEBUG
#define cmsPageDebug() { \
debug[0] = pageCount; \
debug[1] = currentCtx.page; \
debug[2] = pageMaxRow; \
debug[3] = currentCtx.cursorRow; } struct _dummy
#else
#define cmsPageDebug()
#endif
static void cmsUpdateMaxRow(displayPort_t *instance)
{
maxRow = 0;
pageMaxRow = 0;
for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) {
maxRow++;
pageMaxRow++;
}
if (maxRow > MAX_MENU_ITEMS(instance)) {
maxRow = MAX_MENU_ITEMS(instance);
if (pageMaxRow > MAX_MENU_ITEMS(instance)) {
pageMaxRow = MAX_MENU_ITEMS(instance);
}
maxRow--;
pageMaxRow--;
}
static uint8_t cmsCursorAbsolute(displayPort_t *instance)
{
return currentCtx.cursorRow + currentCtx.page * MAX_MENU_ITEMS(instance);
}
static void cmsPageSelect(displayPort_t *instance, int8_t newpage)
{
currentCtx.page = (newpage + pageCount) % pageCount;
pageTop = &currentCtx.menu->entries[currentCtx.page * MAX_MENU_ITEMS(instance)];
cmsUpdateMaxRow(instance);
displayClearScreen(instance);
}
static void cmsPageNext(displayPort_t *instance)
{
cmsPageSelect(instance, currentCtx.page + 1);
}
static void cmsPagePrev(displayPort_t *instance)
{
cmsPageSelect(instance, currentCtx.page - 1);
}
static void cmsFormatFloat(int32_t value, char *floatString)
@ -377,7 +412,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
uint8_t i;
OSD_Entry *p;
uint8_t top = (pDisplay->rows - maxRow) / 2 - 1;
uint8_t top = (pDisplay->rows - pageMaxRow) / 2 - 1;
// Polled (dynamic) value display denominator.
@ -396,17 +431,9 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
SET_PRINTLABEL(p);
SET_PRINTVALUE(p);
}
if (i > MAX_MENU_ITEMS(pDisplay)) // max per page
{
pageTopAlt = pageTop + MAX_MENU_ITEMS(pDisplay);
if (pageTopAlt->type == OME_END)
pageTopAlt = NULL;
}
pDisplay->cleared = false;
} else if (drawPolled) {
for (p = pageTop ; p <= pageTop + maxRow ; p++) {
for (p = pageTop ; p <= pageTop + pageMaxRow ; p++) {
if (IS_DYNAMIC(p))
SET_PRINTVALUE(p);
}
@ -414,19 +441,21 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
// Cursor manipulation
while ((pageTop + cursorRow)->type == OME_Label) // skip label
cursorRow++;
while ((pageTop + currentCtx.cursorRow)->type == OME_Label) // skip label
currentCtx.cursorRow++;
if (pDisplay->cursorRow >= 0 && cursorRow != pDisplay->cursorRow) {
cmsPageDebug();
if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) {
room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, " ");
}
if (room < 30)
return;
if (pDisplay->cursorRow != cursorRow) {
room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, cursorRow + top, " >");
pDisplay->cursorRow = cursorRow;
if (pDisplay->cursorRow != currentCtx.cursorRow) {
room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCtx.cursorRow + top, " >");
pDisplay->cursorRow = currentCtx.cursorRow;
}
if (room < 30)
@ -458,46 +487,58 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
}
}
static void cmsMenuCountPage(displayPort_t *pDisplay)
{
OSD_Entry *p;
for (p = currentCtx.menu->entries; p->type != OME_END; p++);
pageCount = (p - currentCtx.menu->entries - 1) / MAX_MENU_ITEMS(pDisplay) + 1;
}
long cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
{
CMS_Menu *pMenu = (CMS_Menu *)ptr;
if (pMenu) {
if (!pMenu) {
return 0;
}
#ifdef CMS_MENU_DEBUG
if (pMenu->GUARD_type != OME_MENU) {
// ptr isn't pointing to a CMS_Menu.
if (pMenu->GUARD_type <= OME_MAX) {
strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1);
} else {
strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1);
}
pMenu = &menuErr;
if (pMenu->GUARD_type != OME_MENU) {
// ptr isn't pointing to a CMS_Menu.
if (pMenu->GUARD_type <= OME_MAX) {
strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1);
} else {
strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1);
}
pMenu = &menuErr;
}
#endif
if (pMenu != currentCtx.menu) {
// Stack the current menu and move to a new menu.
// The (pMenu == curretMenu) case occurs when reopening for display sw
if (pMenu != currentMenu) {
menuStack[menuStackIdx] = currentMenu;
cursorRow += pageTop - currentMenu->entries; // Convert cursorRow to absolute value
menuStackHistory[menuStackIdx] = cursorRow;
menuStackIdx++;
menuStack[menuStackIdx++] = currentCtx;
currentMenu = pMenu;
cursorRow = 0;
currentCtx.menu = pMenu;
currentCtx.cursorRow = 0;
if (pMenu->onEnter)
pMenu->onEnter();
}
if (pMenu->onEnter)
pMenu->onEnter();
pageTop = currentMenu->entries;
pageTopAlt = NULL;
cmsMenuCountPage(pDisplay);
cmsPageSelect(pDisplay, 0);
} else {
// The (pMenu == curretMenu) case occurs when reopening for display cycling
// currentCtx.cursorRow has been saved as absolute; convert it back to page + relative
displayClearScreen(pDisplay);
cmsUpdateMaxRow(pDisplay);
int8_t cursorAbs = currentCtx.cursorRow;
currentCtx.cursorRow = cursorAbs % MAX_MENU_ITEMS(pDisplay);
cmsMenuCountPage(pDisplay);
cmsPageSelect(pDisplay, cursorAbs / MAX_MENU_ITEMS(pDisplay));
}
cmsPageDebug();
return 0;
}
@ -505,30 +546,21 @@ STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay)
{
// Let onExit function decide whether to allow exit or not.
if (currentMenu->onExit && currentMenu->onExit(pageTop + cursorRow) < 0)
if (currentCtx.menu->onExit && currentCtx.menu->onExit(pageTop + currentCtx.cursorRow) < 0) {
return -1;
if (menuStackIdx) {
displayClearScreen(pDisplay);
menuStackIdx--;
currentMenu = menuStack[menuStackIdx];
cursorRow = menuStackHistory[menuStackIdx];
// cursorRow is absolute offset of a focused entry when stacked.
// Convert it back to page and relative offset.
pageTop = currentMenu->entries; // Temporary for cmsUpdateMaxRow()
cmsUpdateMaxRow(pDisplay);
if (cursorRow > maxRow) {
// Cursor was in the second page.
pageTopAlt = currentMenu->entries;
pageTop = pageTopAlt + maxRow + 1;
cursorRow -= (maxRow + 1);
cmsUpdateMaxRow(pDisplay); // Update maxRow for the second page
}
}
if (!menuStackIdx) {
return 0;
}
currentCtx = menuStack[--menuStackIdx];
cmsMenuCountPage(pDisplay);
cmsPageSelect(pDisplay, currentCtx.page);
cmsPageDebug();
return 0;
}
@ -540,12 +572,15 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
if (!pCurrentDisplay)
return;
cmsInMenu = true;
currentMenu = &menuMain;
currentCtx = (cmsCtx_t){ &menuMain, 0, 0 };
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
// Switch display
displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
if (pNextDisplay != pCurrentDisplay) {
// DisplayPort has been changed.
// Convert cursorRow to absolute value
currentCtx.cursorRow = cmsCursorAbsolute(pCurrentDisplay);
displayRelease(pCurrentDisplay);
pCurrentDisplay = pNextDisplay;
} else {
@ -553,7 +588,7 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
}
}
displayGrab(pCurrentDisplay); // grab the display for use by the CMS
cmsMenuChange(pCurrentDisplay, currentMenu);
cmsMenuChange(pCurrentDisplay, currentCtx.menu);
}
static void cmsTraverseGlobalExit(const CMS_Menu *pMenu)
@ -570,39 +605,47 @@ static void cmsTraverseGlobalExit(const CMS_Menu *pMenu)
}
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
{
if (ptr) {
displayClearScreen(pDisplay);
{
int exitType = (int)ptr;
switch (exitType) {
case CMS_EXIT_SAVE:
case CMS_EXIT_SAVEREBOOT:
cmsTraverseGlobalExit(&menuMain);
if (currentCtx.menu->onExit)
currentCtx.menu->onExit((OSD_Entry *)NULL); // Forced exit
saveConfigAndNotify();
break;
case CMS_EXIT:
break;
}
cmsInMenu = false;
displayRelease(pDisplay);
currentCtx.menu = NULL;
if (exitType == CMS_EXIT_SAVEREBOOT) {
displayClearScreen(pDisplay);
displayWrite(pDisplay, 5, 3, "REBOOTING...");
displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
stopMotors();
stopPwmAllMotors();
delay(200);
cmsTraverseGlobalExit(&menuMain);
if (currentMenu->onExit)
currentMenu->onExit((OSD_Entry *)NULL); // Forced exit
saveConfigAndNotify();
}
cmsInMenu = false;
displayRelease(pDisplay);
currentMenu = NULL;
if (ptr)
systemReset();
}
ENABLE_ARMING_FLAG(OK_TO_ARM);
return 0;
}
// Stick/key detection and key codes
#define IS_HI(X) (rcData[X] > 1750)
@ -625,7 +668,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
uint16_t res = BUTTON_TIME;
OSD_Entry *p;
if (!currentMenu)
if (!currentCtx.menu)
return res;
if (key == KEY_MENU) {
@ -639,42 +682,32 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
}
if (key == KEY_DOWN) {
if (cursorRow < maxRow) {
cursorRow++;
if (currentCtx.cursorRow < pageMaxRow) {
currentCtx.cursorRow++;
} else {
if (pageTopAlt) { // we have another page
displayClearScreen(pDisplay);
p = pageTopAlt;
pageTopAlt = pageTop;
pageTop = (OSD_Entry *)p;
cmsUpdateMaxRow(pDisplay);
}
cursorRow = 0; // Goto top in any case
cmsPageNext(pDisplay);
currentCtx.cursorRow = 0; // Goto top in any case
}
}
if (key == KEY_UP) {
cursorRow--;
currentCtx.cursorRow--;
if ((pageTop + cursorRow)->type == OME_Label && cursorRow > 0)
cursorRow--;
// Skip non-title labels
if ((pageTop + currentCtx.cursorRow)->type == OME_Label && currentCtx.cursorRow > 0)
currentCtx.cursorRow--;
if (cursorRow == -1 || (pageTop + cursorRow)->type == OME_Label) {
if (pageTopAlt) {
displayClearScreen(pDisplay);
p = pageTopAlt;
pageTopAlt = pageTop;
pageTop = (OSD_Entry *)p;
cmsUpdateMaxRow(pDisplay);
}
cursorRow = maxRow; // Goto bottom in any case
if (currentCtx.cursorRow == -1 || (pageTop + currentCtx.cursorRow)->type == OME_Label) {
// Goto previous page
cmsPagePrev(pDisplay);
currentCtx.cursorRow = pageMaxRow;
}
}
if (key == KEY_DOWN || key == KEY_UP)
return res;
p = pageTop + cursorRow;
p = pageTop + currentCtx.cursorRow;
switch (p->type) {
case OME_Submenu:
@ -878,19 +911,19 @@ void cmsUpdate(uint32_t currentTimeUs)
if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
key = KEY_MENU;
}
else if (IS_HI(PITCH)) {
else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_MID(ROLL) && IS_HI(PITCH)) {
key = KEY_UP;
}
else if (IS_LO(PITCH)) {
else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_MID(ROLL) && IS_LO(PITCH)) {
key = KEY_DOWN;
}
else if (IS_LO(ROLL)) {
else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_LO(ROLL) && IS_MID(PITCH)) {
key = KEY_LEFT;
}
else if (IS_HI(ROLL)) {
else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_HI(ROLL) && IS_MID(PITCH)) {
key = KEY_RIGHT;
}
else if (IS_HI(YAW) || IS_LO(YAW))
else if ((IS_HI(YAW) || IS_LO(YAW)) && IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH))
{
key = KEY_ESC;
}
@ -969,8 +1002,6 @@ void cmsHandler(timeUs_t currentTimeUs)
}
}
// Is initializing with menuMain better?
// Can it be done with the current main()?
void cmsInit(void)
{
cmsDeviceCount = 0;

6
src/main/cms/cms.h
View File

@ -18,3 +18,9 @@ void cmsUpdate(uint32_t currentTimeUs);
#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID"
#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT"
#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP"
// cmsMenuExit special ptr values
#define CMS_EXIT (0)
#define CMS_EXIT_SAVE (1)
#define CMS_EXIT_SAVEREBOOT (2)

5
src/main/cms/cms_menu_builtin.c
View File

@ -138,8 +138,9 @@ static OSD_Entry menuMainEntries[] =
#endif
{"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0},
{"MISC", OME_Submenu, cmsMenuChange, &cmsx_menuMisc, 0},
{"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0},
{"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0},
{"EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT, 0},
{"SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0},
{"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVEREBOOT, 0},
#ifdef CMS_MENU_DEBUG
{"ERR SAMPLE", OME_Submenu, cmsMenuChange, &menuInfoEntries[0], 0},
#endif

9
src/main/common/maths.c
View File

@ -99,6 +99,15 @@ float acos_approx(float x)
}
#endif
int gcd(int num, int denom)
{
if (denom == 0) {
return num;
}
return gcd(denom, num % denom);
}
float powerf(float base, int exp) {
float result = base;
for (int count = 1; count < exp; count++) result *= base;

1
src/main/common/maths.h
View File

@ -76,6 +76,7 @@ typedef union {
fp_angles_def angles;
} fp_angles_t;
int gcd(int num, int denom);
float powerf(float base, int exp);
int32_t applyDeadband(int32_t value, int32_t deadband);

1
src/main/drivers/light_ws2811strip_hal.c
View File

@ -129,6 +129,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
TIM_OCInitStructure.Pulse = 0;
TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCNPolarity = TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;

6
src/main/drivers/pwm_output.c
View File

@ -59,11 +59,15 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH: TIM_OCPOLARITY_LOW;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
} else {
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET;
TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH;
}
TIM_OCInitStructure.Pulse = value;

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

@ -792,7 +792,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro
switch (cmdMSP) {
case MSP_STATUS_EX:
sbufWriteU16(dst, 0); // task delta
sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
#ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter());
#else
@ -807,7 +807,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro
break;
case MSP_STATUS:
sbufWriteU16(dst, 0); // task delta
sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
#ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter());
#else
@ -1375,7 +1375,6 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
static mspResult_e mspOsdSlaveProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
UNUSED(cmdMSP);
UNUSED(src);
// Nothing OSD SLAVE specific yet.
return MSP_RESULT_ERROR;
}
#endif
@ -2145,8 +2144,31 @@ void mspFcProcessReply(mspPacket_t *reply)
UNUSED(src); // potentially unused depending on compile options.
switch (reply->cmd) {
case MSP_DISPLAYPORT: {
#ifndef OSD_SLAVE
case MSP_ANALOG:
{
uint8_t batteryVoltage = sbufReadU8(src);
uint16_t mAhDrawn = sbufReadU16(src);
uint16_t rssi = sbufReadU16(src);
uint16_t amperage = sbufReadU16(src);
UNUSED(rssi);
UNUSED(batteryVoltage);
UNUSED(amperage);
UNUSED(mAhDrawn);
#ifdef USE_MSP_CURRENT_METER
currentMeterMSPSet(amperage, mAhDrawn);
#endif
break;
}
#endif
#ifdef USE_OSD_SLAVE
case MSP_DISPLAYPORT:
{
osdSlaveIsLocked = true; // lock it as soon as a MSP_DISPLAYPORT message is received to prevent accidental CLI/DFU mode.
int subCmd = sbufReadU8(src);
switch (subCmd) {
@ -2184,9 +2206,9 @@ void mspFcProcessReply(mspPacket_t *reply)
osdSlaveDrawScreen();
}
}
#endif
break;
}
#endif
}
}

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

@ -33,3 +33,5 @@ void mspFcInit(void);
void mspOsdSlaveInit(void);
mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
void mspFcProcessReply(mspPacket_t *reply);
void mspSerialProcessStreamSchedule(void);

11
src/main/fc/fc_tasks.c
View File

@ -23,6 +23,8 @@
#include "cms/cms.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/utils.h"
@ -81,6 +83,8 @@
#include "telemetry/telemetry.h"
#include "io/osd_slave.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
#endif
@ -115,7 +119,12 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
return;
}
#endif
mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand, mspFcProcessReply);
#ifndef OSD_SLAVE
bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;
#else
bool evaluateMspData = osdSlaveIsLocked ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;;
#endif
mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply);
}
void taskBatteryAlerts(timeUs_t currentTimeUs)

2
src/main/flight/failsafe.c
View File

@ -88,7 +88,7 @@ void failsafeInit(void)
return;
}
failsafePhase_e failsafePhase()
failsafePhase_e failsafePhase(void)
{
return failsafeState.phase;
}

2
src/main/io/displayport_msp.c
View File

@ -58,7 +58,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len
return 0;
}
#endif
return mspSerialPush(cmd, buf, len);
return mspSerialPush(cmd, buf, len, MSP_DIRECTION_REPLY);
}
static int heartbeat(displayPort_t *displayPort)

3
src/main/io/osd_slave.c
View File

@ -40,6 +40,9 @@
//#define OSD_SLAVE_DEBUG
// when locked the system ignores requests to enter cli or bootloader mode via serial connection.
bool osdSlaveIsLocked = false;
static displayPort_t *osdDisplayPort;
static void osdDrawLogo(int x, int y)

2
src/main/io/osd_slave.h
View File

@ -22,6 +22,8 @@
struct displayPort_s;
extern bool osdSlaveIsLocked;
// init
void osdSlaveInit(struct displayPort_s *osdDisplayPort);

6
src/main/msp/msp.h
View File

@ -26,10 +26,16 @@ typedef enum {
MSP_RESULT_NO_REPLY = 0
} mspResult_e;
typedef enum {
MSP_DIRECTION_REPLY = 0,
MSP_DIRECTION_REQUEST = 1
} mspDirection_e;
typedef struct mspPacket_s {
sbuf_t buf;
int16_t cmd;
int16_t result;
uint8_t direction;
} mspPacket_t;
struct serialPort_s;

4
src/main/msp/msp_protocol.h
View File

@ -59,7 +59,7 @@
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 35 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_MINOR 36 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_LENGTH 2
@ -101,7 +101,6 @@
#define MSP_NAME 10 //out message Returns user set board name - betaflight
#define MSP_SET_NAME 11 //in message Sets board name - betaflight
//
// MSP commands for Cleanflight original features
//
@ -312,3 +311,4 @@
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface

23
src/main/msp/msp_serial.c
View File

@ -27,12 +27,13 @@
#include "io/serial.h"
#include "drivers/system.h"
#include "msp/msp.h"
#include "msp/msp_serial.h"
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
{
memset(mspPortToReset, 0, sizeof(mspPort_t));
@ -138,7 +139,13 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
serialBeginWrite(msp->port);
const int len = sbufBytesRemaining(&packet->buf);
const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
uint8_t hdr[8] = {
'$',
'M',
packet->result == MSP_RESULT_ERROR ? '!' : packet->direction == MSP_DIRECTION_REPLY ? '>' : '<',
mspLen,
packet->cmd
};
int hdrLen = 5;
#define CHECKSUM_STARTPOS 3 // checksum starts from mspLen field
if (len >= JUMBO_FRAME_SIZE_LIMIT) {
@ -165,6 +172,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
.buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
.cmd = -1,
.result = 0,
.direction = MSP_DIRECTION_REPLY,
};
uint8_t *outBufHead = reply.buf.ptr;
@ -172,6 +180,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
.buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
.cmd = msp->cmdMSP,
.result = 0,
.direction = MSP_DIRECTION_REQUEST,
};
mspPostProcessFnPtr mspPostProcessFn = NULL;
@ -182,7 +191,6 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
mspSerialEncode(msp, &reply);
}
msp->c_state = MSP_IDLE;
return mspPostProcessFn;
}
@ -215,7 +223,9 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
if (!mspPort->port) {
continue;
}
mspPostProcessFnPtr mspPostProcessFn = NULL;
while (serialRxBytesWaiting(mspPort->port)) {
const uint8_t c = serialRead(mspPort->port);
@ -231,9 +241,12 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
} else if (mspPort->packetType == MSP_PACKET_REPLY) {
mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
}
mspPort->c_state = MSP_IDLE;
break; // process one command at a time so as not to block.
}
}
if (mspPostProcessFn) {
waitForSerialPortToFinishTransmitting(mspPort->port);
mspPostProcessFn(mspPort->port);
@ -262,7 +275,7 @@ void mspSerialInit(void)
mspSerialAllocatePorts();
}
int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction)
{
int ret = 0;
@ -281,6 +294,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
.buf = { .ptr = data, .end = data + datalen, },
.cmd = cmd,
.result = 0,
.direction = direction,
};
ret = mspSerialEncode(mspPort, &push);
@ -288,6 +302,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
return ret; // return the number of bytes written
}
uint32_t mspSerialTxBytesFree()
{
uint32_t ret = UINT32_MAX;

3
src/main/msp/msp_serial.h
View File

@ -67,11 +67,10 @@ typedef struct mspPort_s {
uint8_t inBuf[MSP_PORT_INBUF_SIZE];
} mspPort_t;
void mspSerialInit(void);
bool mspSerialWaiting(void);
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn);
void mspSerialAllocatePorts(void);
void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen);
int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction);
uint32_t mspSerialTxBytesFree(void);

15
src/main/sensors/battery.c
View File

@ -76,9 +76,13 @@ static batteryState_e consumptionState;
#ifdef USE_VIRTUAL_CURRENT_METER
#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL
#else
#ifdef USE_MSP_CURRENT_METER
#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_MSP
#else
#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE
#endif
#endif
#endif
#ifdef BOARD_HAS_VOLTAGE_DIVIDER
#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
@ -302,6 +306,11 @@ void batteryInit(void)
case CURRENT_METER_ESC:
#ifdef ESC_SENSOR
currentMeterESCInit();
#endif
break;
case CURRENT_METER_MSP:
#ifdef USE_MSP_CURRENT_METER
currentMeterMSPInit();
#endif
break;
@ -362,6 +371,12 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
currentMeterESCRefresh(lastUpdateAt);
currentMeterESCReadCombined(&currentMeter);
}
#endif
break;
case CURRENT_METER_MSP:
#ifdef USE_MSP_CURRENT_METER
currentMeterMSPRefresh(currentTimeUs);
currentMeterMSPRead(&currentMeter);
#endif
break;

49
src/main/sensors/current.c
View File

@ -56,12 +56,15 @@ const uint8_t currentMeterIds[] = {
CURRENT_METER_ID_ESC_MOTOR_11,
CURRENT_METER_ID_ESC_MOTOR_12,
#endif
#ifdef USE_MSP_CURRENT_METER
CURRENT_METER_ID_MSP_1,
#endif
};
const uint8_t supportedCurrentMeterCount = ARRAYLEN(currentMeterIds);
//
// ADC/Virtual/ESC shared
// ADC/Virtual/ESC/MSP shared
//
void currentMeterReset(currentMeter_t *meter)
@ -225,6 +228,45 @@ void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter)
}
#endif
#ifdef USE_MSP_CURRENT_METER
#include "common/streambuf.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
currentMeterMSPState_t currentMeterMSPState;
void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn)
{
// We expect the FC's MSP_ANALOG response handler to call this function
currentMeterMSPState.amperage = amperage;
currentMeterMSPState.mAhDrawn = mAhDrawn;
}
void currentMeterMSPInit(void)
{
memset(&currentMeterMSPState, 0, sizeof(currentMeterMSPState_t));
}
void currentMeterMSPRefresh(timeUs_t currentTimeUs)
{
// periodically request MSP_ANALOG
static timeUs_t streamRequestAt = 0;
if (cmp32(currentTimeUs, streamRequestAt) > 0) {
streamRequestAt = currentTimeUs + ((1000 * 1000) / 10); // 10hz
mspSerialPush(MSP_ANALOG, NULL, 0, MSP_DIRECTION_REQUEST);
}
}
void currentMeterMSPRead(currentMeter_t *meter)
{
meter->amperageLatest = currentMeterMSPState.amperage;
meter->amperage = currentMeterMSPState.amperage;
meter->mAhDrawn = currentMeterMSPState.mAhDrawn;
}
#endif
//
// API for current meters using IDs
//
@ -241,6 +283,11 @@ void currentMeterRead(currentMeterId_e id, currentMeter_t *meter)
currentMeterVirtualRead(meter);
}
#endif
#ifdef USE_MSP_CURRENT_METER
else if (id == CURRENT_METER_ID_MSP_1) {
currentMeterMSPRead(meter);
}
#endif
#ifdef USE_ESC_SENSOR
else if (id == CURRENT_METER_ID_ESC_COMBINED_1) {
currentMeterESCReadCombined(meter);

23
src/main/sensors/current.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "common/time.h"
#include "current_ids.h"
typedef enum {
@ -24,6 +25,7 @@ typedef enum {
CURRENT_METER_ADC,
CURRENT_METER_VIRTUAL,
CURRENT_METER_ESC,
CURRENT_METER_MSP,
CURRENT_METER_MAX = CURRENT_METER_ESC
} currentMeterSource_e;
@ -48,6 +50,7 @@ typedef enum {
CURRENT_SENSOR_VIRTUAL = 0,
CURRENT_SENSOR_ADC,
CURRENT_SENSOR_ESC,
CURRENT_SENSOR_MSP
} currentSensor_e;
@ -93,6 +96,21 @@ typedef struct currentMeterESCState_s {
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
} currentMeterESCState_t;
//
// MSP
//
typedef struct currentMeterMSPState_s {
int32_t mAhDrawn; // milliampere hours drawn from the battery since start
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
} currentMeterMSPState_t;
//
// Current Meter API
//
void currentMeterReset(currentMeter_t *meter);
void currentMeterADCInit(void);
@ -108,6 +126,11 @@ void currentMeterESCRefresh(int32_t lastUpdateAt);
void currentMeterESCReadCombined(currentMeter_t *meter);
void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter);
void currentMeterMSPInit(void);
void currentMeterMSPRefresh(timeUs_t currentTimeUs);
void currentMeterMSPRead(currentMeter_t *meter);
void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn);
//
// API for reading current meters by id.
//

3
src/main/sensors/current_ids.h
View File

@ -66,4 +66,7 @@ typedef enum {
CURRENT_METER_ID_VIRTUAL_1 = 80, // 80-89 for virtual meters
CURRENT_METER_ID_VIRTUAL_2,
CURRENT_METER_ID_MSP_1 = 90, // 90-99 for MSP meters
CURRENT_METER_ID_MSP_2,
} currentMeterId_e;

92
src/main/sensors/gyro.c
View File

@ -74,7 +74,13 @@ gyro_t gyro;
STATIC_UNIT_TESTED gyroDev_t gyroDev0;
static int16_t gyroTemperature0;
static uint16_t calibratingG = 0;
typedef struct gyroCalibration_s {
int32_t g[XYZ_AXIS_COUNT];
stdev_t var[XYZ_AXIS_COUNT];
uint16_t calibratingG;
} gyroCalibration_t;
STATIC_UNIT_TESTED gyroCalibration_t gyroCalibration;
static filterApplyFnPtr softLpfFilterApplyFn;
static void *softLpfFilter[3];
@ -412,12 +418,12 @@ void gyroInitFilters(void)
bool isGyroCalibrationComplete(void)
{
return calibratingG == 0;
return gyroCalibration.calibratingG == 0;
}
static bool isOnFinalGyroCalibrationCycle(void)
static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
{
return calibratingG == 1;
return gyroCalibration->calibratingG == 1;
}
static uint16_t gyroCalculateCalibratingCycles(void)
@ -425,56 +431,50 @@ static uint16_t gyroCalculateCalibratingCycles(void)
return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
}
static bool isOnFirstGyroCalibrationCycle(void)
static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
{
return calibratingG == gyroCalculateCalibratingCycles();
return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles();
}
void gyroSetCalibrationCycles(void)
{
calibratingG = gyroCalculateCalibratingCycles();
gyroCalibration.calibratingG = gyroCalculateCalibratingCycles();
}
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, uint8_t gyroMovementCalibrationThreshold)
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold)
{
static int32_t g[3];
static stdev_t var[3];
for (int axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle()) {
g[axis] = 0;
devClear(&var[axis]);
if (isOnFirstGyroCalibrationCycle(gyroCalibration)) {
gyroCalibration->g[axis] = 0;
devClear(&gyroCalibration->var[axis]);
// gyroZero is set to zero until calibration complete
gyroDev->gyroZero[axis] = 0;
}
// Sum up CALIBRATING_GYRO_CYCLES readings
g[axis] += gyroDev->gyroADC[axis];
devPush(&var[axis], gyroDev->gyroADC[axis]);
gyroCalibration->g[axis] += gyroDev->gyroADCRaw[axis];
devPush(&gyroCalibration->var[axis], gyroDev->gyroADCRaw[axis]);
// Reset global variables to prevent other code from using un-calibrated data
gyroDev->gyroADC[axis] = 0;
gyroDev->gyroZero[axis] = 0;
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
const float stddev = devStandardDeviation(&gyroCalibration->var[axis]);
if (isOnFinalGyroCalibrationCycle()) {
const float dev = devStandardDeviation(&var[axis]);
DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(dev));
DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles();
return;
}
gyroDev->gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
gyroDev->gyroZero[axis] = (gyroCalibration->g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
}
}
if (isOnFinalGyroCalibrationCycle()) {
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
beeper(BEEPER_GYRO_CALIBRATED);
}
calibratingG--;
gyroCalibration->calibratingG--;
}
@ -489,14 +489,13 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev)
#endif
gyroDev->dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroDev->gyroADC[X] = gyroDev->gyroADCRaw[X];
gyroDev->gyroADC[Y] = gyroDev->gyroADCRaw[Y];
gyroDev->gyroADC[Z] = gyroDev->gyroADCRaw[Z];
gyroDev->gyroADC[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X];
gyroDev->gyroADC[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y];
gyroDev->gyroADC[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z];
alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroDev->gyroADC[axis] -= gyroDev->gyroZero[axis];
// scale gyro output to degrees per second
float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale;
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
@ -522,15 +521,8 @@ void gyroUpdate(void)
return;
}
gyroDev0.dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroDev0.gyroADC[X] = gyroDev0.gyroADCRaw[X];
gyroDev0.gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
gyroDev0.gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete();
if (calibrationComplete) {
if (isGyroCalibrationComplete()) {
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
// SPI-based gyro so can read and update in ISR
if (gyroConfig()->gyro_isr_update) {
@ -541,12 +533,23 @@ void gyroUpdate(void)
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
debug[3] = (uint16_t)(micros() & 0xffff);
#endif
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroDev0.gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign);
} else {
performGyroCalibration(&gyroDev0, gyroConfig()->gyroMovementCalibrationThreshold);
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
// Reset gyro values to zero to prevent other code from using uncalibrated data
gyro.gyroADCf[X] = 0.0f;
gyro.gyroADCf[Y] = 0.0f;
gyro.gyroADCf[Z] = 0.0f;
// still calibrating, so no need to further process gyro data
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroDev0.gyroADC[axis] -= gyroDev0.gyroZero[axis];
// scale gyro output to degrees per second
float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale;
@ -561,11 +564,6 @@ void gyroUpdate(void)
gyro.gyroADCf[axis] = gyroADCf;
}
if (!calibrationComplete) {
gyroDev0.gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
gyroDev0.gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale);
gyroDev0.gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale);
}
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyse(&gyroDev0, &gyro);
#endif

2
src/main/target/ANYFCF7/target.h
View File

@ -21,8 +21,6 @@
#define USBD_PRODUCT_STRING "AnyFCF7"
#define USE_ESC_SENSOR
#define LED0 PB7
#define LED1 PB6

2
src/main/target/ANYFCM7/target.h
View File

@ -86,7 +86,7 @@
#define USE_SOFTSERIAL1
#define USE_SOFTSERIAL2
#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART2, USART3, UART4, UART5, USART6, USART7, USART8, SOFTSERIAL x 2
#define SERIAL_PORT_COUNT 9 //VCP, USART1, USART2, USART3, UART4, UART5, USART6, SOFTSERIAL x 2
#define USE_SPI
#define USE_SPI_DEVICE_1

2
src/main/target/BLUEJAYF4/target.h
View File

@ -144,8 +144,6 @@
#define VBAT_ADC_PIN PC3
#define CURRENT_METER_ADC_PIN PC2
#define USE_ESC_SENSOR
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL

2
src/main/target/FURYF4/target.h
View File

@ -183,8 +183,6 @@
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define USE_ESC_SENSOR
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff

2
src/main/target/FURYF7/target.h
View File

@ -21,8 +21,6 @@
#define USBD_PRODUCT_STRING "FuryF7"
#define USE_ESC_SENSOR
#define LED0 PB5
#define LED1 PB4

1
src/main/target/NERO/target.h
View File

@ -120,7 +120,6 @@
#define USE_ADC
#define VBAT_ADC_PIN PC3
//#define USE_ESC_SENSOR
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL

2
src/main/target/NUCLEOF722/target.h
View File

@ -93,7 +93,7 @@
#define USE_SOFTSERIAL1
#define USE_SOFTSERIAL2
#define SERIAL_PORT_COUNT 4 //VCP, USART2, USART3, UART4, UART5, USART6, USART7, USART8, SOFTSERIAL x 2
#define SERIAL_PORT_COUNT 6 //VCP, USART2, USART3, UART4,SOFTSERIAL x 2
#define USE_SPI
#define USE_SPI_DEVICE_1

1
src/main/target/OMNIBUSF4/target.c
View File

@ -45,6 +45,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 1), // S3_OUT D1_ST6
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S4_OUT D1_ST1
#if defined(CL_RACINGF4)
DEF_TIM(TIM3, CH1, PB4, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM
DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT
#elif defined(OMNIBUSF4SD)
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT

13
src/main/target/OMNIBUSF4/target.h
View File

@ -41,6 +41,7 @@
//#define LED1 PB4 // Remove this at the next major release
#define BEEPER PB4
#define BEEPER_INVERTED
#define BEEPER_PWM_HZ 3800 // Beeper PWM frequency in Hz
#ifdef OMNIBUSF4SD
#define INVERTER_PIN_UART6 PC8 // Omnibus F4 V3 and later
@ -191,16 +192,16 @@
//#define RSSI_ADC_PIN PA0
#endif
#define USE_ESC_SENSOR
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define AVOID_UART1_FOR_PWM_PPM
#if defined(CL_RACINGF4)
#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD )
#define SPEKTRUM_BIND_PIN UART6_RX_PIN
#else
#define DEFAULT_FEATURES (FEATURE_OSD)
#define AVOID_UART1_FOR_PWM_PPM
#define SPEKTRUM_BIND_PIN UART1_RX_PIN
#endif
#define SPEKTRUM_BIND_PIN UART1_RX_PIN
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
@ -209,13 +210,13 @@
#define TARGET_IO_PORTD BIT(2)
#ifdef CL_RACINGF4
#define USABLE_TIMER_CHANNEL_COUNT 6
#define USED_TIMERS ( TIM_N(4) | TIM_N(8) )
#define USABLE_TIMER_CHANNEL_COUNT 7
#define USED_TIMERS ( TIM_N(3) | TIM_N(4) | TIM_N(4) | TIM_N(8) )
#else
#ifdef OMNIBUSF4SD
#define USABLE_TIMER_CHANNEL_COUNT 13
#else
#define USABLE_TIMER_CHANNEL_COUNT 12
#endif
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))
#endif

2
src/main/target/REVO/target.h
View File

@ -48,8 +48,6 @@
#endif
#define USE_ESC_SENSOR
#define LED0 PB5
#if defined(PODIUMF4)
#define LED1 PB4

2
src/main/target/SPRACINGF3/target.h
View File

@ -180,6 +180,8 @@
#define USE_OSD_OVER_MSP_DISPLAYPORT
#define USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
#define USE_MSP_CURRENT_METER
#define USE_ESC_SENSOR
#define REMAP_TIM17_DMA

2
src/main/target/SPRACINGF3EVO/target.h
View File

@ -163,6 +163,8 @@
#define OSD
#define USE_OSD_OVER_MSP_DISPLAYPORT
#define USE_MSP_CURRENT_METER
#undef USE_DASHBOARD
#define TRANSPONDER

1
src/main/target/SPRACINGF4EVO/target.h
View File

@ -162,6 +162,7 @@
#define OSD
#define USE_OSD_OVER_MSP_DISPLAYPORT
#define USE_MSP_CURRENT_METER
#define LED_STRIP
#define TRANSPONDER

2
src/main/target/common_fc_pre.h
View File

@ -45,12 +45,14 @@
#ifdef STM32F4
#define USE_DSHOT
#define USE_ESC_SENSOR
#define I2C3_OVERCLOCK true
#define TELEMETRY_IBUS
#endif
#ifdef STM32F7
#define USE_DSHOT
#define USE_ESC_SENSOR
#define I2C3_OVERCLOCK true
#define I2C4_OVERCLOCK true
#define TELEMETRY_IBUS

139
src/test/unit/blackbox_encoding_unittest.cc
View File

@ -29,15 +29,6 @@ extern "C" {
#include "drivers/serial.h"
#include "io/serial.h"
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.device = DEFAULT_BLACKBOX_DEVICE,
.rate_num = 1,
.rate_denom = 1,
.on_motor_test = 0 // default off
);
}
#include "unittest_macros.h"
@ -108,6 +99,7 @@ bool isSerialTransmitBufferEmpty(const serialPort_t *instance)
void serialTestResetBuffers()
{
blackboxPort = &serialTestInstance;
memset(&serialReadBuffer, 0, sizeof(serialReadBuffer));
serialReadPos = 0;
serialReadEnd = 0;
@ -115,9 +107,10 @@ void serialTestResetBuffers()
serialWritePos = 0;
}
TEST(BlackboxTest, Test1)
TEST(BlackboxEncodingTest, TestWriteUnsignedVB)
{
blackboxPort = &serialTestInstance;
serialTestResetBuffers();
blackboxWriteUnsignedVB(0);
EXPECT_EQ(0, serialWriteBuffer[0]);
blackboxWriteUnsignedVB(128);
@ -125,8 +118,131 @@ TEST(BlackboxTest, Test1)
EXPECT_EQ(1, serialWriteBuffer[2]);
}
TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS2)
{
serialTestResetBuffers();
uint8_t *buf = &serialWriteBuffer[0];
int selector;
int32_t v[3];
// 2 bits per field ss11 2233,
v[0] = 0;
v[1] = 0;
v[2] = 0;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(0, selector);
EXPECT_EQ(0, buf[0]);
EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
++buf;
v[0] = 1;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(0x10, buf[0]); // 00010000
EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
++buf;
v[0] = 1;
v[1] = 1;
v[2] = 1;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(0, selector);
EXPECT_EQ(0x15, buf[0]); // 00010101
EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
++buf;
v[0] = -1;
v[1] = -1;
v[2] = -1;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(0, selector);
EXPECT_EQ(0x3F, buf[0]); // 00111111
EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
++buf;
v[0] = -2;
v[1] = -2;
v[2] = -2;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(0, selector);
EXPECT_EQ(0x2A, buf[0]); // 00101010
EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
++buf;
}
TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS554)
{
serialTestResetBuffers();
uint8_t *buf = &serialWriteBuffer[0];
int selector;
int32_t v[3];
// 554 bits per field ss11 1112 2222 3333
// 5 bits per field [-16, 15], 4 bits per field [-8, 7]
v[0] = 15;
v[1] = 15;
v[2] = 7;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(1, selector);
EXPECT_EQ(0x5E, buf[0]); // 0101 1110
EXPECT_EQ(0xF7, buf[1]); // 1111 0111
EXPECT_EQ(0, buf[2]); // ensure next byte has not been written
buf += 2;
v[0] = -16;
v[1] = -16;
v[2] = -8;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(1, selector);
EXPECT_EQ(0x61, buf[0]); // 0110 0001
EXPECT_EQ(0x08, buf[1]); // 0000 1000
EXPECT_EQ(0, buf[2]); // ensure next byte has not been written
buf += 2;
v[0] = 7;
v[1] = 8;
v[2] = 5;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(1, selector);
EXPECT_EQ(0x4E, buf[0]); // 0100 1110
EXPECT_EQ(0x85, buf[1]); // 1000 0101
EXPECT_EQ(0, buf[2]); // ensure next byte has not been written
buf += 2;
}
TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS887)
{
serialTestResetBuffers();
uint8_t *buf = &serialWriteBuffer[0];
int selector;
int32_t v[3];
// 877 bits per field ss11 1111 1122 2222 2333 3333
// 8 bits per field [-128, 127], 7 bits per field [-64, 63]
v[0] = 127;
v[1] = 63;
v[2] = 63;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(2, selector);
EXPECT_EQ(0x9F, buf[0]); // 1001 1111
EXPECT_EQ(0xDF, buf[1]); // 1101 1111
EXPECT_EQ(0xBF, buf[2]); // 1011 1111
EXPECT_EQ(0, buf[3]); // ensure next byte has not been written
buf += 3;
v[0] = -128;
v[1] = -64;
v[2] = -64;
selector = blackboxWriteTag2_3SVariable(v);
EXPECT_EQ(2, selector);
EXPECT_EQ(0xA0, buf[0]); // 1010 0000
EXPECT_EQ(0x20, buf[1]); // 0010 0000
EXPECT_EQ(0x40, buf[2]); // 0100 0000
EXPECT_EQ(0, buf[3]); // ensure next byte has not been written
buf += 3;
}
// STUBS
extern "C" {
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
int32_t blackboxHeaderBudget;
void mspSerialAllocatePorts(void) {}
void blackboxWrite(uint8_t value) {serialWrite(blackboxPort, value);}
@ -140,5 +256,4 @@ int blackboxPrint(const char *s)
const int length = pos - (uint8_t*)s;
return length;
}
}