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

Merge remote-tracking branch 'betaflight/master' into bfdev-spi-refactor

Browse source
This commit is contained in:
jflyper 2017-11-23 14:47:12 +09:00
commit be7c8e384e
64 changed files with 1158 additions and 586 deletions
Download patch file Download diff file Expand all files Collapse all files

42
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 \
@ -198,6 +199,22 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
common/filter.c \
common/maths.c \
common/typeconversion.c \
drivers/accgyro/accgyro_adxl345.c \
drivers/accgyro/accgyro_bma280.c \
drivers/accgyro/accgyro_fake.c \
drivers/accgyro/accgyro_l3g4200d.c \
drivers/accgyro/accgyro_l3gd20.c \
drivers/accgyro/accgyro_lsm303dlhc.c \
drivers/accgyro/accgyro_mma845x.c \
drivers/accgyro/accgyro_mpu3050.c \
drivers/accgyro/accgyro_mpu6050.c \
drivers/accgyro/accgyro_mpu6500.c \
drivers/accgyro/accgyro_mpu.c \
drivers/accgyro/accgyro_spi_bmi160.c \
drivers/accgyro/accgyro_spi_icm20689.c \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \
drivers/adc.c \
drivers/buf_writer.c \
drivers/bus.c \
@ -236,19 +253,31 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
sensors/gyroanalyse.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC) \
drivers/light_ws2811strip.c \
io/displayport_max7456.c \
io/osd.c \
io/osd_slave.c
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
bus_bst_stm32f30x.c \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_fake.c \
drivers/barometer/barometer_ms5611.c \
drivers/bus_i2c_config.c \
drivers/bus_spi_config.c \
drivers/bus_spi_pinconfig.c \
drivers/compass/compass_ak8963.c \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_fake.c \
drivers/compass/compass_hmc5883l.c \
drivers/display_ug2864hsweg01.c \
drivers/inverter.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_hal.c \
drivers/serial_escserial.c \
drivers/serial_pinconfig.c \
drivers/serial_tcp.c \
drivers/serial_uart_init.c \
drivers/serial_uart_pinconfig.c \
drivers/serial_usb_vcp.c \
drivers/transponder_ir.c \
drivers/vtx_rtc6705_soft_spi.c \
drivers/vtx_rtc6705.c \
drivers/vtx_common.c \
@ -257,12 +286,15 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
config/feature.c \
config/parameter_group.c \
config/config_streamer.c \
i2c_bst.c \
interface/cli.c \
interface/settings.c \
io/dashboard.c \
io/osd.c \
io/serial_4way.c \
io/serial_4way_avrootloader.c \
io/serial_4way_stk500v2.c \
io/dashboard.c \
io/transponder_ir.c \
msp/msp_serial.c \
cms/cms.c \
cms/cms_menu_blackbox.c \

1
src/main/blackbox/blackbox.c
View file

@ -54,7 +54,6 @@
#include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"

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;
}

13
src/main/drivers/accgyro/accgyro.h
View file

@ -53,7 +53,6 @@ typedef struct gyroDev_s {
sensorGyroInitFuncPtr initFn; // initialize function
sensorGyroReadFuncPtr readFn; // read 3 axis data function
sensorGyroReadDataFuncPtr temperatureFn; // read temperature if available
sensorGyroUpdateFuncPtr updateFn;
extiCallbackRec_t exti;
busDevice_t bus;
float scale; // scalefactor
@ -66,14 +65,14 @@ typedef struct gyroDev_s {
gyroRateKHz_e gyroRateKHz;
uint8_t mpuDividerDrops;
bool dataReady;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
pthread_mutex_t lock;
#endif
sensor_align_e gyroAlign;
mpuDetectionResult_t mpuDetectionResult;
ioTag_t mpuIntExtiTag;
mpuConfiguration_t mpuConfiguration;
bool gyro_high_fsr;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
pthread_mutex_t lock;
#endif
} gyroDev_t;
typedef struct accDev_s {
@ -84,13 +83,13 @@ typedef struct accDev_s {
int16_t ADCRaw[XYZ_AXIS_COUNT];
char revisionCode; // a revision code for the sensor, if known
bool dataReady;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
pthread_mutex_t lock;
#endif
sensor_align_e accAlign;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
bool acc_high_fsr;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
pthread_mutex_t lock;
#endif
} accDev_t;
static inline void accDevLock(accDev_t *acc)

91
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -64,23 +64,18 @@ mpuResetFnPtr mpuResetFn;
#define MPU_INQUIRY_MASK 0x7E
#ifdef USE_I2C
static void mpu6050FindRevision(gyroDev_t *gyro)
{
bool ack;
UNUSED(ack);
uint8_t readBuffer[6];
uint8_t revision;
uint8_t productId;
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_XA_OFFS_H, readBuffer, 6);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
// determine product ID and revision
uint8_t readBuffer[6];
bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_XA_OFFS_H, readBuffer, 6);
uint8_t revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (ack && revision) {
// Congrats, these parts are better
if (revision == 1) {
gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else if (revision == 2) {
@ -91,9 +86,10 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} else {
ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_PRODUCT_ID, &productId, 1);
uint8_t productId;
ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_PRODUCT_ID, &productId, 1);
revision = productId & 0x0F;
if (!revision) {
if (!ack || revision == 0) {
failureMode(FAILURE_ACC_INCOMPATIBLE);
} else if (revision == 4) {
gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
@ -102,6 +98,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
}
}
}
#endif
/*
* Gyro interrupt service routine
@ -117,19 +114,14 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb)
#endif
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
gyro->dataReady = true;
if (gyro->updateFn) {
gyro->updateFn(gyro);
}
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
const uint32_t now2Us = micros();
debug[1] = (uint16_t)(now2Us - nowUs);
#endif
}
#endif
static void mpuIntExtiInit(gyroDev_t *gyro)
{
#if defined(MPU_INT_EXTI)
if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
return;
}
@ -156,31 +148,14 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(mpuIntIO, true);
#endif
#else
UNUSED(gyro);
#endif
}
static bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t* data, uint8_t length)
{
UNUSED(bus);
const bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
return ack;
}
static bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
UNUSED(bus);
const bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
return ack;
}
#endif // MPU_INT_EXTI
bool mpuAccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, data, 6);
const bool ack = busReadRegisterBuffer(&acc->bus, MPU_RA_ACCEL_XOUT_H, data, 6);
if (!ack) {
return false;
}
@ -192,18 +167,11 @@ bool mpuAccRead(accDev_t *acc)
return true;
}
void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
{
ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
gyro->updateFn = updateFn;
}
}
bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_GYRO_XOUT_H, data, 6);
const bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_GYRO_XOUT_H, data, 6);
if (!ack) {
return false;
}
@ -250,8 +218,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
sensor = mpu6000SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
return true;
}
#endif
@ -267,8 +233,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
return true;
}
#endif
@ -283,8 +247,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
sensor = mpu9250SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
return true;
}
@ -300,8 +262,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
sensor = icm20649SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
return true;
}
#endif
@ -316,8 +276,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
sensor = icm20689SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
return true;
}
#endif
@ -332,8 +290,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
sensor = bmi160Detect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
return true;
}
#endif
@ -347,27 +303,27 @@ void mpuDetect(gyroDev_t *gyro)
// MPU datasheet specifies 30ms.
delay(35);
uint8_t sig = 0;
#ifdef USE_I2C
gyro->bus.bustype = BUSTYPE_I2C;
gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE;
gyro->bus.busdev_u.i2c.address = MPU_ADDRESS;
bool ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1);
uint8_t sig = 0;
bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1);
#else
bool ack = false;
#endif
if (ack) {
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
} else {
if (!ack) {
#ifdef USE_SPI
detectSPISensorsAndUpdateDetectionResult(gyro);
#endif
return;
}
#ifdef USE_I2C
// If an MPU3050 is connected sig will contain 0.
uint8_t inquiryResult;
ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1);
ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
@ -381,9 +337,14 @@ void mpuDetect(gyroDev_t *gyro)
} else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
}
#endif
}
void mpuGyroInit(gyroDev_t *gyro)
{
#ifdef MPU_INT_EXTI
mpuIntExtiInit(gyro);
#else
UNUSED(gyro);
#endif
}

5
src/main/drivers/accgyro/accgyro_mpu.h
View file

@ -140,15 +140,11 @@
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t* data, uint8_t length);
typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
typedef void (*mpuResetFnPtr)(void);
extern mpuResetFnPtr mpuResetFn;
typedef struct mpuConfiguration_s {
mpuReadRegisterFnPtr readFn;
mpuWriteRegisterFnPtr writeFn;
mpuResetFnPtr resetFn;
} mpuConfiguration_t;
@ -213,4 +209,3 @@ bool mpuAccRead(struct accDev_s *acc);
bool mpuGyroRead(struct gyroDev_s *gyro);
bool mpuGyroReadSPI(struct gyroDev_s *gyro);
void mpuDetect(struct gyroDev_s *gyro);
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);

19
src/main/drivers/accgyro/accgyro_mpu3050.c
View file

@ -50,25 +50,24 @@
static void mpu3050Init(gyroDev_t *gyro)
{
bool ack;
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_SMPLRT_DIV, 0);
if (!ack)
const bool ack = busWriteRegister(&gyro->bus, MPU3050_SMPLRT_DIV, 0);
if (!ack) {
failureMode(FAILURE_ACC_INIT);
}
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
busWriteRegister(&gyro->bus, MPU3050_INT_CFG, 0);
busWriteRegister(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
busWriteRegister(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static bool mpu3050GyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_GYRO_OUT, data, 6);
const bool ack = busReadRegisterBuffer(&gyro->bus, MPU3050_GYRO_OUT, data, 6);
if (!ack) {
return false;
}
@ -83,7 +82,7 @@ static bool mpu3050GyroRead(gyroDev_t *gyro)
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{
uint8_t buf[2];
if (!gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_TEMP_OUT, buf, 2)) {
if (!busReadRegisterBuffer(&gyro->bus, MPU3050_TEMP_OUT, buf, 2)) {
return false;
}

16
src/main/drivers/accgyro/accgyro_mpu6050.c
View file

@ -77,23 +77,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff.
// Accel scale 8g (4096 LSB/g)
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG,
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
}

22
src/main/drivers/accgyro/accgyro_mpu6500.c
View file

@ -54,34 +54,34 @@ void mpu6500GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
#ifdef USE_MPU9250_MAG
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
#else
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
#endif
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
#endif
delay(15);
}

26
src/main/drivers/accgyro/accgyro_spi_icm20649.c
View file

@ -95,12 +95,12 @@ void icm20649AccInit(accDev_t *acc)
spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD);
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15);
const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
spiBusWriteRegister(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
delay(15);
acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
delay(15);
}
@ -123,31 +123,31 @@ void icm20649GyroInit(gyroDev_t *gyro)
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); // ensure proper speed
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15);
const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3;
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
// back to bank 0
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN
spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
#endif
}
@ -187,7 +187,7 @@ bool icm20649AccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = acc->mpuConfiguration.readFn(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
const bool ack = spiBusReadRegisterBuffer(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
if (!ack) {
return false;
}

22
src/main/drivers/accgyro/accgyro_spi_icm20689.c
View file

@ -119,32 +119,32 @@ void icm20689GyroInit(gyroDev_t *gyro)
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
// spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
// delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
spiBusWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
// spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
spiBusWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD);

6
src/main/drivers/bus.h
View file

@ -42,11 +42,11 @@ typedef struct busDevice_s {
struct deviceI2C_s {
I2CDevice device;
uint8_t address;
} i2c;
struct deviceMpuSlave_s {
} i2c;
struct deviceMpuSlave_s {
const struct busDevice_s *master;
uint8_t address;
} mpuSlave;
} mpuSlave;
} busdev_u;
} busDevice_t;

1
src/main/drivers/bus_spi.c
View file

@ -171,6 +171,7 @@ uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg)
void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance)
{
bus->bustype = BUSTYPE_SPI;
bus->busdev_u.spi.instance = instance;
}
#endif

2
src/main/drivers/flash_m25p16.c
View file

@ -48,6 +48,7 @@
#define JEDEC_ID_MICRON_M25P16 0x202015
#define JEDEC_ID_MICRON_N25Q064 0x20BA17
#define JEDEC_ID_MICRON_N25Q128 0x20ba18
#define JEDEC_ID_WINBOND_W25Q16 0xEF4015
#define JEDEC_ID_WINBOND_W25Q64 0xEF4017
#define JEDEC_ID_WINBOND_W25Q128 0xEF4018
#define JEDEC_ID_WINBOND_W25Q256 0xEF4019
@ -162,6 +163,7 @@ static bool m25p16_readIdentification(void)
// All supported chips use the same pagesize of 256 bytes
switch (chipID) {
case JEDEC_ID_WINBOND_W25Q16:
case JEDEC_ID_MICRON_M25P16:
geometry.sectors = 32;
geometry.pagesPerSector = 256;

2
src/main/drivers/pwm_output.h
View file

@ -61,6 +61,8 @@ typedef enum {
DSHOT_CMD_LED1_OFF, // BLHeli32 only
DSHOT_CMD_LED2_OFF, // BLHeli32 only
DSHOT_CMD_LED3_OFF, // BLHeli32 only
DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off
DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off
DSHOT_CMD_MAX = 47
} dshotCommands_e;

1
src/main/drivers/sensor.h
View file

@ -42,5 +42,4 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc);
struct gyroDev_s;
typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroUpdateFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data);

29
src/main/fc/config.c
View file

@ -124,10 +124,15 @@ PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig,
.name = { 0 }
);
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2);
#ifndef USE_OSD_SLAVE
#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
#ifdef USE_OSD_SLAVE
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.debug_mode = DEBUG_MODE,
.task_statistics = true,
.boardIdentifier = TARGET_BOARD_IDENTIFIER
);
#else
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.pidProfileIndex = 0,
.activeRateProfile = 0,
@ -137,24 +142,8 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.powerOnArmingGraceTime = 5,
.boardIdentifier = TARGET_BOARD_IDENTIFIER
);
#else
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.pidProfileIndex = 0,
.activeRateProfile = 0,
.debug_mode = DEBUG_MODE,
.task_statistics = true,
.boardIdentifier = TARGET_BOARD_IDENTIFIER
);
#endif
#endif
#ifdef USE_OSD_SLAVE
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.debug_mode = DEBUG_MODE,
.task_statistics = true,
.boardIdentifier = TARGET_BOARD_IDENTIFIER
);
#endif
#ifdef USE_ADC
PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0);
@ -312,7 +301,7 @@ void activateConfig(void)
useRcControlsConfig(currentPidProfile);
useAdjustmentConfig(currentPidProfile);
#ifdef USE_GPS
#ifdef USE_NAV
gpsUsePIDs(currentPidProfile);
#endif

2
src/main/fc/config.h
View file

@ -75,9 +75,7 @@ typedef struct systemConfig_s {
uint8_t activeRateProfile;
uint8_t debug_mode;
uint8_t task_statistics;
#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
uint8_t cpu_overclock;
#endif
uint8_t powerOnArmingGraceTime; // in seconds
char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER) + 1];
} systemConfig_t;

10
src/main/fc/fc_core.c
View file

@ -407,7 +407,9 @@ void processRx(timeUs_t currentTimeUs)
const throttleStatus_e throttleStatus = calculateThrottleStatus();
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcData[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
if (rcData[THROTTLE] >= rxConfig()->airModeActivateThreshold) {
airmodeIsActivated = true; // Prevent Iterm from being reset
}
} else {
airmodeIsActivated = false;
}
@ -551,7 +553,7 @@ void processRx(timeUs_t currentTimeUs)
}
#endif
#ifdef USE_GPS
#ifdef USE_NAV
if (sensors(SENSOR_GPS)) {
updateGpsWaypointsAndMode();
}
@ -615,7 +617,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
}
#endif
#if defined(USE_BARO) || defined(USE_SONAR)
#if defined(USE_ALT_HOLD)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
@ -647,7 +649,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
processRcCommand();
#ifdef USE_GPS
#ifdef USE_NAV
if (sensors(SENSOR_GPS)) {
if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
updateGpsStateForHomeAndHoldMode();

1
src/main/fc/fc_dispatch.c
View file

@ -55,6 +55,7 @@ void dispatchProcess(uint32_t currentTime)
void dispatchAdd(dispatchEntry_t *entry, int delayUs)
{
uint32_t delayedUntil = micros() + delayUs;
entry->delayedUntil = delayedUntil;
dispatchEntry_t **p = &head;
while (*p && cmp32((*p)->delayedUntil, delayedUntil) < 0)
p = &(*p)->next;

8
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,11 +599,17 @@ 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)) {
gpsInit();
#ifdef USE_NAV
navigationInit();
#endif
}
#endif

6
src/main/fc/fc_tasks.c
View file

@ -140,12 +140,13 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
processRx(currentTimeUs);
isRXDataNew = true;
#if !defined(USE_BARO) && !defined(USE_SONAR)
#if !defined(USE_ALT_HOLD)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
#endif
updateArmingStatus();
#ifdef USE_ALT_HOLD
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
updateAltHoldState();
@ -157,6 +158,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
updateSonarAltHoldState();
}
#endif
#endif // USE_ALT_HOLD
}
#endif
@ -196,7 +198,7 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs)
) {
calculateEstimatedAltitude(currentTimeUs);
}}
#endif
#endif // USE_BARO || USE_SONAR
#ifdef USE_TELEMETRY
static void taskTelemetry(timeUs_t currentTimeUs)

69
src/main/fc/rc_controls.c
View file

@ -60,7 +60,6 @@
#include "scheduler/scheduler.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "flight/failsafe.h"
static pidProfile_t *pidProfile;
@ -243,13 +242,13 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
return;
}
// Multiple configuration profiles
// Change PID profile
int newPidProfile = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) { // ROLL left -> Profile 1
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) { // ROLL left -> PID profile 1
newPidProfile = 1;
} else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) { // PITCH up -> Profile 2
} else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) { // PITCH up -> PID profile 2
newPidProfile = 2;
} else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) { // ROLL right -> Profile 3
} else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) { // ROLL right -> PID profile 3
newPidProfile = 3;
}
if (newPidProfile) {
@ -275,28 +274,46 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
}
// Accelerometer Trim
rollAndPitchTrims_t accelerometerTrimsDelta;
memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) {
// in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims
rollAndPitchTrims_t accelerometerTrimsDelta;
memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
bool shouldApplyRollAndPitchTrimDelta = false;
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
accelerometerTrimsDelta.values.pitch = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
accelerometerTrimsDelta.values.pitch = -2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
accelerometerTrimsDelta.values.roll = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
accelerometerTrimsDelta.values.roll = -2;
shouldApplyRollAndPitchTrimDelta = true;
}
if (shouldApplyRollAndPitchTrimDelta) {
applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
repeatAfter(STICK_AUTOREPEAT_MS);
return;
bool shouldApplyRollAndPitchTrimDelta = false;
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
accelerometerTrimsDelta.values.pitch = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
accelerometerTrimsDelta.values.pitch = -2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
accelerometerTrimsDelta.values.roll = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
accelerometerTrimsDelta.values.roll = -2;
shouldApplyRollAndPitchTrimDelta = true;
}
if (shouldApplyRollAndPitchTrimDelta) {
applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
repeatAfter(STICK_AUTOREPEAT_MS);
return;
}
} else {
// in ACRO mode, so use sticks to change RATE profile
switch (rcSticks) {
case THR_HI + YAW_CE + PIT_HI + ROL_CE:
changeControlRateProfile(0);
return;
case THR_HI + YAW_CE + PIT_LO + ROL_CE:
changeControlRateProfile(1);
return;
case THR_HI + YAW_CE + PIT_CE + ROL_HI:
changeControlRateProfile(2);
return;
case THR_HI + YAW_CE + PIT_CE + ROL_LO:
changeControlRateProfile(3);
return;
}
}
#ifdef USE_DASHBOARD

17
src/main/flight/altitude.c
View file

@ -52,13 +52,16 @@ static int32_t estimatedVario = 0; // variometer in cm/s
static int32_t estimatedAltitude = 0; // in cm
#if defined(USE_BARO) || defined(USE_SONAR)
enum {
DEBUG_ALTITUDE_ACC,
DEBUG_ALTITUDE_VEL,
DEBUG_ALTITUDE_HEIGHT
};
// 40hz update rate (20hz LPF on acc)
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
#if defined(USE_ALT_HOLD)
PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
@ -72,8 +75,6 @@ static int32_t errorVelocityI = 0;
static int32_t altHoldThrottleAdjustment = 0;
static int16_t initialThrottleHold;
// 40hz update rate (20hz LPF on acc)
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
#define DEGREES_80_IN_DECIDEGREES 800
@ -202,7 +203,9 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
return result;
}
#endif // USE_ALT_HOLD
#if defined(USE_BARO) || defined(USE_SONAR)
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static timeUs_t previousTimeUs = 0;
@ -280,7 +283,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
}
#endif // BARO
#endif // USE_BARO
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
@ -290,11 +293,15 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
// set vario
estimatedVario = applyDeadband(vel_tmp, 5);
#ifdef USE_ALT_HOLD
static float accZ_old = 0.0f;
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
#else
UNUSED(accZ_tmp);
#endif
}
#endif // defined(USE_BARO) || defined(USE_SONAR)
#endif // USE_BARO || USE_SONAR
int32_t getEstimatedAltitude(void)
{

178
src/main/flight/navigation.c
View file

@ -22,7 +22,7 @@
#include "platform.h"
#ifdef USE_GPS
#ifdef USE_NAV
#include "build/debug.h"
@ -72,16 +72,11 @@ bool areSticksInApModePosition(uint16_t ap_mode);
// **********************
// GPS
// **********************
int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 }; // it's the angles that must be applied for GPS correction
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees
static int16_t nav[2];
static int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode
// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
void navigationInit(void)
@ -104,23 +99,17 @@ void navigationInit(void)
#define NAV_TAIL_FIRST 0 // true - copter comes in with tail first
#define NAV_SET_TAKEOFF_HEADING 1 // true - when copter arrives to home position it rotates it's head to takeoff direction
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
static void GPS_calc_longitude_scaling(int32_t lat);
static void GPS_calc_velocity(void);
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
#ifdef USE_NAV
static bool check_missed_wp(void);
static void GPS_calc_poshold(void);
static void GPS_calc_nav_rate(uint16_t max_speed);
static void GPS_update_crosstrack(void);
static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
static int32_t wrap_36000(int32_t angle);
#endif
static int32_t wrap_18000(int32_t error);
@ -149,7 +138,6 @@ typedef struct {
static PID posholdPID[2];
static PID poshold_ratePID[2];
#ifdef USE_NAV
static PID_PARAM navPID_PARAM;
static PID navPID[2];
@ -181,7 +169,6 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// add in derivative component
return pid_param->kD * pid->derivative;
}
#endif
static void reset_PID(PID *pid)
{
@ -190,8 +177,6 @@ static void reset_PID(PID *pid)
pid->last_derivative = 0;
}
#define GPS_X 1
#define GPS_Y 0
/****************** PI and PID controllers for GPS ********************///32938 -> 33160
@ -200,18 +185,13 @@ static void reset_PID(PID *pid)
#define NAV_SLOW_NAV true
#define NAV_BANK_MAX 3000 // 30deg max banking when navigating (just for security and testing)
static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 };
static float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
static int32_t error[2];
#ifdef USE_NAV
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t rate_error[2];
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
#endif
// Currently used WP
static int32_t GPS_WP[2];
@ -238,95 +218,14 @@ static uint32_t wp_distance;
// used for slow speed wind up when start navigation;
static int16_t waypoint_speed_gov;
////////////////////////////////////////////////////////////////////////////////////
// moving average filter variables
//
#define GPS_FILTER_VECTOR_LENGTH 5
static uint8_t GPS_filter_index = 0;
static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
static int32_t GPS_filter_sum[2];
static int32_t GPS_read[2];
static int32_t GPS_filtered[2];
static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000)
static uint16_t fraction3[2];
// This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing;
// saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home
static int16_t nav_takeoff_bearing;
void GPS_calculateDistanceAndDirectionToHome(void)
void navNewGpsData(void)
{
if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
} else {
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
}
}
void onGpsNewData(void)
{
static uint32_t nav_loopTimer;
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
}
if (!ARMING_FLAG(ARMED))
DISABLE_STATE(GPS_FIX_HOME);
if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED))
GPS_reset_home_position();
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (int axis = 0; axis < 2; axis++) {
GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
// How close we are to a degree line ? its the first three digits from the fractions of degree
// later we use it to Check if we are close to a degree line, if yes, disable averaging,
fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;
GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode...
if (fraction3[axis] > 1 && fraction3[axis] < 999) {
if (axis == LAT) {
gpsSol.llh.lat = GPS_filtered[LAT];
} else {
gpsSol.llh.lon = GPS_filtered[LON];
}
}
}
}
#endif
//
// Calculate time delta for navigation loop, range 0-1.0f, in seconds
//
// Time for calculating x,y speed and navigation pids
dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = MIN(dTnav, 1.0f);
GPS_calculateDistanceAndDirectionToHome();
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
#ifdef USE_NAV
if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
// we are navigating
@ -367,19 +266,6 @@ void onGpsNewData(void)
break;
}
} //end of gps calcs
#endif
}
void GPS_reset_home_position(void)
{
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
}
}
// reset navigation (stop the navigation processor, and clear nav)
@ -393,9 +279,7 @@ void GPS_reset_nav(void)
nav[i] = 0;
reset_PID(&posholdPID[i]);
reset_PID(&poshold_ratePID[i]);
#ifdef USE_NAV
reset_PID(&navPID[i]);
#endif
}
}
@ -411,12 +295,10 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#ifdef USE_NAV
navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f;
navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f;
navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#endif
}
// OK here is the onboard GPS code
@ -431,12 +313,6 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
// this is used to offset the shrinking longitude as we go towards the poles
// It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter
//
static void GPS_calc_longitude_scaling(int32_t lat)
{
float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
GPS_scaleLonDown = cos_approx(rads);
}
////////////////////////////////////////////////////////////////////////////////////
// Sets the waypoint to navigate, reset neccessary variables and calculate initial values
//
@ -454,7 +330,6 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
waypoint_speed_gov = navigationConfig()->nav_speed_min;
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Check if we missed the destination somehow
//
@ -465,24 +340,7 @@ static bool check_missed_wp(void)
temp = wrap_18000(temp);
return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
}
#endif
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
////////////////////////////////////////////////////////////////////////////////////
// Get distance between two points in cm
// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
{
float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
*bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg
if (*bearing < 0)
*bearing += 36000;
}
////////////////////////////////////////////////////////////////////////////////////
// keep old calculation function for compatibility (could be removed later) distance in meters, bearing in degree
@ -495,32 +353,6 @@ static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1,
// *bearing = d2 / 100; //convert to degrees
//}
////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps position data
//
static void GPS_calc_velocity(void)
{
static int16_t speed_old[2] = { 0, 0 };
static int32_t last_coord[2] = { 0, 0 };
static uint8_t init = 0;
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
speed_old[GPS_X] = actual_speed[GPS_X];
speed_old[GPS_Y] = actual_speed[GPS_Y];
}
init = 1;
last_coord[LON] = gpsSol.llh.lon;
last_coord[LAT] = gpsSol.llh.lat;
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate a location error between two gps coordinates
// Because we are using lat and lon to do our distance errors here's a quick chart:
@ -536,7 +368,6 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in
error[LAT] = *target_lat - *gps_lat; // Y Error
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
@ -642,7 +473,6 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
}
return max_speed;
}
#endif
////////////////////////////////////////////////////////////////////////////////////
// Utilities
@ -656,7 +486,6 @@ static int32_t wrap_18000(int32_t error)
return error;
}
#ifdef USE_NAV
static int32_t wrap_36000(int32_t angle)
{
if (angle > 36000)
@ -665,7 +494,6 @@ static int32_t wrap_36000(int32_t angle)
angle += 36000;
return angle;
}
#endif
void updateGpsStateForHomeAndHoldMode(void)
{

15
src/main/flight/navigation.h
View file

@ -18,13 +18,8 @@
#pragma once
#include "common/axis.h"
#include "io/gps.h"
// navigation mode
typedef enum {
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} navigationMode_e;
// FIXME ap_mode is badly named, it's a value that is compared to rcCommand, not a flag at it's name implies.
@ -40,22 +35,14 @@ typedef struct navigationConfig_s {
PG_DECLARE(navigationConfig_t, navigationConfig);
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern int32_t GPS_home[2];
extern int32_t GPS_hold[2];
extern uint16_t GPS_distanceToHome; // distance to home point in meters
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern navigationMode_e nav_mode; // Navigation mode
void navigationInit(void);
void GPS_reset_home_position(void);
void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);
void gpsUsePIDs(struct pidProfile_s *pidProfile);
void updateGpsStateForHomeAndHoldMode(void);
void updateGpsWaypointsAndMode(void);
void onGpsNewData(void);

14
src/main/flight/pid.c
View file

@ -34,6 +34,7 @@
#include "config/parameter_group_ids.h"
#include "drivers/sound_beeper.h"
#include "drivers/time.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"
@ -44,7 +45,8 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "io/gps.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
@ -409,6 +411,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
static timeUs_t crashDetectedAtUs;
static timeUs_t previousTimeUs;
// calculate actual deltaT in seconds
const float deltaT = (currentTimeUs - previousTimeUs) * 0.000001f;
previousTimeUs = currentTimeUs;
// Dynamic ki component to gradually scale back integration when above windup point
const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
@ -491,6 +498,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate I component
const float ITerm = axisPID_I[axis];
// use dT (not deltaT) for ITerm calculation to avoid wind-up caused by jitter
const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
@ -509,8 +517,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
}
const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y
// Divide rate change by dT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / dT;
// Divide rate change by deltaT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / deltaT;
previousRateError[axis] = rD;

4
src/main/interface/cli.c
View file

@ -3642,9 +3642,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead),
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
#endif
#ifdef USE_RX_FRSKY_D
CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind),
#endif
#ifdef USE_RX_FRSKY_D
CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky RX", NULL, cliFrSkyBind),
#endif
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
#ifdef USE_GPS

76
src/main/interface/msp.c
View file

@ -150,6 +150,8 @@ typedef enum {
#define RATEPROFILE_MASK (1 << 7)
#endif //USE_OSD_SLAVE
#define RTC_NOT_SUPPORTED 0xff
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#define ESC_4WAY 0xff
@ -1220,9 +1222,41 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
}
}
#endif
break;
break;
#endif
case MSP_TX_INFO:
sbufWriteU8(dst, rssiSource);
uint8_t rtcDateTimeIsSet = 0;
#ifdef USE_RTC_TIME
dateTime_t dt;
if (rtcGetDateTime(&dt)) {
rtcDateTimeIsSet = 1;
}
#else
rtcDateTimeIsSet = RTC_NOT_SUPPORTED;
#endif
sbufWriteU8(dst, rtcDateTimeIsSet);
break;
#ifdef USE_RTC_TIME
case MSP_RTC:
{
dateTime_t dt;
if (rtcGetDateTime(&dt)) {
sbufWriteU16(dst, dt.year);
sbufWriteU8(dst, dt.month);
sbufWriteU8(dst, dt.day);
sbufWriteU8(dst, dt.hours);
sbufWriteU8(dst, dt.minutes);
sbufWriteU8(dst, dt.seconds);
sbufWriteU16(dst, dt.millis);
}
}
break;
#endif
default:
return false;
}
@ -1251,7 +1285,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sb
}
#endif // USE_OSD_SLAVE
#ifdef USE_GPS
#ifdef USE_NAV
static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src)
{
uint8_t wp_no;
@ -1312,7 +1346,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
uint32_t i;
uint8_t value;
const unsigned int dataSize = sbufBytesRemaining(src);
#ifdef USE_GPS
#ifdef USE_NAV
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
#endif
@ -1712,7 +1746,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
#endif
case MSP_ARMING_DISABLE:
case MSP_SET_ARMING_DISABLED:
{
const uint8_t command = sbufReadU8(src);
if (command) {
@ -1743,7 +1777,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
gpsSol.groundSpeed = sbufReadU16(src);
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break;
#endif // USE_GPS
#ifdef USE_NAV
case MSP_SET_WP:
wp_no = sbufReadU8(src); //get the wp number
lat = sbufReadU32(src);
@ -1768,7 +1803,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
}
break;
#endif
#endif // USE_NAV
case MSP_SET_FEATURE_CONFIG:
featureClearAll();
@ -1943,18 +1978,19 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef USE_RTC_TIME
case MSP_SET_RTC:
{
dateTime_t dt;
dt.year = sbufReadU16(src);
dt.month = sbufReadU8(src);
dt.day = sbufReadU8(src);
dt.hours = sbufReadU8(src);
dt.minutes = sbufReadU8(src);
dt.seconds = sbufReadU8(src);
dt.millis = 0;
rtcSetDateTime(&dt);
}
break;
{
dateTime_t dt;
dt.year = sbufReadU16(src);
dt.month = sbufReadU8(src);
dt.day = sbufReadU8(src);
dt.hours = sbufReadU8(src);
dt.minutes = sbufReadU8(src);
dt.seconds = sbufReadU8(src);
dt.millis = 0;
rtcSetDateTime(&dt);
}
break;
#endif
case MSP_TX_INFO:
@ -2177,7 +2213,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
mspFc4waySerialCommand(dst, src, mspPostProcessFn);
ret = MSP_RESULT_ACK;
#endif
#ifdef USE_GPS
#ifdef USE_NAV
} else if (cmdMSP == MSP_WP) {
mspFcWpCommand(dst, src);
ret = MSP_RESULT_ACK;

6
src/main/interface/msp_protocol.h
View file

@ -214,7 +214,7 @@
#define MSP_CAMERA_CONTROL 98
#define MSP_ARMING_DISABLE 99
#define MSP_SET_ARMING_DISABLED 99
//
// OSD specific
@ -230,7 +230,8 @@
#define MSP_BEEPER_CONFIG 184
#define MSP_SET_BEEPER_CONFIG 185
#define MSP_TX_INFO 186
#define MSP_SET_TX_INFO 186 // in message Used to send runtime information from TX lua scripts to the firmware
#define MSP_TX_INFO 187 // out message Used by TX lua scripts to read information from the firmware
//
// Multwii original MSP commands
@ -324,3 +325,4 @@
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface
#define MSP_SET_RTC 246 //in message Sets the RTC clock
#define MSP_RTC 247 //out message Gets the RTC clock

13
src/main/interface/settings.c
View file

@ -554,7 +554,7 @@ const clivalue_t valueTable[] = {
#endif
// PG_NAVIGATION_CONFIG
#ifdef USE_GPS
#ifdef USE_NAV
{ "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, gps_wp_radius) },
{ "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_controls_heading) },
{ "nav_speed_min", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_speed_min) },
@ -616,23 +616,24 @@ const clivalue_t valueTable[] = {
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) },
#ifdef USE_ALT_HOLD
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].P) },
{ "i_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].I) },
{ "d_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].D) },
{ "p_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) },
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) },
#endif
{ "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
{ "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },
{ "d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
{ "p_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) },
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) },
{ "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) },
{ "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) },
{ "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) },
#ifdef USE_GPS
#ifdef USE_NAV
{ "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].P) },
{ "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].I) },
{ "gps_pos_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].D) },

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();

210
src/main/io/gps.c
View file

@ -48,7 +48,7 @@
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "flight/navigation.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "sensors/sensors.h"
@ -71,6 +71,28 @@ static char *gpsPacketLogChar = gpsPacketLog;
// **********************
// GPS
// **********************
int32_t GPS_home[2];
uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees
int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 }; // it's the angles that must be applied for GPS correction
float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
int16_t actual_speed[2] = { 0, 0 };
int16_t nav_takeoff_bearing;
navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode
// moving average filter variables
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#ifdef GPS_FILTERING
#define GPS_FILTER_VECTOR_LENGTH 5
static uint8_t GPS_filter_index = 0;
static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
static int32_t GPS_filter_sum[2];
static int32_t GPS_read[2];
static int32_t GPS_filtered[2];
static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000)
static uint16_t fraction3[2];
#endif
gpsSolutionData_t gpsSol;
uint32_t GPS_packetCount = 0;
uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
@ -111,6 +133,7 @@ static const gpsInitData_t gpsInitData[] = {
#define DEFAULT_BAUD_RATE_INDEX 0
#ifdef USE_GPS_UBLOX
static const uint8_t ubloxInit[] = {
//Preprocessor Pedestrian Dynamic Platform Model Option
#if defined(GPS_UBLOX_MODE_PEDESTRIAN)
@ -179,7 +202,7 @@ static const ubloxSbas_t ubloxSbas[] = {
{ SBAS_MSAS, { 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
{ SBAS_GAGAN, { 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
};
#endif // USE_GPS_UBLOX
typedef enum {
GPS_UNKNOWN,
@ -212,8 +235,12 @@ static void shiftPacketLog(void)
}
static void gpsNewData(uint16_t c);
#ifdef USE_GPS_NMEA
static bool gpsNewFrameNMEA(char c);
#endif
#ifdef USE_GPS_UBLOX
static bool gpsNewFrameUBLOX(uint8_t data);
#endif
static void gpsSetState(gpsState_e state)
{
@ -268,6 +295,7 @@ void gpsInit(void)
gpsSetState(GPS_INITIALIZING);
}
#ifdef USE_GPS_NMEA
void gpsInitNmea(void)
{
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
@ -316,7 +344,9 @@ void gpsInitNmea(void)
break;
}
}
#endif // USE_GPS_NMEA
#ifdef USE_GPS_UBLOX
void gpsInitUblox(void)
{
uint32_t now;
@ -400,17 +430,22 @@ void gpsInitUblox(void)
break;
}
}
#endif // USE_GPS_UBLOX
void gpsInitHardware(void)
{
switch (gpsConfig()->provider) {
case GPS_NMEA:
gpsInitNmea();
break;
case GPS_NMEA:
#ifdef USE_GPS_NMEA
gpsInitNmea();
#endif
break;
case GPS_UBLOX:
gpsInitUblox();
break;
case GPS_UBLOX:
#ifdef USE_GPS_UBLOX
gpsInitUblox();
#endif
break;
}
}
@ -494,12 +529,17 @@ static void gpsNewData(uint16_t c)
bool gpsNewFrame(uint8_t c)
{
switch (gpsConfig()->provider) {
case GPS_NMEA: // NMEA
return gpsNewFrameNMEA(c);
case GPS_UBLOX: // UBX binary
return gpsNewFrameUBLOX(c);
case GPS_NMEA: // NMEA
#ifdef USE_GPS_NMEA
return gpsNewFrameNMEA(c);
#endif
break;
case GPS_UBLOX: // UBX binary
#ifdef USE_GPS_UBLOX
return gpsNewFrameUBLOX(c);
#endif
break;
}
return false;
}
@ -566,6 +606,7 @@ static uint32_t GPS_coord_to_degrees(char *coordinateString)
*/
// helper functions
#ifdef USE_GPS_NMEA
static uint32_t grab_fields(char *src, uint8_t mult)
{ // convert string to uint32
uint32_t i;
@ -764,7 +805,9 @@ static bool gpsNewFrameNMEA(char c)
}
return frameOK;
}
#endif // USE_GPS_NMEA
#ifdef USE_GPS_UBLOX
// UBX support
typedef struct {
uint8_t preamble1;
@ -1086,9 +1129,10 @@ static bool gpsNewFrameUBLOX(uint8_t data)
}
return parsed;
}
#endif // USE_GPS_UBLOX
static void gpsHandlePassthrough(uint8_t data)
{
{
gpsNewData(data);
#ifdef USE_DASHBOARD
if (feature(FEATURE_DASHBOARD)) {
@ -1114,4 +1158,142 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
}
float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
void GPS_calc_longitude_scaling(int32_t lat)
{
float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
GPS_scaleLonDown = cos_approx(rads);
}
void GPS_reset_home_position(void)
{
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
#ifdef USE_NAV
nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
#endif
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
}
}
////////////////////////////////////////////////////////////////////////////////////
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
// Get distance between two points in cm
// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
{
float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
*bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg
if (*bearing < 0)
*bearing += 36000;
}
void GPS_calculateDistanceAndDirectionToHome(void)
{
if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
} else {
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps position data
//
static void GPS_calc_velocity(void)
{
static int16_t speed_old[2] = { 0, 0 };
static int32_t last_coord[2] = { 0, 0 };
static uint8_t init = 0;
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
speed_old[GPS_X] = actual_speed[GPS_X];
speed_old[GPS_Y] = actual_speed[GPS_Y];
}
init = 1;
last_coord[LON] = gpsSol.llh.lon;
last_coord[LAT] = gpsSol.llh.lat;
}
void onGpsNewData(void)
{
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
}
if (!ARMING_FLAG(ARMED))
DISABLE_STATE(GPS_FIX_HOME);
if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED))
GPS_reset_home_position();
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (int axis = 0; axis < 2; axis++) {
GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
// How close we are to a degree line ? its the first three digits from the fractions of degree
// later we use it to Check if we are close to a degree line, if yes, disable averaging,
fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;
GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode...
if (fraction3[axis] > 1 && fraction3[axis] < 999) {
if (axis == LAT) {
gpsSol.llh.lat = GPS_filtered[LAT];
} else {
gpsSol.llh.lon = GPS_filtered[LON];
}
}
}
}
#endif
//
// Calculate time delta for navigation loop, range 0-1.0f, in seconds
//
// Time for calculating x,y speed and navigation pids
static uint32_t nav_loopTimer;
dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = MIN(dTnav, 1.0f);
GPS_calculateDistanceAndDirectionToHome();
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
#ifdef USE_NAV
navNewGpsData();
#endif
}
#endif

24
src/main/io/gps.h
View file

@ -17,6 +17,7 @@
#pragma once
#include "common/axis.h"
#include "common/time.h"
#include "config/parameter_group.h"
@ -25,6 +26,8 @@
#define LON 1
#define GPS_DEGREES_DIVIDER 10000000L
#define GPS_X 1
#define GPS_Y 0
typedef enum {
GPS_NMEA = 0,
@ -114,6 +117,22 @@ typedef struct gpsData_s {
#define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display.
extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
extern int32_t GPS_home[2];
extern uint16_t GPS_distanceToHome; // distance to home point in meters
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles
extern int16_t actual_speed[2];
extern int16_t nav_takeoff_bearing;
// navigation mode
typedef enum {
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} navigationMode_e;
extern navigationMode_e nav_mode; // Navigation mode
extern gpsData_t gpsData;
extern gpsSolutionData_t gpsSol;
@ -134,4 +153,9 @@ void gpsUpdate(timeUs_t currentTimeUs);
bool gpsNewFrame(uint8_t c);
struct serialPort_s;
void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
void onGpsNewData(void);
void GPS_reset_home_position(void);
void GPS_calc_longitude_scaling(int32_t lat);
void navNewGpsData(void);
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);

17
src/main/io/osd.c
View file

@ -843,7 +843,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
{
/* Position elements near centre of screen and disabled by default */
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
osdConfig->item_pos[i] = OSD_POS(10, 6);
osdConfig->item_pos[i] = OSD_POS(10, 7);
}
/* Always enable warnings elements by default */
@ -861,6 +861,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = true;
osdConfig->enabled_stats[OSD_STAT_TIMER_1] = false;
osdConfig->enabled_stats[OSD_STAT_TIMER_2] = true;
osdConfig->enabled_stats[OSD_STAT_RTC_DATE_TIME] = false;
osdConfig->units = OSD_UNIT_METRIC;
@ -1103,11 +1104,23 @@ static bool isSomeStatEnabled(void) {
static void osdShowStats(void)
{
uint8_t top = 2;
char buff[10];
char buff[OSD_ELEMENT_BUFFER_LENGTH];
displayClearScreen(osdDisplayPort);
displayWrite(osdDisplayPort, 2, top++, " --- STATS ---");
if (osdConfig()->enabled_stats[OSD_STAT_RTC_DATE_TIME]) {
bool success = false;
#ifdef USE_RTC_TIME
success = printRtcDateTime(&buff[0]);
#endif
if (!success) {
tfp_sprintf(buff, "NO RTC");
}
displayWrite(osdDisplayPort, 2, top++, buff);
}
if (osdConfig()->enabled_stats[OSD_STAT_TIMER_1]) {
osdFormatTimer(buff, false, OSD_TIMER_1);
osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);

1
src/main/io/osd.h
View file

@ -102,6 +102,7 @@ typedef enum {
OSD_STAT_TIMER_2,
OSD_STAT_MAX_DISTANCE,
OSD_STAT_BLACKBOX_NUMBER,
OSD_STAT_RTC_DATE_TIME,
OSD_STAT_COUNT // MUST BE LAST
} osd_stats_e;

8
src/main/rx/flysky.c
View file

@ -173,7 +173,7 @@ static void checkTimeout (void)
if(countTimeout > 31) {
timeout = timings->syncPacket;
setRssiFiltered(0);
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
} else {
timeout = timings->packet;
countTimeout++;
@ -197,7 +197,7 @@ static void checkRSSI (void)
rssi_dBm = 50 + sum / (3 * FLYSKY_RSSI_SAMPLE_COUNT); // range about [95...52], -dBm
int16_t tmp = 2280 - 24 * rssi_dBm;// convert to [0...1023]
setRssiFiltered(constrain(tmp, 0, 1023));
setRssiFiltered(constrain(tmp, 0, 1023), RSSI_SOURCE_RX_PROTOCOL);
}
static bool isValidPacket (const uint8_t *packet) {
@ -384,6 +384,10 @@ void flySkyInit (const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rx
startRxChannel = getNextChannel(0);
}
if (rssiSource == RSSI_SOURCE_NONE) {
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
}
A7105WriteReg(A7105_0F_CHANNEL, startRxChannel);
A7105Strobe(A7105_RX); // start listening

25
src/main/rx/fport.c
View file

@ -46,6 +46,7 @@
#define FPORT_TIME_NEEDED_PER_FRAME_US 3000
#define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000
#define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
#define FPORT_MAX_TELEMETRY_AGE_MS 500
#define FPORT_FRAME_MARKER 0x7E
@ -106,8 +107,6 @@ typedef struct fportFrame_s {
fportData_t data;
} fportFrame_t;
static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = 0, .data = 0 };
#define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t)
#define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t))
@ -134,7 +133,7 @@ static serialPort_t *fportPort;
static bool telemetryEnabled = false;
static void reportFrameError(uint8_t errorReason) {
static volatile uint16_t frameErrors = 0;
static volatile uint16_t frameErrors = 0;
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, ++frameErrors);
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason);
@ -239,6 +238,7 @@ static uint8_t fportFrameStatus(void)
static smartPortPayload_t payloadBuffer;
static smartPortPayload_t *mspPayload = NULL;
static bool hasTelemetryRequest = false;
static timeUs_t lastRcFrameReceivedMs = 0;
uint8_t result = RX_FRAME_PENDING;
@ -260,7 +260,9 @@ static uint8_t fportFrameStatus(void)
} else {
result = sbusChannelsDecode(&frame->data.controlData.channels);
setRssiUnfiltered(scaleRange(frame->data.controlData.rssi, 0, 100, 0, 1024));
setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
lastRcFrameReceivedMs = millis();
}
break;
@ -319,16 +321,17 @@ static uint8_t fportFrameStatus(void)
processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
}
if (clearToSend) {
smartPortWriteFrameFport(&emptySmartPortFrame);
clearToSend = false;
}
clearToSend = false;
}
}
#endif
}
if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
lastRcFrameReceivedMs = 0;
}
return result;
}
@ -359,6 +362,10 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
#if defined(USE_TELEMETRY_SMARTPORT)
telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport);
#endif
if (rssiSource == RSSI_SOURCE_NONE) {
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
}
}
return fportPort != NULL;

8
src/main/rx/frsky_d.c
View file

@ -133,7 +133,7 @@ static void compute_RSSIdbm(uint8_t *packet)
RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65;
}
setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024));
setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
}
#if defined(USE_TELEMETRY_FRSKY)
@ -714,11 +714,17 @@ static void frskyD_Rx_Setup(rx_spi_protocol_e protocol)
RX_enable();
#endif
#if defined(USE_FRSKY_D_TELEMETRY)
#if defined(USE_TELEMETRY_FRSKY)
initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi,
&frSkyTelemetryWriteSpi);
#endif
if (rssiSource == RSSI_SOURCE_NONE) {
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
}
#endif
// if(!frSkySpiDetect())//detect spi working routine
// return;
}

104
src/main/rx/rx.c
View file

@ -68,11 +68,12 @@
const char rcChannelLetters[] = "AERT12345678abcdefgh";
static uint16_t rssi = 0; // range: [0;1023]
static bool useMspRssi = true;
static timeUs_t lastMspRssiUpdateUs = 0;
#define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec
rssiSource_t rssiSource;
static bool rxDataReceived = false;
static bool rxSignalReceived = false;
static bool rxSignalReceivedNotDataDriven = false;
@ -360,6 +361,15 @@ void rxInit(void)
}
#endif
#if defined(USE_ADC)
if (feature(FEATURE_RSSI_ADC)) {
rssiSource = RSSI_SOURCE_ADC;
} else
#endif
if (rxConfig()->rssi_channel > 0) {
rssiSource = RSSI_SOURCE_RX_CHANNEL;
}
rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
}
@ -612,10 +622,48 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig)
}
}
void setRssiFiltered(uint16_t newRssi)
void setRssiFiltered(uint16_t newRssi, rssiSource_t source)
{
if (source != rssiSource) {
return;
}
rssi = newRssi;
useMspRssi = false;
}
#define RSSI_SAMPLE_COUNT 16
#define RSSI_MAX_VALUE 1023
void setRssiUnfiltered(uint16_t rssiValue, rssiSource_t source)
{
if (source != rssiSource) {
return;
}
static uint16_t rssiSamples[RSSI_SAMPLE_COUNT];
static uint8_t rssiSampleIndex = 0;
static unsigned sum = 0;
sum = sum + rssiValue;
sum = sum - rssiSamples[rssiSampleIndex];
rssiSamples[rssiSampleIndex] = rssiValue;
rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT;
int16_t rssiMean = sum / RSSI_SAMPLE_COUNT;
rssi = rssiMean;
}
void setRssiMsp(uint8_t newMspRssi)
{
if (rssiSource == RSSI_SOURCE_NONE) {
rssiSource = RSSI_SOURCE_MSP;
}
if (rssiSource == RSSI_SOURCE_MSP) {
rssi = ((uint16_t)newMspRssi) << 2;
lastMspRssiUpdateUs = micros();
}
}
static void updateRSSIPWM(void)
@ -629,7 +677,7 @@ static void updateRSSIPWM(void)
}
// Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
setRssiFiltered(constrain((uint16_t)(((pwmRssi - 1000) / 1000.0f) * 1024.0f), 0, 1023));
setRssiFiltered(constrain((uint16_t)(((pwmRssi - 1000) / 1000.0f) * 1024.0f), 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL);
}
static void updateRSSIADC(timeUs_t currentTimeUs)
@ -646,54 +694,36 @@ static void updateRSSIADC(timeUs_t currentTimeUs)
const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
uint16_t rssiValue = (uint16_t)((1024.0f * adcRssiSample) / (rxConfig()->rssi_scale * 100.0f));
rssiValue = constrain(rssiValue, 0, 1023);
rssiValue = constrain(rssiValue, 0, RSSI_MAX_VALUE);
// RSSI_Invert option
if (rxConfig()->rssi_invert) {
rssiValue = 1024 - rssiValue;
rssiValue = RSSI_MAX_VALUE - rssiValue;
}
setRssiUnfiltered((uint16_t)rssiValue);
setRssiUnfiltered((uint16_t)rssiValue, RSSI_SOURCE_ADC);
#endif
}
#define RSSI_SAMPLE_COUNT 16
void setRssiUnfiltered(uint16_t rssiValue)
{
static uint16_t rssiSamples[RSSI_SAMPLE_COUNT];
static uint8_t rssiSampleIndex = 0;
static unsigned sum = 0;
sum = sum + rssiValue;
sum = sum - rssiSamples[rssiSampleIndex];
rssiSamples[rssiSampleIndex] = rssiValue;
rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT;
int16_t rssiMean = sum / RSSI_SAMPLE_COUNT;
setRssiFiltered(rssiMean);
}
void setRssiMsp(uint8_t newMspRssi)
{
if (useMspRssi) {
rssi = ((uint16_t)newMspRssi) << 2;
lastMspRssiUpdateUs = micros();
}
}
void updateRSSI(timeUs_t currentTimeUs)
{
if (rxConfig()->rssi_channel > 0) {
switch (rssiSource) {
case RSSI_SOURCE_RX_CHANNEL:
updateRSSIPWM();
} else if (feature(FEATURE_RSSI_ADC)) {
break;
case RSSI_SOURCE_ADC:
updateRSSIADC(currentTimeUs);
} else if (useMspRssi) {
break;
case RSSI_SOURCE_MSP:
if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
rssi = 0;
}
break;
default:
break;
}
}

18
src/main/rx/rx.h
View file

@ -159,6 +159,16 @@ typedef struct rxRuntimeConfig_s {
rcFrameStatusFnPtr rcFrameStatusFn;
} rxRuntimeConfig_t;
typedef enum rssiSource_e {
RSSI_SOURCE_NONE = 0,
RSSI_SOURCE_ADC,
RSSI_SOURCE_RX_CHANNEL,
RSSI_SOURCE_RX_PROTOCOL,
RSSI_SOURCE_MSP,
} rssiSource_t;
extern rssiSource_t rssiSource;
extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount
void rxInit(void);
@ -169,10 +179,10 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
void parseRcChannels(const char *input, rxConfig_t *rxConfig);
void setRssiFiltered(uint16_t newRssi);
void setRssiUnfiltered(uint16_t rssiValue);
void setRssiMsp(uint8_t newMspRssi);
void updateRSSI(timeUs_t currentTimeUs);
void setRssiFiltered(const uint16_t newRssi, const rssiSource_t source);
void setRssiUnfiltered(const uint16_t rssiValue, const rssiSource_t source);
void setRssiMsp(const uint8_t newMspRssi);
void updateRSSI(const timeUs_t currentTimeUs);
uint16_t getRssi(void);
void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig);

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;
}

6
src/main/target/BETAFLIGHTF3/target.h
View file

@ -20,6 +20,12 @@
#define TARGET_BOARD_IDENTIFIER "BFF3"
// Removing some features to make the firmware fit the flash space
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_JETIEXBUS
#undef USE_TELEMETRY_LTM
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
#define BEEPER PC15

7
src/main/target/BETAFLIGHTF4/target.h
View file

@ -49,10 +49,11 @@
#define USE_MPU_DATA_READY_SIGNAL
#define USE_BARO
#define USE_BARO_SPI_BMP280
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define BMP280_SPI_INSTANCE SPI2
#define BMP280_CS_PIN PB3
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_OSD
#define USE_MAX7456

2
src/main/target/BETAFLIGHTF4/target.mk
View file

@ -6,4 +6,6 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu.c \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/max7456.c

3
src/main/target/MATEKF405/target.h
View file

@ -58,6 +58,9 @@
#define USE_ACC_SPI_MPU6500
#define ACC_MPU6500_ALIGN CW180_DEG
#define MAG
#define USE_MAG_HMC5883
// *************** Baro **************************
#define USE_I2C

1
src/main/target/MATEKF405/target.mk
View file

@ -8,4 +8,5 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/max7456.c

4
src/main/target/OMNIBUS/target.h
View file

@ -21,10 +21,12 @@
#undef USE_TELEMETRY_IBUS
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_JETIEXBUS
#undef USE_SERIALRX_JETIEXBUS
#undef USE_TELEMETRY_MAVLINK
#undef USE_TELEMETRY_LTM
#undef USE_RCDEVICE
#undef USE_RTC_TIME
#undef USE_DASHBOARD
#define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus

3
src/main/target/RCEXPLORERF3/target.h
View file

@ -104,6 +104,9 @@
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART1
#define USE_GPS
#define USE_GPS_UBLOX
#define USE_GPS_NMEA
#define USE_NAV
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

18
src/main/target/SPRACINGF3EVO/target.h
View file

@ -25,10 +25,8 @@
#define TARGET_BOARD_IDENTIFIER "SPEV"
#endif
#if !defined(AIORACERF3)
#define USE_TARGET_CONFIG
#ifdef AIORACERF3
#undef USE_TARGET_CONFIG // no space left
#endif
#ifdef SPRACINGF3MQ
@ -38,12 +36,14 @@
#define SPRACINGF3MQ_REV 2
#endif
#undef USE_UNCOMMON_MIXERS // no space left
#endif
#undef USE_TELEMETRY_JETIEXBUS // no space left
#undef USE_SERIALRX_JETIEXBUS // no space left
#undef USE_DASHBOARD // no space left
#undef USE_RTC_TIME // no space left
#endif // SPRACINGF3MQ
// Space reduction measures to make the firmware fit into flash:
#undef USE_TELEMETRY_JETIEXBUS
#undef USE_SERIALRX_JETIEXBUS
#undef USE_TELEMETRY_MAVLINK
#undef USE_DASHBOARD
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT

5
src/main/target/common_fc_post.h
View file

@ -61,6 +61,11 @@
#endif
#endif
// undefine USE_ALT_HOLD if there is no baro or sonar to support it
#if defined(USE_ALT_HOLD) && !defined(USE_BARO) && !defined(USE_SONAR)
#undef USE_ALT_HOLD
#endif
/* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */
#if !defined(VTX_COMMON) || !defined(VTX_CONTROL)
#undef VTX_COMMON

4
src/main/target/common_fc_pre.h
View file

@ -138,13 +138,17 @@
#define USE_SPEKTRUM_FAKE_RSSI
#define USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
#define USE_SPEKTRUM_VTX_CONTROL
#define USE_SPEKTRUM_CMS_TELEMETRY
#endif
#endif
#if (FLASH_SIZE > 256)
// Temporarily moved GPS here because of overflowing flash size on F3
#define USE_GPS
#define USE_GPS_UBLOX
#define USE_GPS_NMEA
#define USE_NAV
#define USE_ALT_HOLD
#define USE_UNCOMMON_MIXERS
#define USE_OSD_ADJUSTMENTS
#endif

1
src/main/telemetry/hott.c
View file

@ -74,7 +74,6 @@
#include "flight/altitude.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "io/gps.h"

1
src/main/telemetry/ltm.c
View file

@ -70,7 +70,6 @@
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/altitude.h"
#include "flight/navigation.h"
#include "telemetry/telemetry.h"
#include "telemetry/ltm.h"

1
src/main/telemetry/smartport.c
View file

@ -36,7 +36,6 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "interface/msp.h"

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);

4
src/test/Makefile
View file

@ -155,10 +155,12 @@ osd_unittest_SRC := \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/common/maths.c \
$(USER_DIR)/common/printf.c \
$(USER_DIR)/common/time.c \
$(USER_DIR)/fc/runtime_config.c
osd_unittest_DEFINES := \
USE_OSD
USE_OSD \
USE_RTC_TIME
parameter_groups_unittest_SRC := \

1
src/test/unit/arming_prevention_unittest.cc
View file

@ -662,6 +662,7 @@ extern "C" {
void accSetCalibrationCycles(uint16_t) {}
void baroSetCalibrationCycles(uint16_t) {}
void changePidProfile(uint8_t) {}
void changeControlRateProfile(uint8_t) {}
void dashboardEnablePageCycling(void) {}
void dashboardDisablePageCycling(void) {}
bool imuQuaternionHeadfreeOffsetSet(void) { return true; }

25
src/test/unit/osd_unittest.cc
View file

@ -29,6 +29,8 @@ extern "C" {
#include "config/parameter_group_ids.h"
#include "common/time.h"
#include "drivers/max7456_symbols.h"
#include "fc/config.h"
@ -57,7 +59,7 @@ extern "C" {
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome;
uint16_t GPS_directionToHome;
int16_t GPS_directionToHome;
int32_t GPS_coord[2];
gpsSolutionData_t gpsSol;
@ -188,7 +190,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");
@ -291,6 +293,7 @@ TEST(OsdTest, TestStatsImperial)
osdConfigMutable()->enabled_stats[OSD_STAT_END_BATTERY] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_TIMER_1] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_TIMER_2] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_RTC_DATE_TIME] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_MAX_DISTANCE] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = false;
@ -310,6 +313,18 @@ TEST(OsdTest, TestStatsImperial)
// a GPS fix is present
stateFlags |= GPS_FIX | GPS_FIX_HOME;
// and
// this RTC time
dateTime_t dateTime;
dateTime.year = 2017;
dateTime.month = 11;
dateTime.day = 19;
dateTime.hours = 10;
dateTime.minutes = 12;
dateTime.seconds = 0;
dateTime.millis = 0;
rtcSetDateTime(&dateTime);
// when
// the craft is armed
doTestArm();
@ -347,6 +362,7 @@ TEST(OsdTest, TestStatsImperial)
// then
// statistics screen should display the following
int row = 3;
displayPortTestBufferSubstring(2, row++, "2017-11-19 10:12:");
displayPortTestBufferSubstring(2, row++, "TOTAL ARM : 00:05.00");
displayPortTestBufferSubstring(2, row++, "LAST ARM : 00:03");
displayPortTestBufferSubstring(2, row++, "MAX SPEED : 28");
@ -397,6 +413,7 @@ TEST(OsdTest, TestStatsMetric)
// then
// statistics screen should display the following
int row = 3;
displayPortTestBufferSubstring(2, row++, "2017-11-19 10:12:");
displayPortTestBufferSubstring(2, row++, "TOTAL ARM : 00:07.50");
displayPortTestBufferSubstring(2, row++, "LAST ARM : 00:02");
displayPortTestBufferSubstring(2, row++, "MAX SPEED : 28");
@ -873,6 +890,10 @@ extern "C" {
return simulationTime;
}
uint32_t millis() {
return micros() / 1000;
}
bool isBeeperOn() {
return false;
}

1
src/test/unit/rc_controls_unittest.cc
View file

@ -699,6 +699,7 @@ void blackboxLogEvent(FlightLogEvent, flightLogEventData_t *) {}
bool cmsInMenu = false;
uint8_t armingFlags = 0;
uint16_t flightModeFlags = 0;
int16_t heading;
uint8_t stateFlags = 0;
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];