Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Merge branch 'master' into gps_task_time

Browse source
This commit is contained in:
Michael Keller 2021-05-17 22:44:18 +12:00 • committed by GitHub
commit e9b77e9f30
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
104 changed files with 1879 additions and 583 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/link/stm32_flash_g474.ld
View file

@ -12,10 +12,12 @@
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 4K
FLASH1 (rx) : ORIGIN = 0x08001000, LENGTH = 492K
FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 8K
FLASH_CUSTOM_DEFAULTS (r) : ORIGIN = 0x0807E000, LENGTH = 8K
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 10K
FLASH_CUSTOM_DEFAULTS (r) : ORIGIN = 0x08002800, LENGTH = 6K
FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 8K
FLASH1 (rx) : ORIGIN = 0x08006000, LENGTH = DEFINED(USE_CUSTOM_DEFAULTS_EXTENDED) ? 480K : 488K
FLASH_CUSTOM_DEFAULTS_EXTENDED (r): ORIGIN = DEFINED(USE_CUSTOM_DEFAULTS_EXTENDED) ? 0x0807E000 : 0x08080000, LENGTH = DEFINED(USE_CUSTOM_DEFAULTS_EXTENDED) ? 8K : 0K
SYSTEM_MEMORY (r) : ORIGIN = 0x1FFF0000, LENGTH = 64K

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

@ -1035,15 +1035,15 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
}
blackboxWriteUnsignedVB(gpsSol.numSat);
blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]);
blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]);
blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[GPS_LATITUDE]);
blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[GPS_LONGITUDE]);
blackboxWriteUnsignedVB(gpsSol.llh.altCm / 10); // was originally designed to transport meters in int16, but +-3276.7m is a good compromise
blackboxWriteUnsignedVB(gpsSol.groundSpeed);
blackboxWriteUnsignedVB(gpsSol.groundCourse);
gpsHistory.GPS_numSat = gpsSol.numSat;
gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat;
gpsHistory.GPS_coord[LON] = gpsSol.llh.lon;
gpsHistory.GPS_coord[GPS_LATITUDE] = gpsSol.llh.lat;
gpsHistory.GPS_coord[GPS_LONGITUDE] = gpsSol.llh.lon;
}
#endif
@ -1461,8 +1461,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting,
rcSmoothingData->derivativeCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rcSmoothingData->inputFilterType,
rcSmoothingData->derivativeFilterType);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency,
rcSmoothingData->derivativeCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs);
@ -1639,8 +1637,8 @@ STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs)
writeGPSHomeFrame();
writeGPSFrame(currentTimeUs);
} else if (gpsSol.numSat != gpsHistory.GPS_numSat
|| gpsSol.llh.lat != gpsHistory.GPS_coord[LAT]
|| gpsSol.llh.lon != gpsHistory.GPS_coord[LON]) {
|| gpsSol.llh.lat != gpsHistory.GPS_coord[GPS_LATITUDE]
|| gpsSol.llh.lon != gpsHistory.GPS_coord[GPS_LONGITUDE]) {
//We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame(currentTimeUs);
}

17
src/main/cli/cli.c
View file

@ -4967,28 +4967,17 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
cliPrintLinef("%d.%03dms", avgRxFrameUs / 1000, avgRxFrameUs % 1000);
}
}
cliPrintLinef("# Input filter type: %s", lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]);
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency);
if (rcSmoothingData->inputCutoffSetting == 0) {
cliPrintLine("(auto)");
} else {
cliPrintLine("(manual)");
}
cliPrintf("# Derivative filter type: %s", lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]);
if (rcSmoothingData->derivativeFilterTypeSetting == RC_SMOOTHING_DERIVATIVE_AUTO) {
cliPrintLine(" (auto)");
} else {
cliPrintLinefeed();
}
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency);
if (rcSmoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_OFF) {
cliPrintLine("off)");
if (rcSmoothingData->derivativeCutoffSetting == 0) {
cliPrintLine("auto)");
} else {
if (rcSmoothingData->derivativeCutoffSetting == 0) {
cliPrintLine("auto)");
} else {
cliPrintLine("manual)");
}
cliPrintLine("manual)");
}
} else {
cliPrintLine("INTERPOLATION");

16
src/main/cli/settings.c
View file

@ -412,12 +412,6 @@ static const char * const lookupTableRcSmoothingType[] = {
static const char * const lookupTableRcSmoothingDebug[] = {
"ROLL", "PITCH", "YAW", "THROTTLE"
};
static const char * const lookupTableRcSmoothingInputType[] = {
"PT1", "BIQUAD"
};
static const char * const lookupTableRcSmoothingDerivativeType[] = {
"OFF", "PT1", "BIQUAD", "AUTO"
};
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON
@ -605,8 +599,6 @@ const lookupTableEntry_t lookupTables[] = {
#ifdef USE_RC_SMOOTHING_FILTER
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON
LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm),
@ -750,8 +742,6 @@ const clivalue_t valueTable[] = {
{ "rc_smoothing_input_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_cutoff) },
{ "rc_smoothing_derivative_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_cutoff) },
{ "rc_smoothing_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DEBUG }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_debug_axis) },
{ "rc_smoothing_input_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_INPUT_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_type) },
{ "rc_smoothing_derivative_type",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_type) },
{ "rc_smoothing_auto_smoothness",VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor) },
#endif // USE_RC_SMOOTHING_FILTER
@ -1174,6 +1164,7 @@ const clivalue_t valueTable[] = {
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
{ "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
{ "ff_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_smooth_factor) },
{ "ff_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jitter_factor) },
#endif
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },
@ -1326,7 +1317,7 @@ const clivalue_t valueTable[] = {
{ "osd_link_quality_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, link_quality_alarm) },
#endif
#ifdef USE_RX_RSSI_DBM
{ "osd_rssi_dbm_alarm", VAR_INT16 | MASTER_VALUE, .config.minmaxUnsigned = { CRSF_RSSI_MIN, CRSF_SNR_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_dbm_alarm) },
{ "osd_rssi_dbm_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { CRSF_RSSI_MIN, CRSF_SNR_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_dbm_alarm) },
#endif
{ "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, cap_alarm) },
{ "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, alt_alarm) },
@ -1352,6 +1343,9 @@ const clivalue_t valueTable[] = {
#ifdef USE_RX_LINK_QUALITY_INFO
{ "osd_link_quality_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_LINK_QUALITY]) },
#endif
#ifdef USE_RX_LINK_UPLINK_POWER
{ "osd_link_tx_power_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_TX_UPLINK_POWER]) },
#endif
#ifdef USE_RX_RSSI_DBM
{ "osd_rssi_dbm_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_RSSI_DBM_VALUE]) },
#endif

2
src/main/cli/settings.h
View file

@ -112,8 +112,6 @@ typedef enum {
#ifdef USE_RC_SMOOTHING_FILTER
TABLE_RC_SMOOTHING_TYPE,
TABLE_RC_SMOOTHING_DEBUG,
TABLE_RC_SMOOTHING_INPUT_TYPE,
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON
TABLE_VTX_LOW_POWER_DISARM,

4
src/main/cms/cms_menu_imu.c
View file

@ -519,6 +519,7 @@ static uint8_t cmsx_iterm_relax_cutoff;
#ifdef USE_INTERPOLATED_SP
static uint8_t cmsx_ff_interpolate_sp;
static uint8_t cmsx_ff_smooth_factor;
static uint8_t cmsx_ff_jitter_factor;
#endif
static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
@ -561,6 +562,7 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
#ifdef USE_INTERPOLATED_SP
cmsx_ff_interpolate_sp = pidProfile->ff_interpolate_sp;
cmsx_ff_smooth_factor = pidProfile->ff_smooth_factor;
cmsx_ff_jitter_factor = pidProfile->ff_jitter_factor;
#endif
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@ -608,6 +610,7 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
#ifdef USE_INTERPOLATED_SP
pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp;
pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor;
pidProfile->ff_jitter_factor = cmsx_ff_jitter_factor;
#endif
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@ -625,6 +628,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
#ifdef USE_INTERPOLATED_SP
{ "FF MODE", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_ff_interpolate_sp, 4, lookupTableInterpolatedSetpoint}, 0 },
{ "FF SMOOTHNESS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_smooth_factor, 0, 75, 1 } , 0 },
{ "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_jitter_factor, 0, 20, 1 } , 0 },
#endif
{ "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_boost, 0, 50, 1 } , 0 },
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },

33
src/main/cms/cms_menu_misc.c
View file

@ -22,6 +22,7 @@
#include <stdint.h>
#include <string.h>
#include <ctype.h>
#include <math.h>
#include "platform.h"
@ -79,6 +80,20 @@ static const void *cmsx_menuRcConfirmBack(displayPort_t *pDisp, const OSD_Entry
}
}
static int16_t rcDataInt[AUX4 + 1];
static const void *cmsx_menuRcOnDisplayUpdate(displayPort_t *pDisp, const OSD_Entry *selected)
{
UNUSED(pDisp);
UNUSED(selected);
for (int i = 0; i <= AUX4; i++) {
rcDataInt[i] = lroundf(rcData[i]);
}
return NULL;
}
//
// RC preview
//
@ -86,15 +101,15 @@ static const OSD_Entry cmsx_menuRcEntries[] =
{
{ "-- RC PREV --", OME_Label, NULL, NULL, 0},
{ "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[ROLL], 1, 2500, 0 }, DYNAMIC },
{ "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[PITCH], 1, 2500, 0 }, DYNAMIC },
{ "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[THROTTLE], 1, 2500, 0 }, DYNAMIC },
{ "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[YAW], 1, 2500, 0 }, DYNAMIC },
{ "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[ROLL], 1, 2500, 0 }, DYNAMIC },
{ "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[PITCH], 1, 2500, 0 }, DYNAMIC },
{ "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[THROTTLE], 1, 2500, 0 }, DYNAMIC },
{ "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[YAW], 1, 2500, 0 }, DYNAMIC },
{ "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX1], 1, 2500, 0 }, DYNAMIC },
{ "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX2], 1, 2500, 0 }, DYNAMIC },
{ "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX3], 1, 2500, 0 }, DYNAMIC },
{ "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX4], 1, 2500, 0 }, DYNAMIC },
{ "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX1], 1, 2500, 0 }, DYNAMIC },
{ "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX2], 1, 2500, 0 }, DYNAMIC },
{ "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX3], 1, 2500, 0 }, DYNAMIC },
{ "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX4], 1, 2500, 0 }, DYNAMIC },
{ "BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
@ -107,7 +122,7 @@ CMS_Menu cmsx_menuRcPreview = {
#endif
.onEnter = cmsx_menuRcOnEnter,
.onExit = cmsx_menuRcConfirmBack,
.onDisplayUpdate = NULL,
.onDisplayUpdate = cmsx_menuRcOnDisplayUpdate,
.entries = cmsx_menuRcEntries
};

6
src/main/cms/cms_menu_osd.c
View file

@ -49,6 +49,8 @@
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "rx/crsf.h"
#include "sensors/battery.h"
#ifdef USE_EXTENDED_CMS_MENUS
@ -180,7 +182,7 @@ static CMS_Menu menuOsdActiveElems = {
static uint8_t osdConfig_rssi_alarm;
static uint16_t osdConfig_link_quality_alarm;
static uint8_t osdConfig_rssi_dbm_alarm;
static int16_t osdConfig_rssi_dbm_alarm;
static uint16_t osdConfig_cap_alarm;
static uint16_t osdConfig_alt_alarm;
static uint16_t osdConfig_distance_alarm;
@ -225,7 +227,7 @@ const OSD_Entry menuAlarmsEntries[] =
{"--- ALARMS ---", OME_Label, NULL, NULL, 0},
{"RSSI", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_alarm, 5, 90, 5}, 0},
{"LINK QUALITY", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_link_quality_alarm, 5, 300, 5}, 0},
{"RSSI DBM", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_dbm_alarm, 5, 130, 5}, 0},
{"RSSI DBM", OME_INT16, NULL, &(OSD_INT16_t){&osdConfig_rssi_dbm_alarm, CRSF_RSSI_MIN, CRSF_SNR_MAX, 5}, 0},
{"MAIN BAT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_cap_alarm, 50, 30000, 50}, 0},
{"MAX ALT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_alt_alarm, 1, 200, 1}, 0},
{"MAX DISTANCE", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_distance_alarm, 0, UINT16_MAX, 10}, 0},

65
src/main/common/filter.c
View file

@ -65,6 +65,71 @@ FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
return filter->state;
}
// PT2 Low Pass filter
float pt2FilterGain(float f_cut, float dT)
{
const float order = 2.0f;
const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1));
float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut);
//float RC = 1 / ( 2 * 1.553773974f * M_PIf * f_cut);
// where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2
return dT / (RC + dT);
}
void pt2FilterInit(pt2Filter_t *filter, float k)
{
filter->state = 0.0f;
filter->state1 = 0.0f;
filter->k = k;
}
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
{
filter->k = k;
}
FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
{
filter->state1 = filter->state1 + filter->k * (input - filter->state1);
filter->state = filter->state + filter->k * (filter->state1 - filter->state);
return filter->state;
}
// PT3 Low Pass filter
float pt3FilterGain(float f_cut, float dT)
{
const float order = 3.0f;
const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1));
float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut);
// float RC = 1 / ( 2 * 1.961459177f * M_PIf * f_cut);
// where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3
return dT / (RC + dT);
}
void pt3FilterInit(pt3Filter_t *filter, float k)
{
filter->state = 0.0f;
filter->state1 = 0.0f;
filter->state2 = 0.0f;
filter->k = k;
}
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
{
filter->k = k;
}
FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
{
filter->state1 = filter->state1 + filter->k * (input - filter->state1);
filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2);
filter->state = filter->state + filter->k * (filter->state2 - filter->state);
return filter->state;
}
// Slew filter with limit
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)

25
src/main/common/filter.h
View file

@ -29,6 +29,19 @@ typedef struct pt1Filter_s {
float k;
} pt1Filter_t;
typedef struct pt2Filter_s {
float state;
float state1;
float k;
} pt2Filter_t;
typedef struct pt3Filter_s {
float state;
float state1;
float state2;
float k;
} pt3Filter_t;
typedef struct slewFilter_s {
float state;
float slewLimit;
@ -52,6 +65,8 @@ typedef struct laggedMovingAverage_s {
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_PT2,
FILTER_PT3,
} lowpassFilterType_e;
typedef enum {
@ -81,5 +96,15 @@ void pt1FilterInit(pt1Filter_t *filter, float k);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt2FilterGain(float f_cut, float dT);
void pt2FilterInit(pt2Filter_t *filter, float k);
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
float pt2FilterApply(pt2Filter_t *filter, float input);
float pt3FilterGain(float f_cut, float dT);
void pt3FilterInit(pt3Filter_t *filter, float k);
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
float pt3FilterApply(pt3Filter_t *filter, float input);
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
float slewFilterApply(slewFilter_t *filter, float input);

7
src/main/common/maths.c
View file

@ -115,13 +115,6 @@ int gcd(int num, int denom)
return gcd(denom, num % denom);
}
float powerf(float base, int exp) {
float result = base;
for (int count = 1; count < exp; count++) result *= base;
return result;
}
int32_t applyDeadband(const int32_t value, const int32_t deadband)
{
if (ABS(value) < deadband) {

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

@ -101,7 +101,6 @@ typedef struct fp_rotationMatrix_s {
} fp_rotationMatrix_t;
int gcd(int num, int denom);
float powerf(float base, int exp);
int32_t applyDeadband(int32_t value, int32_t deadband);
float fapplyDeadband(float value, float deadband);

2
src/main/common/sdft.c
View file

@ -38,7 +38,7 @@ static void applySqrt(const sdft_t *sdft, float *data);
void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches)
{
if (!isInitialized) {
rPowerN = powerf(SDFT_R, SDFT_SAMPLE_SIZE);
rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE);
const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE;
float phi = 0.0f;
for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) {

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

@ -183,7 +183,7 @@ typedef struct odrEntry_s {
uint8_t odr; // See GYRO_ODR in datasheet.
} odrEntry_t;
odrEntry_t khzToSupportedODRMap[] = {
static odrEntry_t icm42605PkhzToSupportedODRMap[] = {
{ 8, 3 },
{ 4, 4 },
{ 2, 5 },
@ -205,9 +205,9 @@ void icm42605GyroInit(gyroDev_t *gyro)
if (gyro->gyroRateKHz) {
uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c
uint8_t desiredODRKhz = 8 / gyroSyncDenominator;
for (uint32_t i = 0; i < ARRAYLEN(khzToSupportedODRMap); i++) {
if (khzToSupportedODRMap[i].khz == desiredODRKhz) {
outputDataRate = khzToSupportedODRMap[i].odr;
for (uint32_t i = 0; i < ARRAYLEN(icm42605PkhzToSupportedODRMap); i++) {
if (icm42605PkhzToSupportedODRMap[i].khz == desiredODRKhz) {
outputDataRate = icm42605PkhzToSupportedODRMap[i].odr;
supportedODRFound = true;
break;
}
@ -219,22 +219,14 @@ void icm42605GyroInit(gyroDev_t *gyro)
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
}
uint8_t value;
STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
delay(15);
value = spiBusReadRegister(&gyro->bus, ICM42605_RA_GYRO_CONFIG0);
debug[1] = value;
STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
spiBusWriteRegister(&gyro->bus, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
delay(15);
value = spiBusReadRegister(&gyro->bus, ICM42605_RA_ACCEL_CONFIG0);
debug[2] = value;
spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY);
spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);

16
src/main/drivers/barometer/barometer_bmp388.c
View file

@ -20,28 +20,26 @@
* BMP388 Driver author: Dominic Clifton
*/
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "build/build_config.h"
#if defined(USE_BARO) && (defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388))
#include "build/build_config.h"
#include "build/debug.h"
#include "common/maths.h"
#include "barometer.h"
#include "drivers/barometer/barometer.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/time.h"
#include "drivers/nvic.h"
#include "drivers/time.h"
#include "barometer_bmp388.h"
@ -303,8 +301,8 @@ bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro)
// See datasheet 3.9.2 "Measurement rate in forced mode and normal mode"
baro->up_delay = 234 +
(392 + (powerf(2, BMP388_PRESSURE_OSR + 1) * 2000)) +
(313 + (powerf(2, BMP388_TEMPERATURE_OSR + 1) * 2000));
(392 + (powf(2, BMP388_PRESSURE_OSR + 1) * 2000)) +
(313 + (powf(2, BMP388_TEMPERATURE_OSR + 1) * 2000));
baro->calculate = bmp388Calculate;

25
src/main/drivers/bus_spi_hal.c
View file

@ -36,6 +36,8 @@
#include "nvic.h"
#include "rcc.h"
#define SPI_TIMEOUT_SYS_TICKS (SPI_TIMEOUT_US / 1000)
void spiInitDevice(SPIDevice device, bool leadingEdge)
{
spiDevice_t *spi = &(spiDevice[device]);
@ -93,8 +95,7 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
if (spi->leadingEdge) {
spi->hspi.Init.CLKPolarity = SPI_POLARITY_LOW;
spi->hspi.Init.CLKPhase = SPI_PHASE_1EDGE;
}
else {
} else {
spi->hspi.Init.CLKPolarity = SPI_POLARITY_HIGH;
spi->hspi.Init.CLKPhase = SPI_PHASE_2EDGE;
}
@ -118,10 +119,11 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t out)
bool spiIsBusBusy(SPI_TypeDef *instance)
{
SPIDevice device = spiDeviceByInstance(instance);
if(spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY)
if (spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY) {
return true;
else
} else {
return false;
}
}
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *out, uint8_t *in, int len)
@ -129,20 +131,18 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *out, uint8_t *in, int len
SPIDevice device = spiDeviceByInstance(instance);
HAL_StatusTypeDef status;
#define SPI_DEFAULT_TIMEOUT 10
if (!in) {
// Tx only
status = HAL_SPI_Transmit(&spiDevice[device].hspi, out, len, SPI_DEFAULT_TIMEOUT);
} else if(!out) {
status = HAL_SPI_Transmit(&spiDevice[device].hspi, out, len, SPI_TIMEOUT_SYS_TICKS);
} else if (!out) {
// Rx only
status = HAL_SPI_Receive(&spiDevice[device].hspi, in, len, SPI_DEFAULT_TIMEOUT);
status = HAL_SPI_Receive(&spiDevice[device].hspi, in, len, SPI_TIMEOUT_SYS_TICKS);
} else {
// Tx and Rx
status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, out, in, len, SPI_DEFAULT_TIMEOUT);
status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, out, in, len, SPI_TIMEOUT_SYS_TICKS);
}
if(status != HAL_OK) {
if (status != HAL_OK) {
spiTimeoutUserCallback(instance);
}
@ -210,8 +210,9 @@ void SPI4_IRQHandler(void)
void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor)
{
SPIDevice device = descriptor->userParam;
if (device != SPIINVALID)
if (device != SPIINVALID) {
HAL_DMA_IRQHandler(&spiDevice[device].hdma);
}
}
#endif // USE_DMA
#endif

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

@ -20,6 +20,8 @@
#pragma once
#define SPI_TIMEOUT_US 10000
#if defined(STM32F1) || defined(STM32F3) || defined(STM32F4) || defined(STM32G4)
#define MAX_SPI_PIN_SEL 2
#elif defined(STM32F7)

44
src/main/drivers/bus_spi_ll.c
View file

@ -36,6 +36,7 @@
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/rcc.h"
#include "drivers/time.h"
#ifndef SPI2_SCK_PIN
#define SPI2_NSS_PIN PB12
@ -71,8 +72,6 @@
#define SPI4_NSS_PIN NONE
#endif
#define SPI_DEFAULT_TIMEOUT 10
static LL_SPI_InitTypeDef defaultInit =
{
.TransferDirection = SPI_DIRECTION_2LINES,
@ -107,10 +106,12 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device));
if (spi->leadingEdge == true)
if (spi->leadingEdge == true) {
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF);
else
} else {
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
}
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF);
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF);
@ -136,18 +137,21 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte)
{
uint16_t spiTimeout = 1000;
timeUs_t timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_TXE(instance))
if ((spiTimeout--) == 0)
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
LL_SPI_TransmitData8(instance, txByte);
spiTimeout = 1000;
while (!LL_SPI_IsActiveFlag_RXNE(instance))
if ((spiTimeout--) == 0)
timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_RXNE(instance)) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
return (uint8_t)LL_SPI_ReceiveData8(instance);
}
@ -163,12 +167,14 @@ bool spiIsBusBusy(SPI_TypeDef *instance)
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
{
timeUs_t timeoutStartUs;
// set 16-bit transfer
CLEAR_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD);
while (len > 1) {
int spiTimeout = 1000;
timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
if ((spiTimeout--) == 0) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
@ -181,9 +187,9 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData,
}
LL_SPI_TransmitData16(instance, w);
spiTimeout = 1000;
timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_RXNE(instance)) {
if ((spiTimeout--) == 0) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
@ -197,18 +203,18 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData,
// set 8-bit transfer
SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD);
if (len) {
int spiTimeout = 1000;
timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
if ((spiTimeout--) == 0) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
uint8_t b = txData ? *(txData++) : 0xFF;
LL_SPI_TransmitData8(instance, b);
spiTimeout = 1000;
timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_RXNE(instance)) {
if ((spiTimeout--) == 0) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}

31
src/main/drivers/bus_spi_stdperiph.c
View file

@ -33,6 +33,7 @@
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/rcc.h"
#include "drivers/time.h"
static SPI_InitTypeDef defaultInit = {
.SPI_Mode = SPI_Mode_Master,
@ -104,23 +105,27 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
// return uint8_t value or -1 when failure
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte)
{
uint16_t spiTimeout = 1000;
timeUs_t timeoutStartUs = microsISR();
DISCARD(instance->DR);
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET)
if ((spiTimeout--) == 0)
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
#ifdef STM32F303xC
SPI_SendData8(instance, txByte);
#else
SPI_I2S_SendData(instance, txByte);
#endif
spiTimeout = 1000;
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET)
if ((spiTimeout--) == 0)
timeoutStartUs = microsISR();
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
#ifdef STM32F303xC
return ((uint8_t)SPI_ReceiveData8(instance));
@ -144,15 +149,17 @@ bool spiIsBusBusy(SPI_TypeDef *instance)
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
{
uint16_t spiTimeout = 1000;
timeUs_t timeoutStartUs;
uint8_t b;
DISCARD(instance->DR);
while (len--) {
b = txData ? *(txData++) : 0xFF;
timeoutStartUs = microsISR();
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) {
if ((spiTimeout--) == 0)
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
#ifdef STM32F303xC
SPI_SendData8(instance, b);
@ -160,19 +167,21 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData,
SPI_I2S_SendData(instance, b);
#endif
spiTimeout = 1000;
timeoutStartUs = microsISR();
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) {
if ((spiTimeout--) == 0)
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
#ifdef STM32F303xC
b = SPI_ReceiveData8(instance);
#else
b = SPI_I2S_ReceiveData(instance);
#endif
if (rxData)
if (rxData) {
*(rxData++) = b;
}
}
return true;

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

@ -89,7 +89,8 @@ struct {
{ 0xEF4016, 133, 50, 64, 256 },
// Winbond W25Q64
// Datasheet: https://www.winbond.com/resource-files/w25q64jv%20spi%20%20%20revc%2006032016%20kms.pdf
{ 0xEF4017, 133, 50, 128, 256 },
{ 0xEF4017, 133, 50, 128, 256 }, // W25Q64JV-IQ/JQ
{ 0xEF7017, 133, 50, 128, 256 }, // W25Q64JV-IM/JM*
// Winbond W25Q128
// Datasheet: https://www.winbond.com/resource-files/w25q128fv%20rev.l%2008242015.pdf
{ 0xEF4018, 104, 50, 256, 256 },

5
src/main/drivers/osd_symbols.h
View file

@ -156,3 +156,8 @@
#define SYM_STICK_OVERLAY_CENTER 0x0B
#define SYM_STICK_OVERLAY_VERTICAL 0x16
#define SYM_STICK_OVERLAY_HORIZONTAL 0x17
// GPS degree/minute/second symbols
#define SYM_GPS_DEGREE SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol
#define SYM_GPS_MINUTE 0x27 // '
#define SYM_GPS_SECOND 0x22 // "

14
src/main/fc/controlrate_profile.c
View file

@ -47,16 +47,16 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
.thrExpo8 = 0,
.dynThrPID = 65,
.tpa_breakpoint = 1350,
.rates_type = RATES_TYPE_BETAFLIGHT,
.rcRates[FD_ROLL] = 100,
.rcRates[FD_PITCH] = 100,
.rcRates[FD_YAW] = 100,
.rates_type = RATES_TYPE_ACTUAL,
.rcRates[FD_ROLL] = 7,
.rcRates[FD_PITCH] = 7,
.rcRates[FD_YAW] = 7,
.rcExpo[FD_ROLL] = 0,
.rcExpo[FD_PITCH] = 0,
.rcExpo[FD_YAW] = 0,
.rates[FD_ROLL] = 70,
.rates[FD_PITCH] = 70,
.rates[FD_YAW] = 70,
.rates[FD_ROLL] = 67,
.rates[FD_PITCH] = 67,
.rates[FD_YAW] = 67,
.throttle_limit_type = THROTTLE_LIMIT_TYPE_OFF,
.throttle_limit_percent = 100,
.rate_limit[FD_ROLL] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,

11
src/main/fc/init.c
View file

@ -101,11 +101,9 @@
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
#include "io/dashboard.h"
#include "io/displayport_crsf.h"
#include "io/displayport_frsky_osd.h"
#include "io/displayport_max7456.h"
#include "io/displayport_msp.h"
#include "io/displayport_srxl.h"
#include "io/flashfs.h"
#include "io/gimbal.h"
#include "io/gps.h"
@ -1011,15 +1009,6 @@ void init(void)
}
#endif
#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
// Register the srxl Textgen telemetry sensor as a displayport device
cmsDisplayPortRegister(displayPortSrxlInit());
#endif
#if defined(USE_CMS) && defined(USE_CRSF_CMS_TELEMETRY)
cmsDisplayPortRegister(displayPortCrsfInit());
#endif
setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
#ifdef USE_MOTOR

117
src/main/fc/rc.c
View file

@ -27,7 +27,6 @@
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/config.h"
@ -61,7 +60,11 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
#ifdef USE_INTERPOLATED_SP
// Setpoint in degrees/sec before RC-Smoothing is applied
static float rawSetpoint[XYZ_AXIS_COUNT];
static float oldRcCommand[XYZ_AXIS_COUNT];
static bool isDuplicate[XYZ_AXIS_COUNT];
float rcCommandDelta[XYZ_AXIS_COUNT];
#endif
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;
static bool reverseMotors = false;
@ -82,7 +85,6 @@ enum {
};
#ifdef USE_RC_SMOOTHING_FILTER
#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1
#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation
#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average during initial training
#define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes
@ -91,7 +93,7 @@ enum {
#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining
#define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms
#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz
#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_DERIVATIVE_PT1_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled
#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ 100 // Initial value for "auto" when interpolated feedforward is enabled
static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
#endif // USE_RC_SMOOTHING_FILTER
@ -133,6 +135,11 @@ float getRawSetpoint(int axis)
return rawSetpoint[axis];
}
float getRcCommandDelta(int axis)
{
return rcCommandDelta[axis];
}
#endif
#define THROTTLE_LOOKUP_LENGTH 12
@ -195,7 +202,7 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs
float applyActualRates(const int axis, float rcCommandf, const float rcCommandfAbs)
{
float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
expof = rcCommandfAbs * (powerf(rcCommandf, 5) * expof + rcCommandf * (1 - expof));
expof = rcCommandfAbs * (powf(rcCommandf, 5) * expof + rcCommandf * (1 - expof));
const float centerSensitivity = currentControlRateProfile->rcRates[axis] * 10.0f;
const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity);
@ -395,16 +402,12 @@ uint16_t getCurrentRxRefreshRate(void)
#ifdef USE_RC_SMOOTHING_FILTER
// Determine a cutoff frequency based on filter type and the calculated
// average rx frame time
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1, uint8_t autoSmoothnessFactor)
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
{
if (avgRxFrameTimeUs > 0) {
const float cutoffFactor = (100 - autoSmoothnessFactor) / 100.0f;
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency
const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f));
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)); // link frequency
cutoff = cutoff * cutoffFactor;
if (pt1) {
cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics
}
return lrintf(cutoff);
} else {
return 0;
@ -426,31 +429,17 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
uint16_t oldCutoff = smoothingData->inputCutoffFrequency;
if (smoothingData->inputCutoffSetting == 0) {
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->inputFilterType == RC_SMOOTHING_INPUT_PT1), smoothingData->autoSmoothnessFactor);
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
}
// initialize or update the input filter
if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch
switch (smoothingData->inputFilterType) {
case RC_SMOOTHING_INPUT_PT1:
if (!smoothingData->filterInitialized) {
pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
} else {
pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
}
break;
case RC_SMOOTHING_INPUT_BIQUAD:
default:
if (!smoothingData->filterInitialized) {
biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
} else {
biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
}
break;
if (!smoothingData->filterInitialized) {
pt3FilterInit((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
}
}
}
@ -458,15 +447,12 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
// update or initialize the derivative filter
oldCutoff = smoothingData->derivativeCutoffFrequency;
if ((rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)
&& (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF)
&& (rcSmoothingData.derivativeCutoffSetting == 0)) {
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_PT1), smoothingData->autoSmoothnessFactor);
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
}
if (!smoothingData->filterInitialized) {
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis, smoothingData->derivativeFilterType);
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis);
} else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
}
@ -505,14 +491,7 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
{
// if the input cutoff is 0 (auto) then we need to calculate cutoffs
if (rcSmoothingData.inputCutoffSetting == 0) {
return true;
}
// if the derivative type isn't OFF, and the cutoff is 0, and interpolated feedforward is not enabled then we need to calculate
if ((rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)
&& (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF)
&& (rcSmoothingData.derivativeCutoffSetting == 0)) {
if ((rcSmoothingData.inputCutoffSetting == 0) || (rcSmoothingData.derivativeCutoffSetting == 0)) {
return true;
}
return false;
@ -533,39 +512,19 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
rcSmoothingData.averageFrameTimeUs = 0;
rcSmoothingData.autoSmoothnessFactor = rxConfig()->rc_smoothing_auto_factor;
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
rcSmoothingData.inputFilterType = rxConfig()->rc_smoothing_input_type;
rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff;
rcSmoothingData.derivativeFilterTypeSetting = rxConfig()->rc_smoothing_derivative_type;
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_AUTO) {
// for derivative filter type "AUTO" set to BIQUAD for classic FF and PT1 for interpolated FF
if (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF) {
rcSmoothingData.derivativeFilterType = RC_SMOOTHING_DERIVATIVE_BIQUAD;
} else {
rcSmoothingData.derivativeFilterType = RC_SMOOTHING_DERIVATIVE_PT1;
}
} else {
rcSmoothingData.derivativeFilterType = rxConfig()->rc_smoothing_derivative_type;
}
rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff;
rcSmoothingResetAccumulation(&rcSmoothingData);
rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting;
if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) {
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
// calculate the fixed derivative cutoff used for interpolated feedforward
const float cutoffFactor = (100 - rcSmoothingData.autoSmoothnessFactor) / 100.0f;
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_DERIVATIVE_PT1_HZ * cutoffFactor; // PT1 cutoff frequency
if (rcSmoothingData.derivativeFilterType == RC_SMOOTHING_DERIVATIVE_BIQUAD) {
// convert to an equivalent BIQUAD cutoff
derivativeCutoff = sqrt(derivativeCutoff * RC_SMOOTHING_IDENTITY_FREQUENCY);
}
rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff);
} else {
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
}
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
// calculate the initial derivative cutoff used for interpolated feedforward until RC interval is known
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactor / 10.0f));
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ * cutoffFactor; // PT1 cutoff frequency
rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff);
} else {
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
}
calculateCutoffs = rcSmoothingAutoCalculate();
@ -658,16 +617,7 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value
if (rcSmoothingData.filterInitialized) {
switch (rcSmoothingData.inputFilterType) {
case RC_SMOOTHING_INPUT_PT1:
rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
break;
case RC_SMOOTHING_INPUT_BIQUAD:
default:
rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
break;
}
rcCommand[updatedChannel] = pt3FilterApply((pt3Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
} else {
// If filter isn't initialized yet then use the actual unsmoothed rx channel data
rcCommand[updatedChannel] = lastRxData[updatedChannel];
@ -694,6 +644,9 @@ FAST_CODE void processRcCommand(void)
#ifdef USE_INTERPOLATED_SP
if (isRxDataNew) {
for (int i = FD_ROLL; i <= FD_YAW; i++) {
isDuplicate[i] = (oldRcCommand[i] == rcCommand[i]);
rcCommandDelta[i] = fabsf(rcCommand[i] - oldRcCommand[i]);
oldRcCommand[i] = rcCommand[i];
float rcCommandf;
if (i == FD_YAW) {
rcCommandf = rcCommand[i] / rcCommandYawDivider;
@ -763,7 +716,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
for (int axis = 0; axis < 3; axis++) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
float tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;

3
src/main/fc/rc.h
View file

@ -34,7 +34,7 @@ typedef enum {
#ifdef USE_RC_SMOOTHING_FILTER
#define RC_SMOOTHING_AUTO_FACTOR_MIN 0
#define RC_SMOOTHING_AUTO_FACTOR_MAX 50
#define RC_SMOOTHING_AUTO_FACTOR_MAX 250
#endif
void processRcCommand(void);
@ -51,6 +51,7 @@ rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void);
float getRawSetpoint(int axis);
float getRcCommandDelta(int axis);
float applyCurve(int axis, float deflection);
bool getShouldUpdateFf();
void updateRcRefreshRate(timeUs_t currentTimeUs);

22
src/main/fc/rc_controls.h
View file

@ -70,18 +70,6 @@ typedef enum {
RC_SMOOTHING_TYPE_FILTER
} rcSmoothingType_e;
typedef enum {
RC_SMOOTHING_INPUT_PT1,
RC_SMOOTHING_INPUT_BIQUAD
} rcSmoothingInputFilter_e;
typedef enum {
RC_SMOOTHING_DERIVATIVE_OFF,
RC_SMOOTHING_DERIVATIVE_PT1,
RC_SMOOTHING_DERIVATIVE_BIQUAD,
RC_SMOOTHING_DERIVATIVE_AUTO,
} rcSmoothingDerivativeFilter_e;
#define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL))
#define ROL_HI (2 << (2 * ROLL))
@ -116,19 +104,11 @@ typedef struct rcSmoothingFilterTraining_s {
uint16_t max;
} rcSmoothingFilterTraining_t;
typedef union rcSmoothingFilterTypes_u {
pt1Filter_t pt1Filter;
biquadFilter_t biquadFilter;
} rcSmoothingFilterTypes_t;
typedef struct rcSmoothingFilter_s {
bool filterInitialized;
rcSmoothingFilterTypes_t filter[4];
rcSmoothingInputFilter_e inputFilterType;
pt3Filter_t filter[4];
uint8_t inputCutoffSetting;
uint16_t inputCutoffFrequency;
rcSmoothingDerivativeFilter_e derivativeFilterTypeSetting;
rcSmoothingDerivativeFilter_e derivativeFilterType;
uint8_t derivativeCutoffSetting;
uint16_t derivativeCutoffFrequency;
int averageFrameTimeUs;

1
src/main/fc/runtime_config.h
View file

@ -114,6 +114,7 @@ extern uint16_t flightModeFlags;
typedef enum {
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
GPS_FIX_EVER = (1 << 2),
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))

2
src/main/fc/tasks.c
View file

@ -179,6 +179,8 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
case PROCESS:
ignoreTaskTime();
if (!processRx(currentTimeUs)) {
rxState = CHECK;
break;
}
rxState = MODES;

147
src/main/flight/interpolated_setpoint.c
View file

@ -43,15 +43,12 @@ typedef struct laggedMovingAverageCombined_s {
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
static float prevAcceleration[XYZ_AXIS_COUNT];
static float prevRawSetpoint[XYZ_AXIS_COUNT];
//for smoothing
static float prevDeltaImpl[XYZ_AXIS_COUNT];
static float prevBoostAmount[XYZ_AXIS_COUNT];
static float prevSetpoint[XYZ_AXIS_COUNT]; // equals raw unless interpolated
static float prevSetpointSpeed[XYZ_AXIS_COUNT]; // equals raw unless interpolated
static float prevAcceleration[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
static float prevRcCommandDelta[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
static uint8_t ffStatus[XYZ_AXIS_COUNT];
static bool bigStep[XYZ_AXIS_COUNT];
static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; // to identify multiple identical packets
static uint8_t averagingCount;
static float ffMaxRateLimit[XYZ_AXIS_COUNT];
@ -70,112 +67,86 @@ void interpolatedSpInit(const pidProfile_t *pidProfile) {
FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) {
if (newRcFrame) {
float rawSetpoint = getRawSetpoint(axis);
float absRawSetpoint = fabsf(rawSetpoint);
float rcCommandDelta = getRcCommandDelta(axis);
float setpoint = getRawSetpoint(axis);
const float rxInterval = getCurrentRxRefreshRate() * 1e-6f;
const float rxRate = 1.0f / rxInterval;
float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
float absSetpointSpeed = fabsf(setpointSpeed);
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
float setpointAccelerationModifier = 1.0f;
float setpointAcceleration = 0.0f;
const float ffSmoothFactor = pidGetFfSmoothFactor();
const float ffJitterFactor = pidGetFfJitterFactor();
if (setpointSpeed == 0 && absRawSetpoint < 0.98f * ffMaxRate[axis]) {
// no movement, or sticks at max; ffStatus set
// the max stick check is needed to prevent interpolation when arriving at max sticks
if (prevSetpointSpeed[axis] == 0) {
// no movement on two packets in a row
// do nothing now, but may use status = 3 to smooth following packet
ffStatus[axis] = 3;
} else {
// there was movement on previous packet, now none
if (bigStep[axis] == true) {
// previous movement was big; likely an early FrSky packet
// don't project these forward or we get a sustained large spike
ffStatus[axis] = 2;
} else {
// likely a dropped packet
// interpolate forward using previous setpoint speed and acceleration
setpointSpeed = prevSetpointSpeed[axis] + prevAcceleration[axis];
// use status = 1 to halve the step for the next packet
ffStatus[axis] = 1;
}
// calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold
float ffAttenuator = 1.0f;
if (ffJitterFactor) {
if (rcCommandDelta < ffJitterFactor) {
ffAttenuator = MAX(1.0f - ((rcCommandDelta + prevRcCommandDelta[axis]) / 2.0f) / ffJitterFactor, 0.0f);
ffAttenuator = 1.0f - ffAttenuator * ffAttenuator;
}
}
// interpolate setpoint if necessary
if (rcCommandDelta == 0.0f) {
if (prevDuplicatePacket[axis] == false && fabsf(setpoint) < 0.98f * ffMaxRate[axis]) {
// first duplicate after movement
// interpolate rawSetpoint by adding (speed + acceleration) * attenuator to previous setpoint
setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis]) * ffAttenuator * rxInterval;
// recalculate setpointSpeed and (later) acceleration from this new setpoint value
setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
}
prevDuplicatePacket[axis] = true;
} else {
// we have movement; let's consider what happened on previous packets, using ffStatus
if (ffStatus[axis] != 0) {
if (ffStatus[axis] == 1) {
// was interpolated forward after previous dropped packet after small step
// this step is likely twice as tall as it should be
setpointSpeed = setpointSpeed / 2.0f;
} else if (ffStatus[axis] == 2) {
// we are doing nothing for these to avoid exaggerating the FrSky early packet problem
} else if (ffStatus[axis] == 3) {
// movement after nothing on previous two packets
// reduce boost when higher averaging is used to improve slow stick smoothness
setpointAccelerationModifier /= (averagingCount + 1);
}
ffStatus[axis] = 0;
// all is normal
// movement!
if (prevDuplicatePacket[axis] == true) {
// don't boost the packet after a duplicate, the FF alone is enough, usually
// in part because after a duplicate, the raw up-step is large, so the jitter attenuator is less active
ffAttenuator = 0.0f;
}
prevDuplicatePacket[axis] = false;
}
prevSetpoint[axis] = setpoint;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpoint)); // setpoint after interpolations
}
float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
// determine if this step was a relatively large one, to use when evaluating next packet
// calculate acceleration, smooth and attenuate it
setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
setpointAcceleration = prevAcceleration[axis] + ffSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
setpointAcceleration *= ffAttenuator;
if (absSetpointSpeed > 1.5f * absPrevSetpointSpeed || absPrevSetpointSpeed > 1.5f * absSetpointSpeed){
bigStep[axis] = true;
} else {
bigStep[axis] = false;
}
// smooth deadband type suppression of FF jitter when sticks are at or returning to centre
// only when ff_averaging is 3 or more, for HD or cinematic flying
if (averagingCount > 2) {
const float rawSetpointCentred = absRawSetpoint / averagingCount;
if (rawSetpointCentred < 1.0f) {
setpointSpeed *= rawSetpointCentred;
setpointAcceleration *= rawSetpointCentred;
}
}
// smooth setpointSpeed but don't attenuate
setpointSpeed = prevSetpointSpeed[axis] + ffSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
prevSetpointSpeed[axis] = setpointSpeed;
prevAcceleration[axis] = setpointAcceleration;
// all values afterwards are small numbers
prevRcCommandDelta[axis] = rcCommandDelta;
setpointAcceleration *= pidGetDT();
setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
// calculate boost and prevent kick-back spike at max deflection
const float ffBoostFactor = pidGetFfBoostFactor();
float boostAmount = 0.0f;
if (ffBoostFactor != 0.0f) {
// calculate boost and prevent kick-back spike at max deflection
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
boostAmount = ffBoostFactor * setpointAcceleration * setpointAccelerationModifier;
if (ffBoostFactor) {
if (fabsf(setpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
boostAmount = ffBoostFactor * setpointAcceleration;
}
}
prevSetpointSpeed[axis] = setpointSpeed;
prevRawSetpoint[axis] = rawSetpoint;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100));
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAcceleration * 100));
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(((setpointDeltaImpl[axis] + boostAmount) * 100)));
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, ffStatus[axis]);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base FF
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(boostAmount * 100.0f)); // boost amount
// debug 2 is interpolated setpoint, above
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference
}
// first order smoothing of boost to reduce jitter
const float ffSmoothFactor = pidGetFfSmoothFactor();
boostAmount = prevBoostAmount[axis] + ffSmoothFactor * (boostAmount - prevBoostAmount[axis]);
prevBoostAmount[axis] = boostAmount;
// add boost to base feed forward
setpointDeltaImpl[axis] += boostAmount;
// first order smoothing of FF (second order boost filtering since boost filtered twice)
setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]);
prevDeltaImpl[axis] = setpointDeltaImpl[axis];
// apply averaging
if (type == FF_INTERPOLATE_ON) {
setpointDelta[axis] = setpointDeltaImpl[axis];

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

@ -30,7 +30,6 @@
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#include "config/config_reset.h"
#include "config/simplified_tuning.h"
@ -206,6 +205,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.ff_interpolate_sp = FF_INTERPOLATE_ON,
.ff_max_rate_limit = 100,
.ff_smooth_factor = 37,
.ff_jitter_factor = 7,
.ff_boost = 15,
.dyn_lpf_curve_expo = 5,
.level_race_mode = false,
@ -266,6 +266,11 @@ float pidGetFfSmoothFactor()
{
return pidRuntime.ffSmoothFactor;
}
float pidGetFfJitterFactor()
{
return pidRuntime.ffJitterFactor;
}
#endif
void pidResetIterm(void)
@ -315,7 +320,7 @@ float pidCompensateThrustLinearization(float throttle)
if (pidRuntime.thrustLinearization != 0.0f) {
// for whoops where a lot of TL is needed, allow more throttle boost
const float throttleReversed = (1.0f - throttle);
throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powerf(throttleReversed, 2);
throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powf(throttleReversed, 2);
}
return throttle;
}
@ -325,7 +330,7 @@ float pidApplyThrustLinearization(float motorOutput)
if (pidRuntime.thrustLinearization != 0.0f) {
if (motorOutput > 0.0f) {
const float motorOutputReversed = (1.0f - motorOutput);
motorOutput *= 1.0f + powerf(motorOutputReversed, 2) * pidRuntime.thrustLinearization;
motorOutput *= 1.0f + powf(motorOutputReversed, 2) * pidRuntime.thrustLinearization;
}
}
return motorOutput;
@ -638,14 +643,7 @@ float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelt
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
}
if (pidRuntime.setpointDerivativeLpfInitialized) {
switch (pidRuntime.rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
ret = pt1FilterApply(&pidRuntime.setpointDerivativePt1[axis], pidSetpointDelta);
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
ret = biquadFilterApplyDF1(&pidRuntime.setpointDerivativeBiquad[axis], pidSetpointDelta);
break;
}
ret = pt3FilterApply(&pidRuntime.setpointDerivativePt3[axis], pidSetpointDelta);
if (axis == pidRuntime.rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
}

6
src/main/flight/pid.h
View file

@ -210,6 +210,7 @@ typedef struct pidProfile_s {
uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps
uint8_t ff_jitter_factor; // Number of RC steps below which to attenuate FF
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter
uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount
@ -340,8 +341,7 @@ typedef struct pidRuntime_s {
#endif
#ifdef USE_RC_SMOOTHING_FILTER
pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
pt3Filter_t setpointDerivativePt3[XYZ_AXIS_COUNT];
bool setpointDerivativeLpfInitialized;
uint8_t rcSmoothingDebugAxis;
uint8_t rcSmoothingFilterType;
@ -386,6 +386,7 @@ typedef struct pidRuntime_s {
#ifdef USE_INTERPOLATED_SP
ffInterpolationType_t ffFromInterpolatedSetpoint;
float ffSmoothFactor;
float ffJitterFactor;
#endif
} pidRuntime_t;
@ -440,4 +441,5 @@ float pidGetDT();
float pidGetPidFrequency();
float pidGetFfBoostFactor();
float pidGetFfSmoothFactor();
float pidGetFfJitterFactor();
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);

36
src/main/flight/pid_init.c
View file

@ -30,7 +30,6 @@
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#include "drivers/dshot_command.h"
@ -222,37 +221,22 @@ void pidInit(const pidProfile_t *pidProfile)
}
#ifdef USE_RC_SMOOTHING_FILTER
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis)
{
pidRuntime.rcSmoothingDebugAxis = debugAxis;
pidRuntime.rcSmoothingFilterType = filterType;
if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
if (filterCutoff > 0) {
pidRuntime.setpointDerivativeLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (pidRuntime.rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterInit(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterInitLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
}
pt3FilterInit(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
}
}
}
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
{
if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
if (filterCutoff > 0) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (pidRuntime.rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterUpdateCutoff(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterUpdateLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
}
pt3FilterUpdateCutoff(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
}
}
}
@ -378,7 +362,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#ifdef USE_THRUST_LINEARIZATION
pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powerf(pidRuntime.thrustLinearization, 2);
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powf(pidRuntime.thrustLinearization, 2);
#endif
#if defined(USE_D_MIN)
@ -401,7 +385,13 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#ifdef USE_INTERPOLATED_SP
pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
if (pidProfile->ff_smooth_factor) {
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
} else {
// set automatically according to boost amount, limit to 0.5 for auto
pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->ff_boost) * 2.0f / 100.0f);
}
pidRuntime.ffJitterFactor = pidProfile->ff_jitter_factor;
interpolatedSpInit(pidProfile);
#endif

2
src/main/flight/pid_init.h
View file

@ -24,6 +24,6 @@ void pidInit(const pidProfile_t *pidProfile);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
void pidSetItermAccelerator(float newItermAccelerator);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);

24
src/main/io/displayport_crsf.c
View file

@ -31,14 +31,10 @@
#include "common/printf.h"
#include "common/time.h"
#include "config/feature.h"
#include "drivers/display.h"
#include "drivers/time.h"
#include "io/displayport_crsf.h"
#include "rx/rx.h"
#include "displayport_crsf.h"
#define CRSF_DISPLAY_PORT_OPEN_DELAY_MS 400
#define CRSF_DISPLAY_PORT_CLEAR_DELAY_MS 45
@ -203,18 +199,16 @@ bool crsfDisplayPortIsReady(void)
return (bool)(delayExpired && cmsReady);
}
displayPort_t *displayPortCrsfInit()
static displayPort_t *displayPortCrsfInit()
{
if (featureIsEnabled(FEATURE_TELEMETRY)
&& featureIsEnabled(FEATURE_RX_SERIAL)
&& (rxConfig()->serialrx_provider == SERIALRX_CRSF)) {
crsfDisplayPortSetDimensions(CRSF_DISPLAY_PORT_ROWS_MAX, CRSF_DISPLAY_PORT_COLS_MAX);
displayInit(&crsfDisplayPort, &crsfDisplayPortVTable, DISPLAYPORT_DEVICE_TYPE_CRSF);
crsfDisplayPortSetDimensions(CRSF_DISPLAY_PORT_ROWS_MAX, CRSF_DISPLAY_PORT_COLS_MAX);
displayInit(&crsfDisplayPort, &crsfDisplayPortVTable, DISPLAYPORT_DEVICE_TYPE_CRSF);
return &crsfDisplayPort;
} else {
return NULL;
}
return &crsfDisplayPort;
}
void crsfDisplayportRegister(void)
{
cmsDisplayPortRegister(displayPortCrsfInit());
}
#endif

2
src/main/io/displayport_crsf.h
View file

@ -34,7 +34,7 @@ typedef struct crsfDisplayPortScreen_s {
bool reset;
} crsfDisplayPortScreen_t;
displayPort_t *displayPortCrsfInit(void);
void crsfDisplayportRegister(void);
crsfDisplayPortScreen_t *crsfDisplayPortScreen(void);
void crsfDisplayPortMenuOpen(void);
void crsfDisplayPortMenuExit(void);

2
src/main/io/displayport_hott.c
View file

@ -128,7 +128,7 @@ static const displayPortVTable_t hottVTable = {
.layerCopy = NULL,
};
displayPort_t *displayPortHottInit()
static displayPort_t *displayPortHottInit()
{
hottDisplayPort.device = NULL;
displayInit(&hottDisplayPort, &hottVTable, DISPLAYPORT_DEVICE_TYPE_HOTT);

4
src/main/io/displayport_hott.h
View file

@ -21,10 +21,6 @@
#pragma once
#include "drivers/display.h"
displayPort_t *displayPortHottInit();
extern displayPort_t hottDisplayPort;
void hottDisplayportRegister();
void hottCmsOpen();
void hottSetCmsKey(uint8_t hottKey, bool esc);

31
src/main/io/displayport_srxl.c
View file

@ -23,20 +23,19 @@
#include <string.h>
#include "platform.h"
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) && defined(USE_TELEMETRY_SRXL)
#if defined(USE_SPEKTRUM_CMS_TELEMETRY)
#include "cms/cms.h"
#include "common/utils.h"
#include "config/feature.h"
#include "drivers/display.h"
#include "rx/rx.h"
#include "telemetry/srxl.h"
#include "displayport_srxl.h"
displayPort_t srxlDisplayPort;
static int srxlDrawScreen(displayPort_t *displayPort)
@ -143,20 +142,18 @@ static const displayPortVTable_t srxlVTable = {
.layerCopy = NULL,
};
displayPort_t *displayPortSrxlInit()
static displayPort_t *displayPortSrxlInit()
{
if (featureIsEnabled(FEATURE_TELEMETRY)
&& featureIsEnabled(FEATURE_RX_SERIAL)
&& ((rxConfig()->serialrx_provider == SERIALRX_SRXL) || (rxConfig()->serialrx_provider == SERIALRX_SRXL2))) {
srxlDisplayPort.device = NULL;
displayInit(&srxlDisplayPort, &srxlVTable, DISPLAYPORT_DEVICE_TYPE_SRXL);
srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
srxlDisplayPort.device = NULL;
displayInit(&srxlDisplayPort, &srxlVTable, DISPLAYPORT_DEVICE_TYPE_SRXL);
srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
return &srxlDisplayPort;
} else {
return NULL;
}
return &srxlDisplayPort;
}
void srxlDisplayportRegister(void)
{
cmsDisplayPortRegister(displayPortSrxlInit());
}
#endif

4
src/main/io/displayport_srxl.h
View file

@ -20,6 +20,6 @@
#pragma once
displayPort_t *displayPortSrxlInit();
extern displayPort_t srxlDisplayPort;
void srxlDisplayportRegister(void);

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

@ -968,11 +968,7 @@ static bool gpsNewFrameNMEA(char c)
gps_Msg.longitude *= -1;
break;
case 6:
if (string[0] > '0') {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
gpsSetFixState(string[0] > '0');
break;
case 7:
gps_Msg.numSat = grab_fields(string, 0);
@ -1295,11 +1291,7 @@ static bool UBLOX_parse_gps(void)
gpsSol.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm
if (next_fix) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
gpsSetFixState(next_fix);
_new_position = true;
break;
case MSG_STATUS:
@ -1517,7 +1509,7 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) {
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir);
if (gpsConfig()->gps_use_3d_speed) {
dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f));
}
@ -1527,8 +1519,8 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
}
lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat;
lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
lastAlt = gpsSol.llh.altCm;
lastMillis = currentMillis;
}
@ -1537,8 +1529,8 @@ void GPS_reset_home_position(void)
{
if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon;
GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
@ -1568,7 +1560,7 @@ 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_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
} else {
@ -1603,4 +1595,13 @@ void onGpsNewData(void)
#endif
}
void gpsSetFixState(bool state)
{
if (state) {
ENABLE_STATE(GPS_FIX);
ENABLE_STATE(GPS_FIX_EVER);
} else {
DISABLE_STATE(GPS_FIX);
}
}
#endif

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

@ -25,13 +25,15 @@
#include "pg/pg.h"
#define LAT 0
#define LON 1
#define GPS_DEGREES_DIVIDER 10000000L
#define GPS_X 1
#define GPS_Y 0
typedef enum {
GPS_LATITUDE,
GPS_LONGITUDE
} gpsCoordinateType_e;
typedef enum {
GPS_NMEA = 0,
GPS_UBLOX,
@ -188,4 +190,4 @@ void onGpsNewData(void);
void GPS_reset_home_position(void);
void GPS_calc_longitude_scaling(int32_t lat);
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
void gpsSetFixState(bool state);

14
src/main/msp/msp.c
View file

@ -1451,6 +1451,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, gpsRescueConfig()->descendRate);
sbufWriteU8(dst, gpsRescueConfig()->allowArmingWithoutFix);
sbufWriteU8(dst, gpsRescueConfig()->altitudeMode);
// Added in API version 1.44
sbufWriteU16(dst, gpsRescueConfig()->minRescueDth);
break;
case MSP_GPS_RESCUE_PIDS:
@ -2451,6 +2453,10 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
gpsRescueConfigMutable()->allowArmingWithoutFix = sbufReadU8(src);
gpsRescueConfigMutable()->altitudeMode = sbufReadU8(src);
}
if (sbufBytesRemaining(src) >= 2) {
// Added in API version 1.44
gpsRescueConfigMutable()->minRescueDth = sbufReadU16(src);
}
break;
case MSP_SET_GPS_RESCUE_PIDS:
@ -2660,7 +2666,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU8(src);
#endif
}
if (sbufBytesRemaining(src) >= 1) {
if (sbufBytesRemaining(src) >= 2) {
#if defined(USE_GYRO_DATA_ANALYSE)
// Added in MSP API 1.43
gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
@ -3175,11 +3181,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#ifdef USE_GPS
case MSP_SET_RAW_GPS:
if (sbufReadU8(src)) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
gpsSetFixState(sbufReadU8(src));
gpsSol.numSat = sbufReadU8(src);
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);

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

@ -160,6 +160,7 @@ typedef enum {
OSD_EFFICIENCY,
OSD_TOTAL_FLIGHTS,
OSD_UP_DOWN_REFERENCE,
OSD_TX_UPLINK_POWER,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;

156
src/main/osd/osd_elements.c
View file

@ -59,6 +59,45 @@
CLI parameters should be added before line #endif // end of #ifdef USE_OSD
*/
/*
*********************
OSD element variants:
*********************
Each element can have up to 4 display variants. "Type 1" is always the default and every
every element has an implicit type 1 variant even if no additional options exist. The
purpose is to allow the user to choose a different element display or rendering style to
fit their needs. Like displaying GPS coordinates in a different format, displaying a voltage
with a different number of decimal places, etc. The purpose is NOT to display unrelated
information in different variants of the element. For example it would be inappropriate
to use variants to display RSSI for one type and link quality for another. In this case
they should be separate elements. Remember that element variants are mutually exclusive
and only one type can be displayed at a time. So they shouldn't be used in cases where
the user would want to display different types at the same time - like in the above example
where the user might want to display both RSSI and link quality at the same time.
As variants are added to the firmware, support must also be included in the Configurator.
The following lists the variants implemented so far (please update this as variants are added):
OSD_ALTITUDE
type 1: Altitude with one decimal place
type 2: Altitude with no decimal (whole number only)
OSD_GPS_LON
OSD_GPS_LAT
type 1: Decimal representation with 7 digits
type 2: Decimal representation with 4 digits
type 3: Degrees, minutes, seconds
type 4: Open location code (Google Plus Code)
OSD_MAIN_BATT_USAGE
type 1: Graphical bar showing remaining battery (shrinks as used)
type 2: Graphical bar showing battery used (grows as used)
type 3: Numeric % of remaining battery
type 4: Numeric % or used battery
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
@ -122,7 +161,10 @@
#include "sensors/esc_sensor.h"
#include "sensors/sensors.h"
#include "common/maths.h"
#ifdef USE_GPS_PLUS_CODES
// located in lib/main/google/olc
#include "olc.h"
#endif
#define AH_SYMBOL_COUNT 9
#define AH_SIDEBAR_WIDTH_POS 7
@ -273,22 +315,71 @@ static void osdFormatAltitudeString(char * buff, int32_t altitudeCm, osdElementT
}
#ifdef USE_GPS
static void osdFormatCoordinate(char *buff, char sym, int32_t val)
static void osdFormatCoordinate(char *buff, gpsCoordinateType_e coordinateType, osdElementType_e variantType)
{
// latitude maximum integer width is 3 (-90).
// longitude maximum integer width is 4 (-180).
// We show 7 decimals, so we need to use 12 characters:
// eg: s-180.1234567z s=symbol, z=zero terminator, decimal separator between 0 and 1
int32_t gpsValue = 0;
const char leadingSymbol = (coordinateType == GPS_LONGITUDE) ? SYM_LON : SYM_LAT;
// NOTE: Don't use osdPrintFloat() for this. There are too many decimal places and float math doesn't have enough precision
int pos = 0;
buff[pos++] = sym;
if (val < 0) {
buff[pos++] = '-';
val = -val;
if (STATE(GPS_FIX_EVER)) { // don't display interim coordinates until we get the first position fix
gpsValue = (coordinateType == GPS_LONGITUDE) ? gpsSol.llh.lon : gpsSol.llh.lat;
}
const int degreesPart = ABS(gpsValue) / GPS_DEGREES_DIVIDER;
int fractionalPart = ABS(gpsValue) % GPS_DEGREES_DIVIDER;
switch (variantType) {
#ifdef USE_GPS_PLUS_CODES
#define PLUS_CODE_DIGITS 11
case OSD_ELEMENT_TYPE_4: // Open Location Code
{
*buff++ = SYM_SAT_L;
*buff++ = SYM_SAT_R;
if (STATE(GPS_FIX_EVER)) {
OLC_LatLon location;
location.lat = (double)gpsSol.llh.lat / GPS_DEGREES_DIVIDER;
location.lon = (double)gpsSol.llh.lon / GPS_DEGREES_DIVIDER;
OLC_Encode(&location, PLUS_CODE_DIGITS, buff, OSD_ELEMENT_BUFFER_LENGTH - 3);
} else {
memset(buff, SYM_HYPHEN, PLUS_CODE_DIGITS + 1);
buff[8] = '+';
buff[PLUS_CODE_DIGITS + 1] = '\0';
}
break;
}
#endif // USE_GPS_PLUS_CODES
case OSD_ELEMENT_TYPE_3: // degree, minutes, seconds style. ddd^mm'ss.00"W
{
char trailingSymbol;
*buff++ = leadingSymbol;
const int minutes = fractionalPart * 60 / GPS_DEGREES_DIVIDER;
const int fractionalMinutes = fractionalPart * 60 % GPS_DEGREES_DIVIDER;
const int seconds = fractionalMinutes * 60 / GPS_DEGREES_DIVIDER;
const int tenthSeconds = (fractionalMinutes * 60 % GPS_DEGREES_DIVIDER) * 10 / GPS_DEGREES_DIVIDER;
if (coordinateType == GPS_LONGITUDE) {
trailingSymbol = (gpsValue < 0) ? 'W' : 'E';
} else {
trailingSymbol = (gpsValue < 0) ? 'S' : 'N';
}
tfp_sprintf(buff, "%u%c%02u%c%02u.%u%c%c", degreesPart, SYM_GPS_DEGREE, minutes, SYM_GPS_MINUTE, seconds, tenthSeconds, SYM_GPS_SECOND, trailingSymbol);
break;
}
case OSD_ELEMENT_TYPE_2:
fractionalPart /= 1000;
FALLTHROUGH;
case OSD_ELEMENT_TYPE_1:
default:
*buff++ = leadingSymbol;
if (gpsValue < 0) {
*buff++ = SYM_HYPHEN;
}
tfp_sprintf(buff, (variantType == OSD_ELEMENT_TYPE_1 ? "%u.%07u" : "%u.%04u"), degreesPart, fractionalPart);
break;
}
tfp_sprintf(buff + pos, "%d.%07d", val / GPS_DEGREES_DIVIDER, val % GPS_DEGREES_DIVIDER);
}
#endif // USE_GPS
@ -953,14 +1044,15 @@ static void osdElementGpsHomeDistance(osdElementParms_t *element)
}
}
static void osdElementGpsLatitude(osdElementParms_t *element)
static void osdElementGpsCoordinate(osdElementParms_t *element)
{
osdFormatCoordinate(element->buff, SYM_LAT, gpsSol.llh.lat);
}
static void osdElementGpsLongitude(osdElementParms_t *element)
{
osdFormatCoordinate(element->buff, SYM_LON, gpsSol.llh.lon);
const gpsCoordinateType_e coordinateType = (element->item == OSD_GPS_LON) ? GPS_LONGITUDE : GPS_LATITUDE;
osdFormatCoordinate(element->buff, coordinateType, element->type);
if (STATE(GPS_FIX_EVER) && !STATE(GPS_FIX)) {
SET_BLINK(element->item); // blink if we had a fix but have since lost it
} else {
CLR_BLINK(element->item);
}
}
static void osdElementGpsSats(osdElementParms_t *element)
@ -1036,6 +1128,18 @@ static void osdElementLinkQuality(osdElementParms_t *element)
}
#endif // USE_RX_LINK_QUALITY_INFO
#ifdef USE_RX_LINK_UPLINK_POWER
static void osdElementTxUplinkPower(osdElementParms_t *element)
{
const uint16_t osdUplinkTxPowerMw = rxGetUplinkTxPwrMw();
if (osdUplinkTxPowerMw < 1000) {
tfp_sprintf(element->buff, "%c%3dMW", SYM_RSSI, osdUplinkTxPowerMw);
} else {
osdPrintFloat(element->buff, SYM_RSSI, osdUplinkTxPowerMw / 1000.0f, "", 1, false, 'W');
}
}
#endif // USE_RX_LINK_UPLINK_POWER
#ifdef USE_BLACKBOX
static void osdElementLogStatus(osdElementParms_t *element)
{
@ -1422,6 +1526,9 @@ static const uint8_t osdElementDisplayOrder[] = {
#ifdef USE_RX_LINK_QUALITY_INFO
OSD_LINK_QUALITY,
#endif
#ifdef USE_RX_LINK_UPLINK_POWER
OSD_TX_UPLINK_POWER,
#endif
#ifdef USE_RX_RSSI_DBM
OSD_RSSI_DBM_VALUE,
#endif
@ -1478,8 +1585,8 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
[OSD_WARNINGS] = osdElementWarnings,
[OSD_AVG_CELL_VOLTAGE] = osdElementAverageCellVoltage,
#ifdef USE_GPS
[OSD_GPS_LON] = osdElementGpsLongitude,
[OSD_GPS_LAT] = osdElementGpsLatitude,
[OSD_GPS_LON] = osdElementGpsCoordinate,
[OSD_GPS_LAT] = osdElementGpsCoordinate,
#endif
[OSD_DEBUG] = osdElementDebug,
#ifdef USE_ACC
@ -1527,6 +1634,9 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
#ifdef USE_RX_LINK_QUALITY_INFO
[OSD_LINK_QUALITY] = osdElementLinkQuality,
#endif
#ifdef USE_RX_LINK_UPLINK_POWER
[OSD_TX_UPLINK_POWER] = osdElementTxUplinkPower,
#endif
#ifdef USE_GPS
[OSD_FLIGHT_DIST] = osdElementGpsFlightDistance,
#endif

4
src/main/pg/rx.c
View file

@ -66,9 +66,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.rc_smoothing_input_cutoff = 0, // automatically calculate the cutoff by default
.rc_smoothing_derivative_cutoff = 0, // automatically calculate the cutoff by default
.rc_smoothing_debug_axis = ROLL, // default to debug logging for the roll axis
.rc_smoothing_input_type = RC_SMOOTHING_INPUT_BIQUAD,
.rc_smoothing_derivative_type = RC_SMOOTHING_DERIVATIVE_AUTO, // automatically choose type based on feedforward method
.rc_smoothing_auto_factor = 10,
.rc_smoothing_auto_factor = 30,
.srxl2_unit_id = 1,
.srxl2_baud_fast = true,
.sbus_baud_fast = false,

10
src/main/pg/sdcard.c
View file

@ -45,7 +45,17 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config)
// On generic targets, SPI has precedence over SDIO; SDIO must be post-flash configured.
config->useDma = false;
config->device = SPI_DEV_TO_CFG(SPIINVALID);
#ifdef CONFIG_IN_SDCARD
// CONFIG_ID_SDDCARD requires a default mode.
#if defined(USE_SDCARD_SDIO)
config->mode = SDCARD_MODE_SDIO;
#elif defined(USE_SDCARD_SPI)
config->mode = SDCARD_MODE_SPI;
#endif
#else
config->mode = SDCARD_MODE_NONE;
#endif
#ifdef USE_SDCARD_SPI
// These settings do not work for Unified Targets

17
src/main/rx/crsf.c
View file

@ -70,6 +70,12 @@ static uint8_t telemetryBufLen = 0;
static timeUs_t lastRcFrameTimeUs = 0;
#ifdef USE_RX_LINK_UPLINK_POWER
#define CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT 8
// Uplink power levels by uplinkTXPower expressed in mW (250 mW is from ver >=4.00)
const uint16_t uplinkTXPowerStatesMw[CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250};
#endif
/*
* CRSF protocol
*
@ -130,7 +136,7 @@ typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
* int8_t Uplink SNR ( db )
* uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
* uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz)
* uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
* uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW, 250mW )
* uint8_t Downlink RSSI ( dBm * -1 )
* uint8_t Downlink package success rate / Link quality ( % )
* int8_t Downlink SNR ( db )
@ -175,6 +181,11 @@ static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr,
}
#endif
#ifdef USE_RX_LINK_UPLINK_POWER
const uint8_t crsfUplinkPowerStatesItemIndex = (stats.uplink_TX_Power < CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT) ? stats.uplink_TX_Power : 0;
rxSetUplinkTxPwrMw(uplinkTXPowerStatesMw[crsfUplinkPowerStatesItemIndex]);
#endif
switch (debugMode) {
case DEBUG_CRSF_LINK_STATISTICS_UPLINK:
DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 0, stats.uplink_RSSI_1);
@ -345,7 +356,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
return RX_FRAME_PENDING;
}
STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
STATIC_UNIT_TESTED float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
UNUSED(rxRuntimeState);
/* conversion from RC value to PWM
@ -356,7 +367,7 @@ STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState
* scale factor = (2012-988) / (1811-172) = 0.62477120195241
* offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
*/
return (0.62477120195241f * crsfChannelData[chan]) + 881;
return (0.62477120195241f * (float)crsfChannelData[chan]) + 881;
}
void crsfRxWriteTelemetryData(const void *data, int len)

4
src/main/rx/ghst.c
View file

@ -289,7 +289,7 @@ static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
return true;
}
STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
STATIC_UNIT_TESTED float ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
UNUSED(rxRuntimeState);
@ -302,7 +302,7 @@ STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState
// max 1024 1811 2012us
//
return (5 * (ghstChannelData[chan]+1) / 8) + 880;
return (5 * ((float)ghstChannelData[chan] + 1) / 8) + 880;
}
static timeUs_t ghstFrameTimeUs(void)

2
src/main/rx/ibus.c
View file

@ -196,7 +196,7 @@ static uint8_t ibusFrameStatus(rxRuntimeState_t *rxRuntimeState)
}
static uint16_t ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
static float ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
UNUSED(rxRuntimeState);
return ibusChannelData[chan];

2
src/main/rx/jetiexbus.c
View file

@ -237,7 +237,7 @@ static uint8_t jetiExBusFrameStatus(rxRuntimeState_t *rxRuntimeState)
return frameStatus;
}
static uint16_t jetiExBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
static float jetiExBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
if (chan >= rxRuntimeState->channelCount)
return 0;

2
src/main/rx/msp.c
View file

@ -36,7 +36,7 @@
static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
static bool rxMspFrameDone = false;
uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
float rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
UNUSED(rxRuntimeState);
return mspFrame[chan];

2
src/main/rx/msp.h
View file

@ -22,6 +22,6 @@
struct rxConfig_s;
struct rxRuntimeState_s;
uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
float rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
void rxMspInit(const struct rxConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState);
void rxMspFrameReceive(uint16_t *frame, int channelCount);

4
src/main/rx/pwm.c
View file

@ -41,13 +41,13 @@
#include "rx/rx.h"
#include "rx/pwm.h"
static uint16_t pwmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
static float pwmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
{
UNUSED(rxRuntimeState);
return pwmRead(channel);
}
static uint16_t ppmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
static float ppmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
{
UNUSED(rxRuntimeState);
return ppmRead(channel);

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

@ -85,6 +85,10 @@ static uint16_t linkQuality = 0;
static uint8_t rfMode = 0;
#endif
#ifdef USE_RX_LINK_UPLINK_POWER
static uint16_t uplinkTxPwrMw = 0; //Uplink Tx power in mW
#endif
#define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec
#define RSSI_ADC_DIVISOR (4096 / 1024)
@ -107,8 +111,8 @@ static uint32_t needRxSignalMaxDelayUs;
static uint32_t suspendRxSignalUntil = 0;
static uint8_t skipRxSamples = 0;
static int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define MAX_INVALID_PULS_TIME 300
@ -154,7 +158,7 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRange
}
}
static uint16_t nullReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
static float nullReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
{
UNUSED(rxRuntimeState);
UNUSED(channel);
@ -455,6 +459,13 @@ void setLinkQualityDirect(uint16_t linkqualityValue)
#endif
}
#ifdef USE_RX_LINK_UPLINK_POWER
void rxSetUplinkTxPwrMw(uint16_t uplinkTxPwrMwValue)
{
uplinkTxPwrMw = uplinkTxPwrMwValue;
}
#endif
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{
bool signalReceived = false;
@ -584,15 +595,15 @@ static uint16_t getRxfailValue(uint8_t channel)
}
}
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range)
STATIC_UNIT_TESTED float applyRxChannelRangeConfiguraton(float sample, const rxChannelRangeConfig_t *range)
{
// Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
if (sample == PPM_RCVR_TIMEOUT) {
return PPM_RCVR_TIMEOUT;
}
sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
sample = constrain(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
sample = scaleRangef(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
return sample;
}
@ -604,7 +615,7 @@ static void readRxChannelsApplyRanges(void)
const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
// sample the channel
uint16_t sample;
float sample;
#if defined(USE_RX_MSP_OVERRIDE)
if (rxConfig()->msp_override_channels_mask) {
sample = rxMspOverrideReadRawRc(&rxRuntimeState, rxConfig(), rawChannel);
@ -634,7 +645,7 @@ static void detectAndApplySignalLossBehaviour(void)
rxFlightChannelsValid = true;
for (int channel = 0; channel < rxChannelCount; channel++) {
uint16_t sample = rcRaw[channel];
float sample = rcRaw[channel];
const bool validPulse = useValueFromRx && isPulseValid(sample);
@ -881,6 +892,13 @@ uint16_t rxGetLinkQualityPercent(void)
}
#endif
#ifdef USE_RX_LINK_UPLINK_POWER
uint16_t rxGetUplinkTxPwrMw(void)
{
return uplinkTxPwrMw;
}
#endif
uint16_t rxGetRefreshRate(void)
{
return rxRuntimeState.rxRefreshRate;

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

@ -86,7 +86,7 @@ typedef enum {
extern const char rcChannelLetters[];
extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
extern float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
#define RSSI_SCALE_MIN 1
#define RSSI_SCALE_MAX 255
@ -124,7 +124,7 @@ typedef struct rxChannelRangeConfig_s {
PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs);
struct rxRuntimeState_s;
typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeState, uint8_t chan); // used by receiver driver to return channel data
typedef float (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeState, uint8_t chan); // used by receiver driver to return channel data
typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeState_s *rxRuntimeState);
typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeState_s *rxRuntimeState);
typedef timeUs_t rcGetFrameTimeUsFn(void); // used to retrieve the timestamp in microseconds for the last channel data frame
@ -206,6 +206,9 @@ void setRssiDbmDirect(int16_t newRssiDbm, rssiSource_e source);
void rxSetRfMode(uint8_t rfModeValue);
uint8_t rxGetRfMode(void);
void rxSetUplinkTxPwrMw(uint16_t uplinkTxPwrMwValue);
uint16_t rxGetUplinkTxPwrMw(void);
void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig);
void suspendRxPwmPpmSignal(void);

2
src/main/rx/rx_spi.c
View file

@ -67,7 +67,7 @@ static protocolDataReceivedFnPtr protocolDataReceived;
static protocolProcessFrameFnPtr protocolProcessFrame;
static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
STATIC_UNIT_TESTED float rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
{
STATIC_ASSERT(NRF24L01_MAX_PAYLOAD_SIZE <= RX_SPI_MAX_PAYLOAD_SIZE, NRF24L01_MAX_PAYLOAD_SIZE_larger_than_RX_SPI_MAX_PAYLOAD_SIZE);

4
src/main/rx/sbus_channels.c
View file

@ -87,11 +87,11 @@ uint8_t sbusChannelsDecode(rxRuntimeState_t *rxRuntimeState, const sbusChannels_
return RX_FRAME_COMPLETE;
}
static uint16_t sbusChannelsReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
static float sbusChannelsReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
// Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
return (5 * rxRuntimeState->channelData[chan] / 8) + 880;
return (5 * (float)rxRuntimeState->channelData[chan] / 8) + 880;
}
void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)

13
src/main/rx/spektrum.c
View file

@ -174,18 +174,19 @@ static uint8_t spektrumFrameStatus(rxRuntimeState_t *rxRuntimeState)
return result;
}
static uint16_t spektrumReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
static float spektrumReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
uint16_t data;
float data;
if (chan >= rxRuntimeState->channelCount) {
return 0;
}
if (spekHiRes)
data = 988 + (spekChannelData[chan] >> 1); // 2048 mode
else
data = 988 + spekChannelData[chan]; // 1024 mode
if (spekHiRes) {
data = 0.5f * (float)spekChannelData[chan] + 988; // 2048 mode
} else {
data = spekChannelData[chan] + 988; // 1024 mode
}
return data;
}

6
src/main/rx/srxl2.c
View file

@ -55,7 +55,7 @@
#define SRXL2_MAX_CHANNELS 32
#define SRXL2_FRAME_PERIOD_US 11000 // 5500 for DSMR
#define SRXL2_CHANNEL_SHIFT 5
#define SRXL2_CHANNEL_SHIFT 2
#define SRXL2_CHANNEL_CENTER 0x8000
#define SRXL2_PORT_BAUDRATE_DEFAULT 115200
@ -458,13 +458,13 @@ static bool srxl2ProcessFrame(const rxRuntimeState_t *rxRuntimeState)
return true;
}
static uint16_t srxl2ReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channelIdx)
static float srxl2ReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channelIdx)
{
if (channelIdx >= rxRuntimeState->channelCount) {
return 0;
}
return SPEKTRUM_PULSE_OFFSET + ((rxRuntimeState->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) >> 1);
return ((float)(rxRuntimeState->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) / 16) + SPEKTRUM_PULSE_OFFSET;
}
void srxl2RxWriteData(const void *data, int len)

4
src/main/rx/sumd.c
View file

@ -162,10 +162,10 @@ static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
return frameStatus;
}
static uint16_t sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
static float sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
UNUSED(rxRuntimeState);
return sumdChannels[chan] / 8;
return (float)sumdChannels[chan] / 8;
}
static timeUs_t sumdFrameTimeUsFn(void)

2
src/main/rx/sumh.c
View file

@ -108,7 +108,7 @@ static uint8_t sumhFrameStatus(rxRuntimeState_t *rxRuntimeState)
return RX_FRAME_COMPLETE;
}
static uint16_t sumhReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
static float sumhReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
UNUSED(rxRuntimeState);

2
src/main/rx/xbus.c
View file

@ -248,7 +248,7 @@ static uint8_t xBusFrameStatus(rxRuntimeState_t *rxRuntimeState)
return RX_FRAME_COMPLETE;
}
static uint16_t xBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
static float xBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
uint16_t data;

2
src/main/sensors/current.c
View file

@ -191,7 +191,7 @@ void currentMeterVirtualRefresh(int32_t lastUpdateAt, bool armed, bool throttleL
throttleOffset = 0;
}
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50, 50hz?
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50. Possibly use thrustLinearization if configured.
currentMeterVirtualState.amperage += throttleFactor * (int32_t)currentSensorVirtualConfig()->scale / 1000;
}
updateCurrentmAhDrawnState(&currentMeterVirtualState.mahDrawnState, currentMeterVirtualState.amperage, lastUpdateAt);

6
src/main/sensors/current.h
View file

@ -82,12 +82,12 @@ PG_DECLARE(currentSensorADCConfig_t, currentSensorADCConfig);
typedef struct currentMeterVirtualState_s {
currentMeterMAhDrawnState_t mahDrawnState;
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
int32_t amperage; // current read by current sensor in centiamperes (1/100th A)
} currentSensorVirtualState_t;
typedef struct currentSensorVirtualConfig_s {
int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
uint16_t offset; // offset of the current sensor in millivolt steps
int16_t scale; // scale the throttle to centiamperes, using a hardcoded thrust linearization function (see Battery.md)
uint16_t offset; // offset of the current sensor in centiamperes (1/100th A)
} currentSensorVirtualConfig_t;
PG_DECLARE(currentSensorVirtualConfig_t, currentSensorVirtualConfig);

2
src/main/sensors/gyro_init.c
View file

@ -505,7 +505,7 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t
{
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO)
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605)
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);

2
src/main/target/CRAZYFLIE2/serialrx.c
View file

@ -201,7 +201,7 @@ static uint8_t frameStatus(rxRuntimeState_t *rxRuntimeState)
return RX_FRAME_COMPLETE;
}
static uint16_t readRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
static float readRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
if (chan >= rxRuntimeState->channelCount) {
return 0;

0
src/main/target/STM32_UNIFIED/STM32G47X.mk Normal file
View file

44
src/main/target/STM32_UNIFIED/target.h
View file

@ -94,7 +94,7 @@
#define USE_UART5
#define USE_UART6
#define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 6)
#define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 6)
#define USE_SPI_DEVICE_1
#define USE_SPI_DEVICE_2
@ -139,6 +139,42 @@
#define TARGET_IO_PORTD 0xffff
#define TARGET_IO_PORTE 0xffff
#define TARGET_IO_PORTF 0xffff
#elif defined(STM32G47X)
#define TARGET_BOARD_IDENTIFIER "SG47"
#define USBD_PRODUCT_STRING "Betaflight STM32G47x"
#define USE_I2C_DEVICE_1
#define USE_I2C_DEVICE_2
#define USE_I2C_DEVICE_3
#define USE_I2C_DEVICE_4
#define USE_UART1
#define USE_UART2
#define USE_UART3
#define USE_UART4
#define USE_UART5
#define USE_UART9
#define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 6)
#define USE_SPI_DEVICE_1
#define USE_SPI_DEVICE_2
#define USE_SPI_DEVICE_3
#define USE_SPI_DEVICE_4
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define TARGET_IO_PORTE 0xffff
#define TARGET_IO_PORTF 0xffff
#elif !defined(UNIT_TEST)
#error "No resources defined for this Unified Target."
#endif
// Treat the target as unified, and expect manufacturer id / board name
@ -190,9 +226,12 @@
#define USE_BARO_DPS310
#define USE_BARO_SPI_DPS310
#if !defined(STM32G4)
// G4 support needs fixing
#define USE_SDCARD
#define USE_SDCARD_SPI
#define USE_SDCARD_SDIO
#endif
#define USE_FLASHFS
#define USE_FLASH_TOOLS
@ -207,11 +246,14 @@
#define USE_VTX_RTC6705
#define USE_VTX_RTC6705_SOFTSPI
#if !defined(STM32G4)
// To make it fit RAM
#define USE_TRANSPONDER
#define USE_RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define USE_RANGEFINDER_TF
#endif
#define USE_SPI
#define SPI_FULL_RECONFIGURABILITY

10
src/main/target/STM32_UNIFIED/target.mk
View file

@ -9,12 +9,17 @@ else
ifeq ($(TARGET), STM32F7X2)
F7X2RE_TARGETS += $(TARGET)
else
ifeq ($(TARGET), STM32G47X)
G47X_TARGETS += $(TARGET)
else # STM32F745
F7X5XG_TARGETS += $(TARGET)
endif
endif
endif
endif
ifeq ($(TARGET), $(filter $(TARGET), STM32F405 STM32F745))
# Use a full block (16 kB) of flash for custom defaults - with 1 MB flash we have more than we know how to use anyway
@ -22,8 +27,11 @@ ifeq ($(TARGET), $(filter $(TARGET), STM32F405 STM32F745))
CUSTOM_DEFAULTS_EXTENDED = yes
endif
ifeq ($(TARGET), STM32G47X)
FEATURES += VCP ONBOARDFLASH
else
FEATURES += VCP SDCARD_SPI SDCARD_SDIO ONBOARDFLASH
endif
TARGET_SRC = \
$(addprefix drivers/accgyro/,$(notdir $(wildcard $(SRC_DIR)/drivers/accgyro/*.c))) \

13
src/main/target/common_post.h
View file

@ -90,7 +90,6 @@
#endif
#if !defined(USE_TELEMETRY)
#undef USE_CRSF_CMS_TELEMETRY
#undef USE_TELEMETRY_CRSF
#undef USE_TELEMETRY_GHST
#undef USE_TELEMETRY_FRSKY_HUB
@ -136,10 +135,13 @@
#undef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
#undef USE_SPEKTRUM_VTX_CONTROL
#undef USE_SPEKTRUM_VTX_TELEMETRY
#undef USE_SPEKTRUM_CMS_TELEMETRY
#undef USE_TELEMETRY_SRXL
#endif
#if !defined(USE_CMS) || !defined(USE_TELEMETRY_SRXL)
#undef USE_SPEKTRUM_CMS_TELEMETRY
#endif
#if defined(USE_SERIALRX_SBUS) || defined(USE_SERIALRX_FPORT)
#define USE_SBUS_CHANNELS
#endif
@ -229,6 +231,7 @@
#undef USE_RX_LINK_QUALITY_INFO
#undef USE_OSD_PROFILES
#undef USE_OSD_STICK_OVERLAY
#undef USE_RX_LINK_UPLINK_POWER
#endif
#if defined(USE_GPS_RESCUE)
@ -240,7 +243,7 @@
#define USE_I2C_GYRO
#endif
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_L3GD20)
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605)
#define USE_SPI_GYRO
#endif
@ -406,3 +409,7 @@ extern uint8_t __config_end;
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
#define USE_RX_BIND
#endif
#ifndef USE_GPS
#undef USE_GPS_PLUS_CODES
#endif

2
src/main/target/common_pre.h
View file

@ -397,4 +397,6 @@
#define USE_BATTERY_VOLTAGE_SAG_COMPENSATION
#define USE_RX_MSP_OVERRIDE
#define USE_SIMPLIFIED_TUNING
#define USE_RX_LINK_UPLINK_POWER
#define USE_GPS_PLUS_CODES
#endif

21
src/main/telemetry/crsf.c
View file

@ -30,21 +30,19 @@
#include "build/build_config.h"
#include "build/version.h"
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "cms/cms.h"
#include "config/feature.h"
#include "config/config.h"
#include "common/crc.h"
#include "common/maths.h"
#include "common/printf.h"
#include "common/streambuf.h"
#include "common/utils.h"
#include "cms/cms.h"
#include "drivers/nvic.h"
#include "config/config.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
@ -55,6 +53,9 @@
#include "io/gps.h"
#include "io/serial.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "rx/crsf.h"
#include "rx/crsf_protocol.h"
@ -64,7 +65,7 @@
#include "telemetry/telemetry.h"
#include "telemetry/msp_shared.h"
#include "telemetry/crsf.h"
#include "crsf.h"
#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
@ -525,7 +526,11 @@ void initCrsfTelemetry(void)
}
#endif
crsfScheduleCount = (uint8_t)index;
}
#if defined(USE_CRSF_CMS_TELEMETRY)
crsfDisplayportRegister();
#endif
}
bool checkCrsfTelemetryState(void)
{

16
src/main/telemetry/frsky_hub.c
View file

@ -257,15 +257,15 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
static void sendLatLong(int32_t coord[2])
{
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(coord[LAT], &coordinate);
GPStoDDDMM_MMMM(coord[GPS_LATITUDE], &coordinate);
frSkyHubWriteFrame(ID_LATITUDE_BP, coordinate.dddmm);
frSkyHubWriteFrame(ID_LATITUDE_AP, coordinate.mmmm);
frSkyHubWriteFrame(ID_N_S, coord[LAT] < 0 ? 'S' : 'N');
frSkyHubWriteFrame(ID_N_S, coord[GPS_LATITUDE] < 0 ? 'S' : 'N');
GPStoDDDMM_MMMM(coord[LON], &coordinate);
GPStoDDDMM_MMMM(coord[GPS_LONGITUDE], &coordinate);
frSkyHubWriteFrame(ID_LONGITUDE_BP, coordinate.dddmm);
frSkyHubWriteFrame(ID_LONGITUDE_AP, coordinate.mmmm);
frSkyHubWriteFrame(ID_E_W, coord[LON] < 0 ? 'W' : 'E');
frSkyHubWriteFrame(ID_E_W, coord[GPS_LONGITUDE] < 0 ? 'W' : 'E');
}
#if defined(USE_GPS)
@ -316,8 +316,8 @@ static void sendFakeLatLong(void)
// Heading is only displayed on OpenTX if non-zero lat/long is also sent
int32_t coord[2] = {0,0};
coord[LAT] = ((0.01f * telemetryConfig()->gpsNoFixLatitude) * GPS_DEGREES_DIVIDER);
coord[LON] = ((0.01f * telemetryConfig()->gpsNoFixLongitude) * GPS_DEGREES_DIVIDER);
coord[GPS_LATITUDE] = ((0.01f * telemetryConfig()->gpsNoFixLatitude) * GPS_DEGREES_DIVIDER);
coord[GPS_LONGITUDE] = ((0.01f * telemetryConfig()->gpsNoFixLongitude) * GPS_DEGREES_DIVIDER);
sendLatLong(coord);
}
@ -330,8 +330,8 @@ static void sendGPSLatLong(void)
if (STATE(GPS_FIX) || gpsFixOccured == 1) {
// If we have ever had a fix, send the last known lat/long
gpsFixOccured = 1;
coord[LAT] = gpsSol.llh.lat;
coord[LON] = gpsSol.llh.lon;
coord[GPS_LATITUDE] = gpsSol.llh.lat;
coord[GPS_LONGITUDE] = gpsSol.llh.lon;
sendLatLong(coord);
} else {
// otherwise send fake lat/long in order to display compass value

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

@ -213,8 +213,8 @@ static void ltm_oframe(void)
{
ltm_initialise_packet('O');
#if defined(USE_GPS)
ltm_serialise_32(GPS_home[LAT]);
ltm_serialise_32(GPS_home[LON]);
ltm_serialise_32(GPS_home[GPS_LATITUDE]);
ltm_serialise_32(GPS_home[GPS_LONGITUDE]);
#else
ltm_serialise_32(0);
ltm_serialise_32(0);

4
src/main/telemetry/mavlink.c
View file

@ -367,9 +367,9 @@ void mavlinkSendPosition(void)
mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg,
// latitude Latitude (WGS84), expressed as * 1E7
GPS_home[LAT],
GPS_home[GPS_LATITUDE],
// longitude Longitude (WGS84), expressed as * 1E7
GPS_home[LON],
GPS_home[GPS_LONGITUDE],
// altitude Altitude(WGS84), expressed as * 1000
0);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);

48
src/main/telemetry/srxl.c
View file

@ -29,46 +29,44 @@
#include "build/version.h"
#include "cms/cms.h"
#include "io/displayport_srxl.h"
#include "common/crc.h"
#include "common/streambuf.h"
#include "common/utils.h"
#include "config/config.h"
#include "config/feature.h"
#include "io/gps.h"
#include "io/serial.h"
#include "drivers/dshot.h"
#include "drivers/vtx_common.h"
#include "config/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "io/displayport_srxl.h"
#include "io/gps.h"
#include "io/serial.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx_tramp.h"
#include "pg/rx.h"
#include "pg/motor.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "rx/srxl2.h"
#include "io/spektrum_vtx_control.h"
#include "rx/srxl2.h"
#include "sensors/battery.h"
#include "sensors/adcinternal.h"
#include "sensors/battery.h"
#include "sensors/esc_sensor.h"
#include "telemetry/telemetry.h"
#include "telemetry/srxl.h"
#include "drivers/vtx_common.h"
#include "drivers/dshot.h"
#include "io/vtx_tramp.h"
#include "io/vtx_smartaudio.h"
#include "srxl.h"
#define SRXL_ADDRESS_FIRST 0xA5
#define SRXL_ADDRESS_SECOND 0x80
@ -774,18 +772,24 @@ void initSrxlTelemetry(void)
{
// check if there is a serial port open for SRXL telemetry (ie opened by the SRXL RX)
// and feature is enabled, if so, set SRXL telemetry enabled
if (srxlRxIsActive()) {
srxlTelemetryEnabled = true;
srxl2 = false;
if (srxlRxIsActive()) {
srxlTelemetryEnabled = true;
srxl2 = false;
#if defined(USE_SERIALRX_SRXL2)
} else if (srxl2RxIsActive()) {
srxlTelemetryEnabled = true;
srxl2 = true;
} else if (srxl2RxIsActive()) {
srxlTelemetryEnabled = true;
srxl2 = true;
#endif
} else {
srxlTelemetryEnabled = false;
srxl2 = false;
}
#if defined(USE_SPEKTRUM_CMS_TELEMETRY)
if (srxlTelemetryEnabled) {
srxlDisplayportRegister();
}
#endif
} else {
srxlTelemetryEnabled = false;
srxl2 = false;
}
}
bool checkSrxlTelemetryState(void)

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

@ -59,7 +59,7 @@ extern "C" {
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint16_t averageSystemLoadPercent = 0;
uint8_t cliMode = 0;
uint8_t debugMode = 0;

2
src/test/unit/flight_failsafe_unittest.cc
View file

@ -543,7 +543,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
// STUBS
extern "C" {
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcCommand[4];
int16_t debug[DEBUG16_VALUE_COUNT];
bool isUsingSticksToArm = true;

2
src/test/unit/flight_imu_unittest.cc
View file

@ -204,7 +204,7 @@ TEST(FlightImuTest, TestSmallAngle)
extern "C" {
boxBitmask_t rcModeActivationMask;
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
gyro_t gyro;
acc_t acc;

2
src/test/unit/ledstrip_unittest.cc
View file

@ -299,7 +299,7 @@ uint8_t armingFlags = 0;
uint8_t stateFlags = 0;
uint16_t flightModeFlags = 0;
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
boxBitmask_t rcModeActivationMask;
gpsSolutionData_t gpsSol;

2
src/test/unit/link_quality_unittest.cc
View file

@ -69,7 +69,7 @@ extern "C" {
float rMat[3][3];
pidProfile_t *currentPidProfile;
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome;
int16_t GPS_directionToHome;

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

@ -73,7 +73,7 @@ extern "C" {
pidProfile_t *currentPidProfile;
int16_t debug[DEBUG16_VALUE_COUNT];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome;
int16_t GPS_directionToHome;

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

@ -640,7 +640,7 @@ uint8_t armingFlags = 0;
uint16_t flightModeFlags = 0;
int16_t heading;
uint8_t stateFlags = 0;
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
pidProfile_t *currentPidProfile;
rxRuntimeState_t rxRuntimeState;
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);

2
src/test/unit/rcdevice_unittest.cc
View file

@ -53,7 +53,7 @@ extern "C" {
#include "rx/rx.h"
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
extern rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
extern runcamDevice_t *camDevice;

10
src/test/unit/rx_crsf_unittest.cc
View file

@ -46,7 +46,7 @@ extern "C" {
void crsfDataReceive(uint16_t c);
uint8_t crsfFrameCRC(void);
uint8_t crsfFrameStatus(void);
uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
extern bool crsfFrameDone;
extern crsfFrame_t crsfFrame;
@ -239,10 +239,10 @@ TEST(CrossFireTest, TestCapturedData)
EXPECT_EQ(983, crsfChannelData[3]);
uint8_t crc = crsfFrameCRC();
EXPECT_EQ(crc, crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]);
EXPECT_EQ(999, crsfReadRawRC(NULL, 0));
EXPECT_EQ(1501, crsfReadRawRC(NULL, 1));
EXPECT_EQ(1492, crsfReadRawRC(NULL, 2));
EXPECT_EQ(1495, crsfReadRawRC(NULL, 3));
EXPECT_EQ(999, (uint16_t)crsfReadRawRC(NULL, 0));
EXPECT_EQ(1501, (uint16_t)crsfReadRawRC(NULL, 1));
EXPECT_EQ(1492, (uint16_t)crsfReadRawRC(NULL, 2));
EXPECT_EQ(1495, (uint16_t)crsfReadRawRC(NULL, 3));
++framePtr;
crsfFrame = *(const crsfFrame_t*)framePtr;

2
src/test/unit/rx_ranges_unittest.cc
View file

@ -44,7 +44,7 @@ boxBitmask_t rcModeActivationMask;
int16_t debug[DEBUG16_VALUE_COUNT];
uint8_t debugMode = 0;
extern uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range);
extern float applyRxChannelRangeConfiguraton(float sample, const rxChannelRangeConfig_t *range);
}
#define RANGE_CONFIGURATION(min, max) new (rxChannelRangeConfig_t) {min, max}

2
src/test/unit/vtx_unittest.cc
View file

@ -62,7 +62,7 @@ extern "C" {
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint16_t averageSystemLoadPercent = 0;
uint8_t cliMode = 0;
uint8_t debugMode = 0;