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

Spektrum, CMS over Telemetry. Rebased and squashed.

This commit is contained in:
Anders Hoglund 2017-11-14 15:57:33 +01:00
parent 971abbd9f6
commit 0e1f0e89e7
12 changed files with 457 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
make/source.mk
View File

@ -149,6 +149,7 @@ FC_SRC = \
io/displayport_msp.c \
io/displayport_oled.c \
io/displayport_rcdevice.c \
io/displayport_srxl.c \
io/rcdevice_cam.c \
io/rcdevice.c \
io/rcdevice_osd.c \

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

@ -126,10 +126,18 @@ static displayPort_t *cmsDisplayPortSelectNext(void)
// HoTT Telemetry Screen
// 21 cols x 8 rows
//
// Spektrum SRXL Telemtry Textgenerator
// 13 cols x 9 rows, top row printed as a Bold Heading
// Needs the "smallScreen" adaptions
#define LEFT_MENU_COLUMN 1
#define RIGHT_MENU_COLUMN(p) ((p)->cols - 8)
#define MAX_MENU_ITEMS(p) ((p)->rows - 2)
#define NORMAL_SCREEN_MIN_COLS 18 // Less is a small screen
static bool smallScreen;
static uint8_t leftMenuColumn;
static uint8_t rightMenuColumn;
static uint8_t maxMenuItems;
static uint8_t linesPerMenuItem;
bool cmsInMenu = false;
@ -181,14 +189,15 @@ static CMS_Menu menuErr = {
static void cmsUpdateMaxRow(displayPort_t *instance)
{
UNUSED(instance);
pageMaxRow = 0;
for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) {
pageMaxRow++;
}
if (pageMaxRow > MAX_MENU_ITEMS(instance)) {
pageMaxRow = MAX_MENU_ITEMS(instance);
if (pageMaxRow > maxMenuItems) {
pageMaxRow = maxMenuItems;
}
pageMaxRow--;
@ -196,13 +205,14 @@ static void cmsUpdateMaxRow(displayPort_t *instance)
static uint8_t cmsCursorAbsolute(displayPort_t *instance)
{
return currentCtx.cursorRow + currentCtx.page * MAX_MENU_ITEMS(instance);
UNUSED(instance);
return currentCtx.cursorRow + currentCtx.page * maxMenuItems;
}
static void cmsPageSelect(displayPort_t *instance, int8_t newpage)
{
currentCtx.page = (newpage + pageCount) % pageCount;
pageTop = &currentCtx.menu->entries[currentCtx.page * MAX_MENU_ITEMS(instance)];
pageTop = &currentCtx.menu->entries[currentCtx.page * maxMenuItems];
cmsUpdateMaxRow(instance);
displayClearScreen(instance);
}
@ -244,7 +254,13 @@ static void cmsFormatFloat(int32_t value, char *floatString)
floatString[0] = ' ';
}
static void cmsPadToSize(char *buf, int size)
// CMS on OSD legacy was to use LEFT aligned values, not the RIGHT way ;-)
#define CMS_OSD_RIGHT_ALIGNED_VALUES
#ifndef CMS_OSD_RIGHT_ALIGNED_VALUES
// Pad buffer to the left, i.e. align left
static void cmsPadRightToSize(char *buf, int size)
{
int i;
@ -259,17 +275,69 @@ static void cmsPadToSize(char *buf, int size)
buf[size] = 0;
}
#endif
// Pad buffer to the left, i.e. align right
static void cmsPadLeftToSize(char *buf, int size)
{
int i,j;
int len = strlen(buf);
for (i = size - 1, j = size - len ; i - j >= 0 ; i--) {
buf[i] = buf[i - j];
}
for ( ; i >= 0 ; i--) {
buf[i] = ' ';
}
buf[size] = 0;
}
static void cmsPadToSize(char *buf, int size)
{
// Make absolutely sure the string terminated.
buf[size] = 0x00,
#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
cmsPadLeftToSize(buf, size);
#else
smallScreen ? cmsPadLeftToSize(buf, size) : cmsPadRightToSize(buf, size);
#endif
}
static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row, uint8_t maxSize)
{
int colpos;
int cnt;
cmsPadToSize(buff, maxSize);
#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
colpos = rightMenuColumn - maxSize;
#else
colpos = smallScreen ? rightMenuColumn - maxSize : rightMenuColumn;
#endif
cnt = displayWrite(pDisplay, colpos, row, buff);
return cnt;
}
static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
{
#define CMS_DRAW_BUFFER_LEN 10u
char buff[CMS_DRAW_BUFFER_LEN];
#define CMS_DRAW_BUFFER_LEN 12
#define CMS_NUM_FIELD_LEN 5
char buff[CMS_DRAW_BUFFER_LEN +1]; // Make room for null terminator.
int cnt = 0;
if (smallScreen) {
row++;
}
switch (p->type) {
case OME_String:
if (IS_PRINTVALUE(p) && p->data) {
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data);
strncpy(buff, p->data, CMS_DRAW_BUFFER_LEN);
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN);
CLR_PRINTVALUE(p);
}
break;
@ -278,19 +346,19 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
case OME_Funcall:
if (IS_PRINTVALUE(p)) {
int colPos = RIGHT_MENU_COLUMN(pDisplay);
buff[0]= 0x0;
if ((p->type == OME_Submenu) && p->func && (p->flags & OPTSTRING)) {
// Special case of sub menu entry with optional value display.
char *str = ((CMSMenuOptFuncPtr)p->func)();
cnt = displayWrite(pDisplay, colPos, row, str);
colPos += strlen(str);
strncpy( buff, str, CMS_DRAW_BUFFER_LEN);
}
strncat(buff, ">", CMS_DRAW_BUFFER_LEN);
cnt += displayWrite(pDisplay, colPos, row, ">");
row = smallScreen ? row - 1 : row;
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, strlen(buff));
CLR_PRINTVALUE(p);
}
break;
@ -298,10 +366,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
case OME_Bool:
if (IS_PRINTVALUE(p) && p->data) {
if (*((uint8_t *)(p->data))) {
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
strcpy(buff, "YES");
} else {
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
strcpy(buff, "NO ");
}
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3);
CLR_PRINTVALUE(p);
}
break;
@ -310,9 +380,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
if (IS_PRINTVALUE(p)) {
OSD_TAB_t *ptr = p->data;
char * str = (char *)ptr->names[*ptr->val];
memcpy(buff, str, MAX(CMS_DRAW_BUFFER_LEN, strlen(str)));
cmsPadToSize(buff, CMS_DRAW_BUFFER_LEN);
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
strncpy(buff, str, CMS_DRAW_BUFFER_LEN);
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN);
CLR_PRINTVALUE(p);
}
break;
@ -323,10 +392,11 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
uint16_t *val = (uint16_t *)p->data;
if (VISIBLE(*val)) {
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
strcpy(buff, "YES");
} else {
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
strcpy(buff, "NO ");
}
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3);
CLR_PRINTVALUE(p);
}
break;
@ -336,8 +406,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
if (IS_PRINTVALUE(p) && p->data) {
OSD_UINT8_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPadToSize(buff, 5);
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
CLR_PRINTVALUE(p);
}
break;
@ -346,8 +415,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
if (IS_PRINTVALUE(p) && p->data) {
OSD_INT8_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPadToSize(buff, 5);
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
CLR_PRINTVALUE(p);
}
break;
@ -356,8 +424,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
if (IS_PRINTVALUE(p) && p->data) {
OSD_UINT16_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPadToSize(buff, 5);
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
CLR_PRINTVALUE(p);
}
break;
@ -366,8 +433,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
if (IS_PRINTVALUE(p) && p->data) {
OSD_UINT16_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPadToSize(buff, 5);
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
CLR_PRINTVALUE(p);
}
break;
@ -376,8 +442,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
if (IS_PRINTVALUE(p) && p->data) {
OSD_FLOAT_t *ptr = p->data;
cmsFormatFloat(*ptr->val * ptr->multipler, buff);
cmsPadToSize(buff, 5);
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ???
cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
CLR_PRINTVALUE(p);
}
break;
@ -385,7 +450,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
case OME_Label:
if (IS_PRINTVALUE(p) && p->data) {
// A label with optional string, immediately following text
cnt = displayWrite(pDisplay, LEFT_MENU_COLUMN + 2 + strlen(p->text), row, p->data);
cnt = displayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, p->data);
CLR_PRINTVALUE(p);
}
break;
@ -399,8 +464,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
// Fall through
default:
#ifdef CMS_MENU_DEBUG
// Shouldn't happen. Notify creator of this menu content.
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT");
// Shouldn't happen. Notify creator of this menu content
#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
cnt = displayWrite(pDisplay, rightMenuColumn - 6, row, "BADENT");
#else.
cnt = displayWrite(pDisplay, rightMenuColumn, row, "BADENT");
#endif
#endif
break;
}
@ -415,7 +484,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
uint8_t i;
OSD_Entry *p;
uint8_t top = (pDisplay->rows - pageMaxRow) / 2 - 1;
uint8_t top = smallScreen ? 1 : (pDisplay->rows - pageMaxRow)/2;
// Polled (dynamic) value display denominator.
@ -450,14 +519,14 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
cmsPageDebug();
if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) {
room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, " ");
room -= displayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, " ");
}
if (room < 30)
return;
if (pDisplay->cursorRow != currentCtx.cursorRow) {
room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCtx.cursorRow + top, " >");
room -= displayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, ">");
pDisplay->cursorRow = currentCtx.cursorRow;
}
@ -465,25 +534,23 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
return;
// Print text labels
for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
for (i = 0, p = pageTop; i < maxMenuItems && p->type != OME_END; i++, p++) {
if (IS_PRINTLABEL(p)) {
uint8_t coloff = LEFT_MENU_COLUMN;
coloff += (p->type == OME_Label) ? 1 : 2;
room -= displayWrite(pDisplay, coloff, i + top, p->text);
uint8_t coloff = leftMenuColumn;
coloff += (p->type == OME_Label) ? 0 : 1;
room -= displayWrite(pDisplay, coloff, top + i * linesPerMenuItem, p->text);
CLR_PRINTLABEL(p);
if (room < 30)
return;
}
}
// Print values
// XXX Polled values at latter positions in the list may not be
// XXX printed if not enough room in the middle of the list.
for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
if (IS_PRINTVALUE(p)) {
room -= cmsDrawMenuEntry(pDisplay, p, top + i);
room -= cmsDrawMenuEntry(pDisplay, p, top + i * linesPerMenuItem);
if (room < 30)
return;
}
@ -492,9 +559,10 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
static void cmsMenuCountPage(displayPort_t *pDisplay)
{
UNUSED(pDisplay);
const OSD_Entry *p;
for (p = currentCtx.menu->entries; p->type != OME_END; p++);
pageCount = (p - currentCtx.menu->entries - 1) / MAX_MENU_ITEMS(pDisplay) + 1;
pageCount = (p - currentCtx.menu->entries - 1) / maxMenuItems + 1;
}
STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay); // Forward; will be resolved after merging
@ -538,9 +606,9 @@ long cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
// currentCtx.cursorRow has been saved as absolute; convert it back to page + relative
int8_t cursorAbs = currentCtx.cursorRow;
currentCtx.cursorRow = cursorAbs % MAX_MENU_ITEMS(pDisplay);
currentCtx.cursorRow = cursorAbs % maxMenuItems;
cmsMenuCountPage(pDisplay);
cmsPageSelect(pDisplay, cursorAbs / MAX_MENU_ITEMS(pDisplay));
cmsPageSelect(pDisplay, cursorAbs / maxMenuItems);
}
cmsPageDebug();
@ -594,6 +662,25 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
}
}
displayGrab(pCurrentDisplay); // grab the display for use by the CMS
if ( pCurrentDisplay->cols < NORMAL_SCREEN_MIN_COLS) {
smallScreen = true;
linesPerMenuItem = 2;
leftMenuColumn = 0;
rightMenuColumn = pCurrentDisplay->cols;
maxMenuItems = (pCurrentDisplay->rows) / linesPerMenuItem;
} else {
smallScreen = false;
linesPerMenuItem = 1;
leftMenuColumn = 2;
#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
rightMenuColumn = pCurrentDisplay->cols - 2;
#else
rightMenuColumn = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
#endif
maxMenuItems = pCurrentDisplay->rows - 2;
}
cmsMenuChange(pCurrentDisplay, currentCtx.menu);
}

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

@ -17,7 +17,7 @@ long cmsMenuChange(displayPort_t *pPort, const void *ptr);
long cmsMenuExit(displayPort_t *pPort, const void *ptr);
void cmsUpdate(uint32_t currentTimeUs);
#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID"
#define CMS_STARTUP_HELP_TEXT1 "MENU:THR MID"
#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT"
#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP"

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

@ -53,20 +53,22 @@
// Info
static char infoGitRev[GIT_SHORT_REVISION_LENGTH];
static char infoGitRev[GIT_SHORT_REVISION_LENGTH + 1];
static char infoTargetName[] = __TARGET__;
#include "interface/msp_protocol.h" // XXX for FC identification... not available elsewhere
static long cmsx_InfoInit(void)
{
for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
int i;
for ( i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f')
infoGitRev[i] = shortGitRevision[i] - 'a' + 'A';
else
infoGitRev[i] = shortGitRevision[i];
}
infoGitRev[i] = 0x0; // Terminate string
return 0;
}

6
src/main/fc/fc_init.c
View File

@ -107,6 +107,8 @@
#include "io/vtx_smartaudio.h"
#include "io/vtx_tramp.h"
#include "io/displayport_srxl.h"
#include "scheduler/scheduler.h"
#include "sensors/acceleration.h"
@ -597,6 +599,10 @@ void init(void)
}
#endif
#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY)
// Register the srxl Textgen telemetry sensor as a displayport device
cmsDisplayPortRegister(displayPortSrxlInit());
#endif
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {

135
src/main/io/displayport_srxl.c Normal file
View File

@ -0,0 +1,135 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
#include "common/utils.h"
#include "drivers/display.h"
#include "cms/cms.h"
#include "telemetry/srxl.h"
static displayPort_t srxlDisplayPort;
static int srxlDrawScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int srxlScreenSize(const displayPort_t *displayPort)
{
return displayPort->rows * displayPort->cols;
}
static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c)
{
return (spektrumTmTextGenPutChar(col, row, c));
UNUSED(displayPort);
}
static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s)
{
while (*s) {
srxlWriteChar(displayPort, col++, row, *(s++));
}
return 0;
}
static int srxlClearScreen(displayPort_t *displayPort)
{
for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) {
srxlWriteChar(displayPort, col, row, ' ');
}
}
srxlWriteString(displayPort, 1, 0, "BETAFLIGHT");
if ( displayPort->grabCount == 0 ) {
srxlWriteString(displayPort, 0, 3, CMS_STARTUP_HELP_TEXT1);
srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT2);
srxlWriteString(displayPort, 2, 5, CMS_STARTUP_HELP_TEXT3);
}
return 0;
}
static bool srxlIsTransferInProgress(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return false;
}
static int srxlHeartbeat(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static void srxlResync(displayPort_t *displayPort)
{
UNUSED(displayPort);
}
static uint32_t srxlTxBytesFree(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return UINT32_MAX;
}
static int srxlGrab(displayPort_t *displayPort)
{
return displayPort->grabCount = 1;
}
static int srxlRelease(displayPort_t *displayPort)
{
int cnt = displayPort->grabCount = 0;
srxlClearScreen(displayPort);
return cnt;
}
static const displayPortVTable_t srxlVTable = {
.grab = srxlGrab,
.release = srxlRelease,
.clearScreen = srxlClearScreen,
.drawScreen = srxlDrawScreen,
.screenSize = srxlScreenSize,
.writeString = srxlWriteString,
.writeChar = srxlWriteChar,
.isTransferInProgress = srxlIsTransferInProgress,
.heartbeat = srxlHeartbeat,
.resync = srxlResync,
.txBytesFree = srxlTxBytesFree
};
displayPort_t *displayPortSrxlInit()
{
srxlDisplayPort.device = NULL;
displayInit(&srxlDisplayPort, &srxlVTable);
srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
return &srxlDisplayPort;
}
#endif

20
src/main/io/displayport_srxl.h Normal file
View File

@ -0,0 +1,20 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
displayPort_t *displayPortSrxlInit();

3
src/main/rx/spektrum.c
View File

@ -56,6 +56,7 @@
#define SPEKTRUM_1024_CHANNEL_COUNT 7
#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
#define SPEKTRUM_TELEMETRY_FRAME_DELAY 1000 // Gap between received Rc frame and transmited TM frame, uS
#define SPEKTRUM_BAUDRATE 115200
@ -473,7 +474,7 @@ static uint8_t spektrumFrameStatus(void)
/* only process if srxl enabled, some data in buffer AND servos in phase 0 */
if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80)) {
dispatchAdd(&srxlTelemetryDispatch, 100);
dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY);
}
return RX_FRAME_COMPLETE;
}

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

@ -138,6 +138,7 @@
#define USE_SPEKTRUM_FAKE_RSSI
#define USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
#define USE_SPEKTRUM_VTX_CONTROL
#define USE_SPEKTRUM_CMS_TELEMETRY
#endif
#endif

167
src/main/telemetry/srxl.c
View File

@ -50,8 +50,7 @@
#include "telemetry/telemetry.h"
#include "telemetry/srxl.h"
#define SRXL_CYCLETIME_US 100000 // 100ms, 10 Hz
#define SRXL_CYCLETIME_US 33000 // 33ms, 30 Hz
#define SRXL_ADDRESS_FIRST 0xA5
#define SRXL_ADDRESS_SECOND 0x80
@ -104,8 +103,10 @@ typedef struct
UINT16 rxVoltage; // Volts, 0.01V increments
} STRU_TELE_QOS;
*/
void srxlFrameQos(sbuf_t *dst)
bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16BigEndian(dst, 0xFFFF); // A
@ -115,6 +116,7 @@ void srxlFrameQos(sbuf_t *dst)
sbufWriteU16BigEndian(dst, 0xFFFF); // F
sbufWriteU16BigEndian(dst, 0xFFFF); // H
sbufWriteU16BigEndian(dst, 0xFFFF); // rxVoltage
return true;
}
/*
@ -130,8 +132,10 @@ typedef struct
// If only 1 antenna, set B = A
} STRU_TELE_RPM;
*/
void srxlFrameRpm(sbuf_t *dst)
bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16BigEndian(dst, 0xFFFF); // pulse leading edges
@ -144,6 +148,7 @@ void srxlFrameRpm(sbuf_t *dst)
sbufWriteU16BigEndian(dst, 0xFFFF);
sbufWriteU16BigEndian(dst, 0xFFFF);
sbufWriteU16BigEndian(dst, 0xFFFF);
return true;
}
/*
@ -161,42 +166,156 @@ typedef struct
} STRU_TELE_FP_MAH;
*/
void srxlFrameFlightPackCurrent(sbuf_t *dst)
#define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s
bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs)
{
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16(dst, getAmperage() / 10);
sbufWriteU16(dst, getMAhDrawn());
sbufWriteU16(dst, 0x7fff); // temp A
sbufWriteU16(dst, 0xffff);
sbufWriteU16(dst, 0xffff);
sbufWriteU16(dst, 0x7fff); // temp B
sbufWriteU16(dst, 0xffff);
uint16_t amps = getAmperage() / 10;
uint16_t mah = getMAhDrawn();
static uint16_t sentAmps;
static uint16_t sentMah;
static timeUs_t lastTimeSentFPmAh = 0;
timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh;
if ( (amps != sentAmps) || (mah != sentMah) ||
keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) {
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16(dst, amps);
sbufWriteU16(dst, mah);
sbufWriteU16(dst, 0x7fff); // temp A
sbufWriteU16(dst, 0xffff);
sbufWriteU16(dst, 0xffff);
sbufWriteU16(dst, 0x7fff); // temp B
sbufWriteU16(dst, 0xffff);
sentAmps = amps;
sentMah = mah;
lastTimeSentFPmAh = currentTimeUs;
return true;
}
return false;
}
// schedule array to decide how often each type of frame is sent
#define SRXL_SCHEDULE_COUNT_MAX 3
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
typedef void (*srxlScheduleFnPtr)(sbuf_t *dst);
const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = {
// Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display.
#define SPEKTRUM_SRXL_DEVICE_TEXTGEN (0x0C) // Text Generator
#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS (9) // Text Generator ROWS
#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS (13) // Text Generator COLS
/*
typedef struct
{
UINT8 identifier;
UINT8 sID; // Secondary ID
UINT8 lineNumber; // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen)
char text[13]; // 0-terminated text when < 13 chars
} STRU_SPEKTRUM_SRXL_TEXTGEN;
*/
#if ( SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS > SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS )
static char srxlTextBuff[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS][SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS];
static bool lineSent[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS];
#else
static char srxlTextBuff[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS][SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS];
static bool lineSent[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS];
#endif
//**************************************************************************
// API Running in external client task context. E.g. in the CMS task
int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c)
{
if (row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS && col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS) {
srxlTextBuff[row][col] = c;
lineSent[row] = false;
}
return 0;
}
//**************************************************************************
bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
static uint8_t lineNo = 0;
int lineCount = 0;
// Skip already sent lines...
while (lineSent[lineNo] &&
lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) {
lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
lineCount++;
}
sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU8(dst, lineNo);
sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS);
lineSent[lineNo] = true;
lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
// Always send something, Always one user frame after the two mandatory frames
// I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true
// too keep the "Waltz" sequence intact.
return true;
}
#endif
// Schedule array to decide how often each type of frame is sent
// The frames are scheduled in sets of 3 frames, 2 mandatory and 1 user frame.
// The user frame type is cycled for each set.
// Example. QOS, RPM,.CURRENT, QOS, RPM, TEXT. QOS, RPM, CURRENT, etc etc
#define SRXL_SCHEDULE_MANDATORY_COUNT 2 // Mandatory QOS and RPM sensors
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
#define SRXL_SCHEDULE_CMS_COUNT 1
#else
#define SRXL_SCHEDULE_CMS_COUNT 0
#endif
#define SRXL_SCHEDULE_USER_COUNT (1 + SRXL_SCHEDULE_CMS_COUNT)
#define SRXL_SCHEDULE_COUNT_MAX (SRXL_SCHEDULE_MANDATORY_COUNT + 1)
#define SRXL_TOTAL_COUNT (SRXL_SCHEDULE_MANDATORY_COUNT + SRXL_SCHEDULE_USER_COUNT)
typedef bool (*srxlScheduleFnPtr)(sbuf_t *dst, timeUs_t currentTimeUs);
const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_TOTAL_COUNT] = {
/* must send srxlFrameQos, Rpm and then alternating items of our own */
srxlFrameQos,
srxlFrameRpm,
srxlFrameFlightPackCurrent
srxlFrameFlightPackCurrent,
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
srxlFrameText,
#endif
};
static void processSrxl(void)
static void processSrxl(timeUs_t currentTimeUs)
{
static uint8_t srxlScheduleIndex = 0;
static uint8_t srxlScheduleUserIndex = 0;
sbuf_t srxlPayloadBuf;
sbuf_t *dst = &srxlPayloadBuf;
srxlScheduleFnPtr srxlFnPtr;
if (srxlScheduleIndex < SRXL_SCHEDULE_MANDATORY_COUNT) {
srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex];
} else {
srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex + srxlScheduleUserIndex];
srxlScheduleUserIndex = (srxlScheduleUserIndex + 1) % SRXL_SCHEDULE_USER_COUNT;
}
srxlScheduleFnPtr srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex];
if (srxlFnPtr) {
srxlInitializeFrame(dst);
srxlFnPtr(dst);
srxlFinalize(dst);
if (srxlFnPtr(dst, currentTimeUs)) {
srxlFinalize(dst);
}
}
srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX;
}
@ -227,7 +346,7 @@ void handleSrxlTelemetry(timeUs_t currentTimeUs)
// Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
if (currentTimeUs >= srxlLastCycleTime + SRXL_CYCLETIME_US) {
srxlLastCycleTime = currentTimeUs;
processSrxl();
processSrxl(currentTimeUs);
}
}
#endif

7
src/main/telemetry/srxl.h
View File

@ -22,3 +22,10 @@
void initSrxlTelemetry(void);
bool checkSrxlTelemetryState(void);
void handleSrxlTelemetry(timeUs_t currentTimeUs);
#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS 9
#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 12 // Airware 1.20
//#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 13 // Airware 1.21
#define SPEKTRUM_SRXL_TEXTGEN_CLEAR_SCREEN 255
int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c);

2
src/test/unit/osd_unittest.cc
View File

@ -188,7 +188,7 @@ TEST(OsdTest, TestInit)
// then
// display buffer should contain splash screen
displayPortTestBufferSubstring(7, 8, "MENU: THR MID");
displayPortTestBufferSubstring(7, 8, "MENU:THR MID");
displayPortTestBufferSubstring(11, 9, "+ YAW LEFT");
displayPortTestBufferSubstring(11, 10, "+ PITCH UP");