From bc729e7016921d32a13b5807e591ea271484c205 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 27 Dec 2021 21:13:33 +0000 Subject: [PATCH 01/60] Initial coding This adds two OSD elements handy for gliders: - Remaining glide time - Remaining glide distance --- src/main/drivers/osd_symbols.h | 2 ++ src/main/io/osd.c | 32 ++++++++++++++++++++++++++++++++ src/main/io/osd.h | 2 ++ 3 files changed, 36 insertions(+) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 229ba680a2..2c8b455e6d 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -161,6 +161,8 @@ #define SYM_HEADING_LINE 0xCD // 205 Heading Graphic #define SYM_MAX 0xCE // 206 MAX symbol #define SYM_PROFILE 0xCF // 207 Profile symbol +#define SYM_GLIDE_DIST 0xD0 // 208 Glide Distance +#define SYM_GLIDE_MINS 0xD1 // 209 Glide Minutes #define SYM_LOGO_START 0x101 // 257 to 280, INAV logo diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 724eceb16b..ed7646d6f5 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1078,6 +1078,16 @@ static inline int32_t osdGetAltitudeMsl(void) return getEstimatedActualPosition(Z)+GPS_home.alt; } +uint16_t osdGetRemainingGlideTime(void) { + int32_t value = getEstimatedActualVelocity(Z); + if (value < 0) { + value = osdGetAltitude() / abs(value); + } else { + value = 0; + } + return value; +} + static bool osdIsHeadingValid(void) { #ifdef USE_SECONDARY_IMU @@ -2262,6 +2272,28 @@ static bool osdDrawSingleElement(uint8_t item) buff[4] = '\0'; break; } + case OSD_GLIDE_TIME_REMAINING: + { + uint16_t glideSeconds = osdGetRemainingGlideTime(); + buff[0] = SYM_GLIDE_MINS; + if (glideSeconds > 0) { + tfp_sprintf(buff + 1, "%i", (int)round(glideSeconds / 60)); + } else { + buff[1] = '-'; + buff[2] = '-'; + buff[3] = '-'; + } + buff[4] = '\0'; + break; + } + case OSD_GLIDE_RANGE: + { + uint16_t glideSeconds = osdGetRemainingGlideTime(); + uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; + buff[0] = SYM_GLIDE_DIST; + osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); + break; + } #endif case OSD_ACTIVE_PROFILE: diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a690ca7f8a..8041d8a7cc 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -239,6 +239,8 @@ typedef enum { OSD_AIR_MAX_SPEED, OSD_ACTIVE_PROFILE, OSD_MISSION, + OSD_GLIDE_TIME_REMAINING, + OSD_GLIDE_RANGE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From 4b4e5c427630374660fd273495b9e7cd653a2c82 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 28 Dec 2021 15:09:45 +0000 Subject: [PATCH 02/60] Added filter to vario reading and added infinite check to range --- src/main/io/osd.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ed7646d6f5..8b704f4df9 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1079,13 +1079,19 @@ static inline int32_t osdGetAltitudeMsl(void) } uint16_t osdGetRemainingGlideTime(void) { - int32_t value = getEstimatedActualVelocity(Z); + float value = getEstimatedActualVelocity(Z); + static pt1Filter_t glideTimeFilterState; + const timeMs_t curTimeMs = millis(); + static timeMs_t glideTimeUpdatedMs; + value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); + glideTimeUpdatedMs = curTimeMs; + if (value < 0) { - value = osdGetAltitude() / abs(value); + value = osdGetAltitude() / abs((int)value); } else { value = 0; } - return value; + return (uint16_t)round(value); } static bool osdIsHeadingValid(void) @@ -2279,9 +2285,7 @@ static bool osdDrawSingleElement(uint8_t item) if (glideSeconds > 0) { tfp_sprintf(buff + 1, "%i", (int)round(glideSeconds / 60)); } else { - buff[1] = '-'; - buff[2] = '-'; - buff[3] = '-'; + tfp_sprintf(buff + 1, "%s", "INF"); } buff[4] = '\0'; break; @@ -2289,9 +2293,14 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_GLIDE_RANGE: { uint16_t glideSeconds = osdGetRemainingGlideTime(); - uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; buff[0] = SYM_GLIDE_DIST; - osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); + if (glideSeconds > 0) { + uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; + osdFormatDistanceStr(buff + 1, glideRangeCM); + } else { + tfp_sprintf(buff + 1, "%s", "INF"); + buff[4] = '\0'; + } break; } #endif From 3efdc152fdad1247992925efff983a5e83900359 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 24 Jan 2022 20:52:08 +0000 Subject: [PATCH 03/60] Made some changes after initial testing. More testing required. --- src/main/io/osd.c | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8b704f4df9..2557b396c7 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2283,9 +2283,16 @@ static bool osdDrawSingleElement(uint8_t item) uint16_t glideSeconds = osdGetRemainingGlideTime(); buff[0] = SYM_GLIDE_MINS; if (glideSeconds > 0) { - tfp_sprintf(buff + 1, "%i", (int)round(glideSeconds / 60)); + buff[2] = SYM_BLANK; + buff[3] = SYM_BLANK; + if (glideSeconds < 60) { + tfp_sprintf(buff + 1, "%c%d", SYM_ZERO_HALF_TRAILING_DOT, glideSeconds); + } else { + tfp_sprintf(buff + 1, "%d", (glideSeconds/60)); + //ui2a(glideSeconds / 60, 10, 0, buff + 1); + } } else { - tfp_sprintf(buff + 1, "%s", "INF"); + tfp_sprintf(buff + 1, "%s", "---"); } buff[4] = '\0'; break; @@ -2296,10 +2303,10 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_GLIDE_DIST; if (glideSeconds > 0) { uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; - osdFormatDistanceStr(buff + 1, glideRangeCM); + osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); } else { - tfp_sprintf(buff + 1, "%s", "INF"); - buff[4] = '\0'; + tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); + buff[5] = '\0'; } break; } From 115f65ae4709dcb55708cdcd935c4087be63a705 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 29 Mar 2022 19:19:45 +0100 Subject: [PATCH 04/60] Update osd.c Updated from master and added filter to try and smooth the result. Need to test. --- src/main/io/osd.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 233f51312e..fb00513262 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1088,9 +1088,10 @@ static inline int32_t osdGetAltitudeMsl(void) uint16_t osdGetRemainingGlideTime(void) { float value = getEstimatedActualVelocity(Z); - static pt1Filter_t glideTimeFilterState; + static pt1Filter_t velocityZFilterState, glideTimeFilterState; const timeMs_t curTimeMs = millis(); - static timeMs_t glideTimeUpdatedMs; + static timeMs_t velocityZTimeUpdatedMs, glideTimeUpdatedMs; + value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); glideTimeUpdatedMs = curTimeMs; @@ -1099,6 +1100,10 @@ uint16_t osdGetRemainingGlideTime(void) { } else { value = 0; } + + value = pt1FilterApply4(&velocityZFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - velocityZTimeUpdatedMs)); + velocityZTimeUpdatedMs = curTimeMs; + return (uint16_t)round(value); } From eaf2372ba4d39ad6933b9eef97ca83d98cc1d7dd Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 21 Apr 2022 17:28:32 +0100 Subject: [PATCH 05/60] Removed second filter --- src/main/io/osd.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fb00513262..064300a25d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1088,9 +1088,9 @@ static inline int32_t osdGetAltitudeMsl(void) uint16_t osdGetRemainingGlideTime(void) { float value = getEstimatedActualVelocity(Z); - static pt1Filter_t velocityZFilterState, glideTimeFilterState; + static pt1Filter_t glideTimeFilterState; const timeMs_t curTimeMs = millis(); - static timeMs_t velocityZTimeUpdatedMs, glideTimeUpdatedMs; + static timeMs_t glideTimeUpdatedMs; value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); glideTimeUpdatedMs = curTimeMs; @@ -1101,9 +1101,6 @@ uint16_t osdGetRemainingGlideTime(void) { value = 0; } - value = pt1FilterApply4(&velocityZFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - velocityZTimeUpdatedMs)); - velocityZTimeUpdatedMs = curTimeMs; - return (uint16_t)round(value); } From b6d64ceb928e8822d680fcbaa7462278c2f1443a Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 21 Apr 2022 20:04:37 +0100 Subject: [PATCH 06/60] Added climb efficiency --- src/main/common/constants.h | 1 + src/main/drivers/osd_symbols.h | 5 +++ src/main/io/osd.c | 60 ++++++++++++++++++++++++++++++++++ src/main/io/osd.h | 1 + 4 files changed, 67 insertions(+) diff --git a/src/main/common/constants.h b/src/main/common/constants.h index cef3834fe6..540046cfa3 100644 --- a/src/main/common/constants.h +++ b/src/main/common/constants.h @@ -22,4 +22,5 @@ #define FEET_PER_KILOFEET 1000 // Used for altitude #define METERS_PER_KILOMETER 1000 #define METERS_PER_MILE 1609.344 +#define METERS_PER_FOOT 3.28084 #define METERS_PER_NAUTICALMILE 1852.001 diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 9f37702be1..46543ee90f 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -167,6 +167,11 @@ #define SYM_GLIDE_DIST 0xD3 // 211 Glide Distance #define SYM_GLIDE_MINS 0xD4 // 212 Glide Minutes +#define SYM_MAH_V_FT_0 0xD5 // 213 mAh/v-ft left +#define SYM_MAH_V_FT_1 0xD6 // 214 mAh/v-ft right +#define SYM_MAH_V_M_0 0xD7 // 215 mAh/v-m left +#define SYM_MAH_V_M_1 0xD8 // 216 mAh/v-m right + #define SYM_LOGO_START 0x101 // 257 to 280, INAV logo #define SYM_LOGO_WIDTH 6 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1a01925a8c..f46101db36 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2322,6 +2322,66 @@ static bool osdDrawSingleElement(uint8_t item) buff[4] = '\0'; break; } + case OSD_CLIMB_EFFICIENCY: + { + // amperage is in centi amps, vertical speed is in cms/s. We want + // mah/dist only to show when vertical speed > 1m/s. + static pt1Filter_t eFilterState; + static timeUs_t efficiencyUpdated = 0; + int32_t value = 0; + bool moreThanAh = false; + timeUs_t currentTimeUs = micros(); + timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + if (getEstimatedActualVelocity(Z) > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / getEstimatedActualVelocity(Z)) / 0.0036f, 1, US2S(efficiencyTimeDelta)); + + efficiencyUpdated = currentTimeUs; + } else { + value = eFilterState.state; + } + } + bool efficiencyValid = (value > 0) && (getEstimatedActualVelocity(Z) > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + // mAh/foot + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_FOOT / 10, 1, 0, 2, 3); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_FT_0, SYM_MAH_V_FT_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_MI_0; + buff[4] = SYM_MAH_MI_1; + buff[5] = '\0'; + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // mAh/metre + moreThanAh = osdFormatCentiNumber(buff, value * 100, 1, 0, 2, 3); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_M_0, SYM_MAH_V_M_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } + break; + } + break; + } case OSD_GLIDE_TIME_REMAINING: { uint16_t glideSeconds = osdGetRemainingGlideTime(); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index ee59b7ff74..a64e82c82c 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -259,6 +259,7 @@ typedef enum { OSD_SWITCH_INDICATOR_3, OSD_GLIDE_TIME_REMAINING, OSD_GLIDE_RANGE, + OSD_CLIMB_EFFICIENCY, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From 1a82b4225cda825831f8f04c6481680c89e972d9 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 25 Apr 2022 07:39:20 +0100 Subject: [PATCH 07/60] Update osd.c Glide time and distance OK. More work needed on climb efficiency. --- src/main/io/osd.c | 46 ++++++++++++++++++++-------------------------- 1 file changed, 20 insertions(+), 26 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f46101db36..1a801a3fe6 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2324,21 +2324,21 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_CLIMB_EFFICIENCY: { - // amperage is in centi amps, vertical speed is in cms/s. We want - // mah/dist only to show when vertical speed > 1m/s. - static pt1Filter_t eFilterState; - static timeUs_t efficiencyUpdated = 0; + // amperage is in centi amps (10mA), vertical speed is in cms/s. We want + // Ah/dist only to show when vertical speed > 1m/s. + static pt1Filter_t veFilterState; + static timeUs_t vEfficiencyUpdated = 0; int32_t value = 0; - bool moreThanAh = false; timeUs_t currentTimeUs = micros(); - timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated); if (getEstimatedActualVelocity(Z) > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / getEstimatedActualVelocity(Z)) / 0.0036f, 1, US2S(efficiencyTimeDelta)); + if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + // Amps / m/s + value = pt1FilterApply4(&veFilterState, ((float)getAmperage() / 100.0f) / (getEstimatedActualVelocity(Z) / 0.01f), 1, US2S(vEfficiencyTimeDelta)); - efficiencyUpdated = currentTimeUs; + vEfficiencyUpdated = currentTimeUs; } else { - value = eFilterState.state; + value = veFilterState.state; } } bool efficiencyValid = (value > 0) && (getEstimatedActualVelocity(Z) > 100); @@ -2349,16 +2349,13 @@ static bool osdDrawSingleElement(uint8_t item) FALLTHROUGH; case OSD_UNIT_IMPERIAL: // mAh/foot - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_FOOT / 10, 1, 0, 2, 3); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_FT_0, SYM_MAH_V_FT_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } + osdFormatCentiNumber(buff, value * METERS_PER_FOOT, 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_FT_0, SYM_MAH_V_FT_1); + if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_MI_0; - buff[4] = SYM_MAH_MI_1; + buff[3] = SYM_MAH_V_FT_0; + buff[4] = SYM_MAH_V_FT_1; buff[5] = '\0'; } break; @@ -2366,16 +2363,13 @@ static bool osdDrawSingleElement(uint8_t item) FALLTHROUGH; case OSD_UNIT_METRIC: // mAh/metre - moreThanAh = osdFormatCentiNumber(buff, value * 100, 1, 0, 2, 3); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_M_0, SYM_MAH_V_M_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } + osdFormatCentiNumber(buff, value, 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_M_0, SYM_MAH_V_M_1); + if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; + buff[3] = SYM_MAH_V_M_0; + buff[4] = SYM_MAH_V_M_1; buff[5] = '\0'; } break; From 45417d9b65036dd289e0245ceb4b9047b6f8123c Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 26 Apr 2022 21:13:28 +0100 Subject: [PATCH 08/60] Corrected variable names --- src/main/drivers/osd_symbols.h | 8 ++++---- src/main/io/osd.c | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 46543ee90f..2710ea302b 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -167,10 +167,10 @@ #define SYM_GLIDE_DIST 0xD3 // 211 Glide Distance #define SYM_GLIDE_MINS 0xD4 // 212 Glide Minutes -#define SYM_MAH_V_FT_0 0xD5 // 213 mAh/v-ft left -#define SYM_MAH_V_FT_1 0xD6 // 214 mAh/v-ft right -#define SYM_MAH_V_M_0 0xD7 // 215 mAh/v-m left -#define SYM_MAH_V_M_1 0xD8 // 216 mAh/v-m right +#define SYM_AH_V_FT_0 0xD5 // 213 mAh/v-ft left +#define SYM_AH_V_FT_1 0xD6 // 214 mAh/v-ft right +#define SYM_AH_V_M_0 0xD7 // 215 mAh/v-m left +#define SYM_AH_V_M_1 0xD8 // 216 mAh/v-m right #define SYM_LOGO_START 0x101 // 257 to 280, INAV logo diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1a801a3fe6..4a6eb460f2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2350,12 +2350,12 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_IMPERIAL: // mAh/foot osdFormatCentiNumber(buff, value * METERS_PER_FOOT, 1, 2, 2, 3); - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_FT_0, SYM_MAH_V_FT_1); + tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_V_FT_0; - buff[4] = SYM_MAH_V_FT_1; + buff[3] = SYM_AH_V_FT_0; + buff[4] = SYM_AH_V_FT_1; buff[5] = '\0'; } break; @@ -2364,12 +2364,12 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC: // mAh/metre osdFormatCentiNumber(buff, value, 1, 2, 2, 3); - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_V_M_0, SYM_MAH_V_M_1); + tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_V_M_0; - buff[4] = SYM_MAH_V_M_1; + buff[3] = SYM_AH_V_M_0; + buff[4] = SYM_AH_V_M_1; buff[5] = '\0'; } break; From 634bcaf0640445e2ba788cc46f25706b31630402 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 30 Apr 2022 21:23:53 +0100 Subject: [PATCH 09/60] Changed maths --- src/main/io/osd.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4a6eb460f2..aae2edcb65 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2334,7 +2334,7 @@ static bool osdDrawSingleElement(uint8_t item) if (getEstimatedActualVelocity(Z) > 0) { if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { // Amps / m/s - value = pt1FilterApply4(&veFilterState, ((float)getAmperage() / 100.0f) / (getEstimatedActualVelocity(Z) / 0.01f), 1, US2S(vEfficiencyTimeDelta)); + value = pt1FilterApply4(&veFilterState, ((float)getAmperage() / 100.0f) / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); vEfficiencyUpdated = currentTimeUs; } else { @@ -2349,10 +2349,10 @@ static bool osdDrawSingleElement(uint8_t item) FALLTHROUGH; case OSD_UNIT_IMPERIAL: // mAh/foot - osdFormatCentiNumber(buff, value * METERS_PER_FOOT, 1, 2, 2, 3); - tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1); - - if (!efficiencyValid) { + if (efficiencyValid) { + osdFormatCentiNumber(buff, value * METERS_PER_FOOT, 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1); + } else { buff[0] = buff[1] = buff[2] = '-'; buff[3] = SYM_AH_V_FT_0; buff[4] = SYM_AH_V_FT_1; @@ -2363,10 +2363,10 @@ static bool osdDrawSingleElement(uint8_t item) FALLTHROUGH; case OSD_UNIT_METRIC: // mAh/metre - osdFormatCentiNumber(buff, value, 1, 2, 2, 3); - tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1); - - if (!efficiencyValid) { + if (efficiencyValid) { + osdFormatCentiNumber(buff, value, 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1); + } else { buff[0] = buff[1] = buff[2] = '-'; buff[3] = SYM_AH_V_M_0; buff[4] = SYM_AH_V_M_1; From e68be9c6530877f30e723d38f734a816ac4be1b0 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 4 May 2022 20:08:58 +0100 Subject: [PATCH 10/60] Multiply by 100 to for the centi-function --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index aae2edcb65..3ec7b429ef 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2350,7 +2350,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_IMPERIAL: // mAh/foot if (efficiencyValid) { - osdFormatCentiNumber(buff, value * METERS_PER_FOOT, 1, 2, 2, 3); + osdFormatCentiNumber(buff, (value * METERS_PER_FOOT) * 100, 1, 2, 2, 3); tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1); } else { buff[0] = buff[1] = buff[2] = '-'; @@ -2364,7 +2364,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC: // mAh/metre if (efficiencyValid) { - osdFormatCentiNumber(buff, value, 1, 2, 2, 3); + osdFormatCentiNumber(buff, value * 100, 1, 2, 2, 3); tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1); } else { buff[0] = buff[1] = buff[2] = '-'; From 0e7639741d807fdcb2f69806590cb8a9ea55ae9f Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 7 May 2022 22:08:59 +0100 Subject: [PATCH 11/60] Prep to merge with master --- src/main/drivers/osd_symbols.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 2710ea302b..e2a76193b9 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -164,13 +164,13 @@ #define SYM_SWITCH_INDICATOR_LOW 0xD0 // 208 Switch High #define SYM_SWITCH_INDICATOR_MID 0xD1 // 209 Switch Mid #define SYM_SWITCH_INDICATOR_HIGH 0xD2 // 210 Switch Low -#define SYM_GLIDE_DIST 0xD3 // 211 Glide Distance -#define SYM_GLIDE_MINS 0xD4 // 212 Glide Minutes +#define SYM_GLIDE_DIST 0xD4 // 212 Glide Distance +#define SYM_GLIDE_MINS 0xD5 // 213 Glide Minutes -#define SYM_AH_V_FT_0 0xD5 // 213 mAh/v-ft left -#define SYM_AH_V_FT_1 0xD6 // 214 mAh/v-ft right -#define SYM_AH_V_M_0 0xD7 // 215 mAh/v-m left -#define SYM_AH_V_M_1 0xD8 // 216 mAh/v-m right +#define SYM_AH_V_FT_0 0xD6 // 214 mAh/v-ft left +#define SYM_AH_V_FT_1 0xD7 // 215 mAh/v-ft right +#define SYM_AH_V_M_0 0xD8 // 216 mAh/v-m left +#define SYM_AH_V_M_1 0xD9 // 217 mAh/v-m right #define SYM_LOGO_START 0x101 // 257 to 280, INAV logo From 17f9aca6f6a20c94b00477bd83a1137894aed736 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 7 May 2022 22:25:56 +0100 Subject: [PATCH 12/60] Reduced calculations needed --- src/main/io/osd.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 36fafddf96..4240d92b20 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2338,8 +2338,8 @@ static bool osdDrawSingleElement(uint8_t item) timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated); if (getEstimatedActualVelocity(Z) > 0) { if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - // Amps / m/s - value = pt1FilterApply4(&veFilterState, ((float)getAmperage() / 100.0f) / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); + // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD + value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); vEfficiencyUpdated = currentTimeUs; } else { @@ -2355,7 +2355,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_IMPERIAL: // mAh/foot if (efficiencyValid) { - osdFormatCentiNumber(buff, (value * METERS_PER_FOOT) * 100, 1, 2, 2, 3); + osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3); tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1); } else { buff[0] = buff[1] = buff[2] = '-'; @@ -2369,7 +2369,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC: // mAh/metre if (efficiencyValid) { - osdFormatCentiNumber(buff, value * 100, 1, 2, 2, 3); + osdFormatCentiNumber(buff, value, 1, 2, 2, 3); tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1); } else { buff[0] = buff[1] = buff[2] = '-'; From b21f4a92bc76dc68faaac929ceb2862b79d67c97 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 10 May 2022 09:58:23 +0100 Subject: [PATCH 13/60] Updated glide time display --- src/main/io/osd.c | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4240d92b20..52121a9ab4 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2383,16 +2383,14 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_GLIDE_TIME_REMAINING: { - uint16_t glideSeconds = osdGetRemainingGlideTime(); + uint16_t glideTime = osdGetRemainingGlideTime(); buff[0] = SYM_GLIDE_MINS; - if (glideSeconds > 0) { - buff[2] = SYM_BLANK; - buff[3] = SYM_BLANK; - if (glideSeconds < 60) { - tfp_sprintf(buff + 1, "%c%d", SYM_ZERO_HALF_TRAILING_DOT, glideSeconds); + if (glideTime > 0) { + if (glideTime < 60) { + tfp_sprintf(buff + 1, "%c%02d", SYM_ZERO_HALF_TRAILING_DOT, glideTime); } else { - tfp_sprintf(buff + 1, "%d", (glideSeconds/60)); - //ui2a(glideSeconds / 60, 10, 0, buff + 1); + glideTime = floor(glideTime/60); + tfp_sprintf(buff + 1, "%3d", glideTime); } } else { tfp_sprintf(buff + 1, "%s", "---"); From 7edd62611ba0a370e25f6b794c797bfc519d8397 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 30 May 2022 19:57:56 +0100 Subject: [PATCH 14/60] Updated glide time display method Updated from master Updated glide time display method --- src/main/io/osd.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e7066e5845..6abcdcac12 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2386,16 +2386,19 @@ static bool osdDrawSingleElement(uint8_t item) uint16_t glideTime = osdGetRemainingGlideTime(); buff[0] = SYM_GLIDE_MINS; if (glideTime > 0) { - if (glideTime < 60) { - tfp_sprintf(buff + 1, "%c%02d", SYM_ZERO_HALF_TRAILING_DOT, glideTime); + // Maximum value we can show in minutes is 99 minutes and 59 seconds. It is extremely unlikely that glide + // time will be longer than 99 minutes. If it is, it will show 99:^^ + if (glideTime > (99 * 60) + 59) { + tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60)); + buff[4] = SYM_DIRECTION; + buff[5] = SYM_DIRECTION; } else { - glideTime = floor(glideTime/60); - tfp_sprintf(buff + 1, "%3d", glideTime); + tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); } } else { - tfp_sprintf(buff + 1, "%s", "---"); + tfp_sprintf(buff + 1, "%s", "--:--"); } - buff[4] = '\0'; + buff[6] = '\0'; break; } case OSD_GLIDE_RANGE: From 23dc73e78b24fe4b7da3b10a5513db3927c8e7d6 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 18 Jun 2022 12:18:06 +0100 Subject: [PATCH 15/60] first build --- src/main/fc/settings.yaml | 5 ++++ src/main/navigation/navigation.c | 6 ++++ src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fw_launch.c | 35 ++++++++++++++++++---- 4 files changed, 41 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4a5f740a33..b4bfe2e7bc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2824,6 +2824,11 @@ groups: field: fw.launch_climb_angle min: 5 max: 45 + - name: nav_fw_launch_manual_throttle + description: "Allow launch with manually controlled throttle. INAV only levels wings and controls climb angle during launch. Throttle is controlled directly by throttle stick movement. Launch timers start when throttle stick is raised above idle." + default_value: OFF + field: fw.launch_manual_throttle + type: bool - name: nav_fw_cruise_yaw_rate description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]" default_value: 20 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f5201ccf89..aa12913d19 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -201,6 +201,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees .launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg + .launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF .cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT, .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT, @@ -1750,6 +1751,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF const timeUs_t currentTimeUs = micros(); UNUSED(previousState); + // Continue immediately to launch in progress if manual launch throttle used + if (navConfig()->fw.launch_manual_throttle) { + return NAV_FSM_EVENT_SUCCESS; + } + if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) { enableFixedWingLaunchController(currentTimeUs); return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ac91fe79f7..eb368703d2 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -294,6 +294,7 @@ typedef struct navConfig_s { uint16_t launch_max_altitude; // cm, altitude where to consider launch ended uint8_t launch_climb_angle; // Target climb angle for launch (deg) uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] + bool launch_manual_throttle; // Allows launch with manual throttle control uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled bool allow_manual_thr_increase; bool useFwNavYawControl; diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 05ea4c6f0d..274a693cc5 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -416,13 +416,27 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { - rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); + uint16_t initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time; + + if (navConfig()->fw.launch_manual_throttle) { + // reset timers when throttle is low and abort launch regardless of launch settings + if (isThrottleLow()) { + fwLaunch.currentStateTimeUs = currentTimeUs; + initialTime = 0; + if (isRollPitchStickDeflected()) { + return FW_LAUNCH_EVENT_ABORT; + } + } + fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; + } else { + rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); + } if (isLaunchMaxAltitudeReached()) { return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state } - if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { + if (areSticksMoved(initialTime, currentTimeUs)) { return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_ABORTED state } @@ -442,9 +456,12 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state } else { - // make a smooth transition from the launch state to the current state for throttle and the pitch angle - const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]); + // Make a smooth transition from the launch state to the current state for pitch angle + // Do the same for throttle when manual launch throttle isn't used + if (!navConfig()->fw.launch_manual_throttle) { + const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]); + } fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); } @@ -498,7 +515,13 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) void resetFixedWingLaunchController(timeUs_t currentTimeUs) { - setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); + if (navConfig()->fw.launch_manual_throttle) { + // no detection or motor control required with manual launch throttle + // so start at launch in progress + setCurrentState(FW_LAUNCH_STATE_IN_PROGRESS, currentTimeUs); + } else { + setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); + } } void enableFixedWingLaunchController(timeUs_t currentTimeUs) From 8b743dbdd85b3f5a79dd486499aea1a521599a48 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 18 Jun 2022 12:20:58 +0100 Subject: [PATCH 16/60] update settings --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 5d43cfe98b..9198b1804a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3082,6 +3082,16 @@ Launch idle throttle - throttle to be set before launch sequence is initiated. I --- +### nav_fw_launch_manual_throttle + +Allows launch with manually controlled throttle. INAV only levels wings and controls climb angle during launch. Throttle is controlled directly by throttle stick movement. Launch timers start when throttle stick is raised above idle. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### nav_fw_launch_max_altitude Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b4bfe2e7bc..278fec521a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2825,7 +2825,7 @@ groups: min: 5 max: 45 - name: nav_fw_launch_manual_throttle - description: "Allow launch with manually controlled throttle. INAV only levels wings and controls climb angle during launch. Throttle is controlled directly by throttle stick movement. Launch timers start when throttle stick is raised above idle." + description: "Allows launch with manually controlled throttle. INAV only levels wings and controls climb angle during launch. Throttle is controlled directly by throttle stick movement. Launch timers start when throttle stick is raised above idle." default_value: OFF field: fw.launch_manual_throttle type: bool From 9c5b7bfa39b5e760f32d1f593200a29cf285f3e3 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 22 Jun 2022 15:57:44 +0100 Subject: [PATCH 17/60] Update + add flight detection --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_fw_launch.c | 21 +++++++++++++++------ 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 9198b1804a..ecec1c8eba 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3084,7 +3084,7 @@ Launch idle throttle - throttle to be set before launch sequence is initiated. I ### nav_fw_launch_manual_throttle -Allows launch with manually controlled throttle. INAV only levels wings and controls climb angle during launch. Throttle is controlled directly by throttle stick movement. Launch timers start when throttle stick is raised above idle. +Allow launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised). | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 278fec521a..018c550975 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2825,7 +2825,7 @@ groups: min: 5 max: 45 - name: nav_fw_launch_manual_throttle - description: "Allows launch with manually controlled throttle. INAV only levels wings and controls climb angle during launch. Throttle is controlled directly by throttle stick movement. Launch timers start when throttle stick is raised above idle." + description: "Allow launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised)." default_value: OFF field: fw.launch_manual_throttle type: bool diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 274a693cc5..cd4900c541 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -263,9 +263,15 @@ static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && isRollPitchStickDeflected(); } +static inline bool isProbablyNotFlying(void) +{ + // Check flight status but only if GPS lock valid + return posControl.flags.estPosStatus == EST_TRUSTED && !isFixedWingFlying(); +} + static void resetPidsIfNeeded(void) { - // Until motors are started don't use PID I-term and reset TPA filter - if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { + // Don't use PID I-term and reset TPA filter until motors are started or until flight is detected + if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { pidResetErrorAccumulators(); pidResetTPAFilter(); } @@ -416,19 +422,22 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { - uint16_t initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time; + uint16_t initialTime = 0; if (navConfig()->fw.launch_manual_throttle) { - // reset timers when throttle is low and abort launch regardless of launch settings + // reset timers when throttle is low or until flight detected and abort launch regardless of launch settings if (isThrottleLow()) { fwLaunch.currentStateTimeUs = currentTimeUs; - initialTime = 0; + fwLaunch.pitchAngle = 0; if (isRollPitchStickDeflected()) { return FW_LAUNCH_EVENT_ABORT; } + } else { + if (isProbablyNotFlying()) fwLaunch.currentStateTimeUs = currentTimeUs; + fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; } - fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; } else { + initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time; rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle); } From 0056cc0011ee0ddd49bfdc8ea77372a8f6597bf9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 23 Jun 2022 22:56:17 +0100 Subject: [PATCH 18/60] Update cms_menu_navigation.c --- src/main/cms/cms_menu_navigation.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 80f7bf09e1..af3944bd56 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -148,6 +148,7 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] = OSD_SETTING_ENTRY("VELOCITY", SETTING_NAV_FW_LAUNCH_VELOCITY), OSD_SETTING_ENTRY("ACCELERATION", SETTING_NAV_FW_LAUNCH_ACCEL), OSD_SETTING_ENTRY("DETECT TIME", SETTING_NAV_FW_LAUNCH_DETECT_TIME), + OSD_SETTING_ENTRY("MANUAL THROTTLE", SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE), OSD_BACK_AND_END_ENTRY, }; From b9f601ea32600a3892eecdb7d5cfac152cc8468d Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 27 Jun 2022 20:45:05 +0100 Subject: [PATCH 19/60] Update navigation_fw_launch.c --- src/main/navigation/navigation_fw_launch.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index cd4900c541..22af917d10 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -260,7 +260,7 @@ static inline bool isLaunchMaxAltitudeReached(void) static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { - return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && isRollPitchStickDeflected(); + return (initialTime + currentStateElapsedMs(currentTimeUs)) >= navConfig()->fw.launch_min_time && isRollPitchStickDeflected(); } static inline bool isProbablyNotFlying(void) From 0c5c78c20dc35297eace532461ccfdc0102f5b40 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 1 Jul 2022 10:39:01 +0100 Subject: [PATCH 20/60] update FW flying status for unpowered planes --- src/main/navigation/navigation_fixedwing.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f05e5fc0f4..2c721f4f00 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -600,7 +600,7 @@ bool isFixedWingFlying(void) #ifdef USE_PITOT airspeed = pitot.airSpeed; #endif - bool throttleCondition = rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle; + bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle; bool velCondition = posControl.actualState.velXY > 250 || airspeed > 250; bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING; From 6097769e6792e6f281e688ee2e96da4905e7853d Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 5 Jul 2022 13:20:29 +0100 Subject: [PATCH 21/60] Improve messages + fix pidReset --- src/main/io/osd.c | 5 +++-- src/main/io/osd.h | 1 + src/main/navigation/navigation.c | 3 ++- src/main/navigation/navigation_fw_launch.c | 6 ++++-- 4 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 5056ead414..3fe6064874 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1534,7 +1534,7 @@ void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { ptr++; } - + buff[ptr] = '\0'; } @@ -4338,7 +4338,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else { /* messages shown only when Failsafe, WP, RTH or Emergency Landing not active */ if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); + messages[messageCount++] = navConfig()->fw.launch_manual_throttle ? OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH_MANUAL) : + OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); const char *launchStateMessage = fixedWingLaunchStateMessage(); if (launchStateMessage) { messages[messageCount++] = launchStateMessage; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 53814003ba..8d78194572 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -105,6 +105,7 @@ #define OSD_MSG_LANDED "LANDED" #define OSD_MSG_PREPARING_LAND "PREPARING TO LAND" #define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH" +#define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" #define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" #define OSD_MSG_AUTOTRIM "(AUTOTRIM)" #define OSD_MSG_AUTOTUNE "(AUTOTUNE)" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index aa12913d19..57c6cf3dac 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3999,7 +3999,8 @@ bool navigationIsExecutingAnEmergencyLanding(void) bool navigationInAutomaticThrottleMode(void) { navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); - return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)); + return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAND)) || + ((stateFlags & NAV_CTL_LAUNCH) && !navConfig()->fw.launch_manual_throttle); } bool navigationIsControllingThrottle(void) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 22af917d10..7ac3e16b0b 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -271,7 +271,7 @@ static inline bool isProbablyNotFlying(void) static void resetPidsIfNeeded(void) { // Don't use PID I-term and reset TPA filter until motors are started or until flight is detected - if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { + if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && isThrottleLow())) { pidResetErrorAccumulators(); pidResetTPAFilter(); } @@ -433,7 +433,9 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t return FW_LAUNCH_EVENT_ABORT; } } else { - if (isProbablyNotFlying()) fwLaunch.currentStateTimeUs = currentTimeUs; + if (isProbablyNotFlying()) { + fwLaunch.currentStateTimeUs = currentTimeUs; + } fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; } } else { From cc527851721996c340fa5fbedb9e28fd210bd793 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 5 Jul 2022 13:42:29 +0100 Subject: [PATCH 22/60] fixes --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_fw_launch.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index d1d20e2d09..7766502bb7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3014,7 +3014,7 @@ Launch idle throttle - throttle to be set before launch sequence is initiated. I ### nav_fw_launch_manual_throttle -Allow launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised). +Allows launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised). | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bc3a7323ca..83d3c13106 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2819,7 +2819,7 @@ groups: min: 5 max: 45 - name: nav_fw_launch_manual_throttle - description: "Allow launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised)." + description: "Allows launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised)." default_value: OFF field: fw.launch_manual_throttle type: bool diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 46893e97cb..1224e53550 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -429,7 +429,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t if (isThrottleLow()) { fwLaunch.currentStateTimeUs = currentTimeUs; fwLaunch.pitchAngle = 0; - if (isRollPitchStickDeflected()) { + if (isRollPitchStickDeflected(LAUNCH_ABORT_STICK_DEADBAND)) { return FW_LAUNCH_EVENT_ABORT; } } else { From e1af6b342331beceaf3bc5d0484779fc6af8ed4a Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 9 Jul 2022 13:33:58 +0200 Subject: [PATCH 23/60] Remove barometer median filter --- src/main/fc/settings.yaml | 5 ---- src/main/sensors/barometer.c | 51 +----------------------------------- src/main/sensors/barometer.h | 1 - 3 files changed, 1 insertion(+), 56 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c382e50de..e195aaed8a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -651,11 +651,6 @@ groups: description: "Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info" default_value: "AUTO" table: baro_hardware - - name: baro_median_filter - description: "3-point median filtering for barometer readouts. No reason to change this setting" - default_value: ON - field: use_median_filtering - type: bool - name: baro_cal_tolerance description: "Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]." default_value: 150 diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 822ccec5cd..a99fc08cf5 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -60,11 +60,10 @@ baro_t baro; // barometer access functions #ifdef USE_BARO -PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 4); PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, .baro_hardware = SETTING_BARO_HARDWARE_DEFAULT, - .use_median_filtering = SETTING_BARO_MEDIAN_FILTER_DEFAULT, .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT ); @@ -248,51 +247,6 @@ bool baroInit(void) return true; } -#define PRESSURE_SAMPLES_MEDIAN 3 - -/* -altitude pressure - 0m 101325Pa - 100m 100129Pa delta = 1196 - 1000m 89875Pa - 1100m 88790Pa delta = 1085 -At sea level an altitude change of 100m results in a pressure change of 1196Pa, at 1000m pressure change is 1085Pa -So set glitch threshold at 1000 - this represents an altitude change of approximately 100m. -*/ -#define PRESSURE_DELTA_GLITCH_THRESHOLD 1000 -static int32_t applyBarometerMedianFilter(int32_t newPressureReading) -{ - static int32_t barometerFilterSamples[PRESSURE_SAMPLES_MEDIAN]; - static int currentFilterSampleIndex = 0; - static bool medianFilterReady = false; - - int nextSampleIndex = currentFilterSampleIndex + 1; - if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) { - nextSampleIndex = 0; - medianFilterReady = true; - } - int previousSampleIndex = currentFilterSampleIndex - 1; - if (previousSampleIndex < 0) { - previousSampleIndex = PRESSURE_SAMPLES_MEDIAN - 1; - } - const int previousPressureReading = barometerFilterSamples[previousSampleIndex]; - - if (medianFilterReady) { - if (ABS(previousPressureReading - newPressureReading) < PRESSURE_DELTA_GLITCH_THRESHOLD) { - barometerFilterSamples[currentFilterSampleIndex] = newPressureReading; - currentFilterSampleIndex = nextSampleIndex; - return quickMedianFilter3(barometerFilterSamples); - } else { - // glitch threshold exceeded, so just return previous reading and don't add the glitched reading to the filter array - return barometerFilterSamples[previousSampleIndex]; - } - } else { - barometerFilterSamples[currentFilterSampleIndex] = newPressureReading; - currentFilterSampleIndex = nextSampleIndex; - return newPressureReading; - } -} - typedef enum { BAROMETER_NEEDS_SAMPLES = 0, BAROMETER_NEEDS_CALCULATION @@ -323,9 +277,6 @@ uint32_t baroUpdate(void) baro.dev.start_ut(&baro.dev); } baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature); - if (barometerConfig()->use_median_filtering) { - baro.baroPressure = applyBarometerMedianFilter(baro.baroPressure); - } state = BAROMETER_NEEDS_SAMPLES; return baro.dev.ut_delay; break; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 7358c64ffe..c2722e7d3c 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -51,7 +51,6 @@ extern baro_t baro; typedef struct barometerConfig_s { uint8_t baro_hardware; // Barometer hardware to use - uint8_t use_median_filtering; // Use 3-point median filtering uint16_t baro_calibration_tolerance; // Baro calibration tolerance (cm at sea level) } barometerConfig_t; From 0209f0b93bab70e69c4ffec92381d93a3f295709 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 9 Jul 2022 13:40:16 +0200 Subject: [PATCH 24/60] Fix docs --- docs/Settings.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 8f61e19060..13b779d065 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -362,16 +362,6 @@ Selection of baro hardware. See Wiki Sensor auto detect and hardware failure det --- -### baro_median_filter - -3-point median filtering for barometer readouts. No reason to change this setting - -| Default | Min | Max | -| --- | --- | --- | -| ON | OFF | ON | - ---- - ### bat_cells Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected. From eb2382697514b6fe042355334489c4862943dba2 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 11 Jul 2022 14:30:57 +0200 Subject: [PATCH 25/60] Compute RC update frequency and allow links slower than 50Hz --- src/main/fc/fc_core.c | 2 +- src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_smoothing.c | 31 ++++++++++++++++++++++++++++++- src/main/fc/rc_smoothing.h | 2 +- src/main/rx/rx.c | 2 +- 5 files changed, 34 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 197378448f..bd099171ae 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -886,7 +886,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) annexCode(dT); if (rxConfig()->rcFilterFrequency) { - rcInterpolationApply(isRXDataNew); + rcInterpolationApply(isRXDataNew, currentTimeUs); } if (isRXDataNew) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 0f069665c2..26194938f8 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -459,7 +459,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .taskName = "RX", .checkFunc = taskUpdateRxCheck, .taskFunc = taskUpdateRxMain, - .desiredPeriod = TASK_PERIOD_HZ(50), // If event-based scheduling doesn't work, fallback to periodic scheduling + .desiredPeriod = TASK_PERIOD_HZ(10), // If event-based scheduling doesn't work, fallback to periodic scheduling .staticPriority = TASK_PRIORITY_HIGH, }, diff --git a/src/main/fc/rc_smoothing.c b/src/main/fc/rc_smoothing.c index acfa9c815c..f1d3b1b182 100644 --- a/src/main/fc/rc_smoothing.c +++ b/src/main/fc/rc_smoothing.c @@ -57,8 +57,37 @@ static void rcInterpolationInit(int rcFilterFreqency) } } -void rcInterpolationApply(bool isRXDataNew) +static int32_t applyRcUpdateFrequencyMedianFilter(int32_t newReading) { + #define RC_FILTER_SAMPLES_MEDIAN 9 + static int32_t filterSamples[RC_FILTER_SAMPLES_MEDIAN]; + static int filterSampleIndex = 0; + static bool medianFilterReady = false; + + filterSamples[filterSampleIndex] = newReading; + ++filterSampleIndex; + if (filterSampleIndex == RC_FILTER_SAMPLES_MEDIAN) { + filterSampleIndex = 0; + medianFilterReady = true; + } + + return medianFilterReady ? quickMedianFilter9(filterSamples) : newReading; +} + +void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs) +{ + // Compute the RC update frequency + static timeUs_t previousRcData; + + if (isRXDataNew) { + const timeDelta_t delta = cmpTimeUs(currentTimeUs, previousRcData); + const float rcUpdateFrequency = applyRcUpdateFrequencyMedianFilter(1.0f / (delta * 0.000001f)); + + DEBUG_SET(DEBUG_ALWAYS, 0, rcUpdateFrequency); + + previousRcData = currentTimeUs; + } + static bool initDone = false; static float initFilterFreqency = 0; diff --git a/src/main/fc/rc_smoothing.h b/src/main/fc/rc_smoothing.h index 4c1ad66bcb..7b286c0bb7 100644 --- a/src/main/fc/rc_smoothing.h +++ b/src/main/fc/rc_smoothing.h @@ -28,4 +28,4 @@ #include #include -void rcInterpolationApply(bool isRXDataNew); \ No newline at end of file +void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f081aed77f..2ca13cc2ac 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -451,7 +451,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } rxDataProcessingRequired = false; - rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ; + rxNextUpdateAtUs = currentTimeUs + DELAY_10_HZ; // only proceed when no more samples to skip and suspend period is over if (skipRxSamples) { From 979aa45c4bfd078da4d99897718202810b722b67 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 11 Jul 2022 14:42:10 +0200 Subject: [PATCH 26/60] Switch RC filtering to PT3 --- src/main/fc/rc_smoothing.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/fc/rc_smoothing.c b/src/main/fc/rc_smoothing.c index f1d3b1b182..7e9cd5a557 100644 --- a/src/main/fc/rc_smoothing.c +++ b/src/main/fc/rc_smoothing.c @@ -47,13 +47,13 @@ #include "flight/mixer.h" -static biquadFilter_t rcSmoothFilter[4]; +static pt3Filter_t rcSmoothFilter[4]; static float rcStickUnfiltered[4]; static void rcInterpolationInit(int rcFilterFreqency) { for (int stick = 0; stick < 4; stick++) { - biquadFilterInitLPF(&rcSmoothFilter[stick], rcFilterFreqency, getLooptime()); + pt3FilterInit(&rcSmoothFilter[stick], pt3FilterGain(rcFilterFreqency, getLooptime() * 1e-6f)); } } @@ -109,7 +109,7 @@ void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs) } for (int stick = 0; stick < 4; stick++) { - rcCommand[stick] = biquadFilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]); + rcCommand[stick] = pt3FilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]); } } @@ -134,6 +134,5 @@ void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs) - From bfcb2c4a0e4cce050163e0bee9695ca72b7ad0b3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 11 Jul 2022 16:31:03 +0200 Subject: [PATCH 27/60] Add option to set RC smoothing based on RC refresh rate --- docs/Settings.md | 24 ++++++++++++-- src/main/fc/rc_smoothing.c | 64 +++++++++++++++++++++++++------------- src/main/fc/settings.yaml | 15 +++++++-- src/main/rx/rx.c | 6 ++-- src/main/rx/rx.h | 2 ++ 5 files changed, 84 insertions(+), 27 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 8f61e19060..420c21f065 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5032,13 +5032,33 @@ Exposition value used for the PITCH/ROLL axes by all the stabilized flights mode --- -### rc_filter_frequency +### rc_filter_auto + +When enabled, INAV will set RC filtering based on refresh rate and smoothing factor. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### rc_filter_lpf_hz RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | Default | Min | Max | | --- | --- | --- | -| 50 | 0 | 100 | +| 50 | 15 | 250 | + +--- + +### rc_filter_smoothing_factor + +The RC filter smoothing factor. The higher the value, the more smoothing but also the more delay in response. Value 1 sets the filter at half the refresh rate. Value 100 sets the filter to aprox. 10% of the RC refresh rate + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 1 | 100 | --- diff --git a/src/main/fc/rc_smoothing.c b/src/main/fc/rc_smoothing.c index 7e9cd5a557..fba7178bf0 100644 --- a/src/main/fc/rc_smoothing.c +++ b/src/main/fc/rc_smoothing.c @@ -47,16 +47,12 @@ #include "flight/mixer.h" +// RC Interpolation is not allowed to go below this value. +#define RC_INTERPOLATION_MIN_FREQUENCY 15 + static pt3Filter_t rcSmoothFilter[4]; static float rcStickUnfiltered[4]; -static void rcInterpolationInit(int rcFilterFreqency) -{ - for (int stick = 0; stick < 4; stick++) { - pt3FilterInit(&rcSmoothFilter[stick], pt3FilterGain(rcFilterFreqency, getLooptime() * 1e-6f)); - } -} - static int32_t applyRcUpdateFrequencyMedianFilter(int32_t newReading) { #define RC_FILTER_SAMPLES_MEDIAN 9 @@ -78,23 +74,21 @@ void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs) { // Compute the RC update frequency static timeUs_t previousRcData; - - if (isRXDataNew) { - const timeDelta_t delta = cmpTimeUs(currentTimeUs, previousRcData); - const float rcUpdateFrequency = applyRcUpdateFrequencyMedianFilter(1.0f / (delta * 0.000001f)); - - DEBUG_SET(DEBUG_ALWAYS, 0, rcUpdateFrequency); - - previousRcData = currentTimeUs; - } - + static int filterFrequency; static bool initDone = false; - static float initFilterFreqency = 0; + + const float dT = getLooptime() * 1e-6f; if (isRXDataNew) { - if (!initDone || (initFilterFreqency != rxConfig()->rcFilterFrequency)) { - rcInterpolationInit(rxConfig()->rcFilterFrequency); - initFilterFreqency = rxConfig()->rcFilterFrequency; + if (!initDone) { + + filterFrequency = rxConfig()->rcFilterFrequency; + + // Initialize the RC smooth filter + for (int stick = 0; stick < 4; stick++) { + pt3FilterInit(&rcSmoothFilter[stick], pt3FilterGain(filterFrequency, dT)); + } + initDone = true; } @@ -108,6 +102,34 @@ void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs) return; } + if (isRXDataNew && rxConfig()->autoSmooth) { + const timeDelta_t delta = cmpTimeUs(currentTimeUs, previousRcData); + const int32_t rcUpdateFrequency = applyRcUpdateFrequencyMedianFilter(1.0f / (delta * 0.000001f)); + + const int nyquist = rcUpdateFrequency / 2; + + int newFilterFrequency = scaleRange( + rxConfig()->autoSmoothFactor, + 1, + 100, + nyquist, + rcUpdateFrequency / 10 + ); + + // Do not allow filter frequency to go below RC_INTERPOLATION_MIN_FREQUENCY or above nuyquist frequency. + newFilterFrequency = constrain(newFilterFrequency, RC_INTERPOLATION_MIN_FREQUENCY, nyquist); + + if (newFilterFrequency != filterFrequency) { + + for (int stick = 0; stick < 4; stick++) { + pt3FilterUpdateCutoff(&rcSmoothFilter[stick], pt3FilterGain(newFilterFrequency, dT)); + } + filterFrequency = newFilterFrequency; + } + + previousRcData = currentTimeUs; + } + for (int stick = 0; stick < 4; stick++) { rcCommand[stick] = pt3FilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]); } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c382e50de..71da62ef39 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -729,11 +729,22 @@ groups: min: 500 max: 10000 default_value: 3000 - - name: rc_filter_frequency + - name: rc_filter_lpf_hz description: "RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values" default_value: 50 field: rcFilterFrequency - min: 0 + min: 15 + max: 250 + - name: rc_filter_auto + description: "When enabled, INAV will set RC filtering based on refresh rate and smoothing factor." + type: bool + default_value: OFF + field: autoSmooth + - name: rc_filter_smoothing_factor + description: "The RC filter smoothing factor. The higher the value, the more smoothing but also the more delay in response. Value 1 sets the filter at half the refresh rate. Value 100 sets the filter to aprox. 10% of the RC refresh rate" + field: autoSmoothFactor + default_value: 30 + min: 1 max: 100 - name: serialrx_provider description: "When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section." diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 2ca13cc2ac..eb8f517748 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -101,7 +101,7 @@ rxLinkStatistics_t rxLinkStatistics; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; -PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 12); #ifndef SERIALRX_PROVIDER #define SERIALRX_PROVIDER 0 @@ -129,7 +129,9 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .rssiMin = SETTING_RSSI_MIN_DEFAULT, .rssiMax = SETTING_RSSI_MAX_DEFAULT, .sbusSyncInterval = SETTING_SBUS_SYNC_INTERVAL_DEFAULT, - .rcFilterFrequency = SETTING_RC_FILTER_FREQUENCY_DEFAULT, + .rcFilterFrequency = SETTING_RC_FILTER_LPF_HZ_DEFAULT, + .autoSmooth = SETTING_RC_FILTER_AUTO_DEFAULT, + .autoSmoothFactor = SETTING_RC_FILTER_SMOOTHING_FACTOR_DEFAULT, #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) .mspOverrideChannels = SETTING_MSP_OVERRIDE_CHANNELS_DEFAULT, #endif diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 64ad7ac1b7..a9b0f03494 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -124,6 +124,8 @@ typedef struct rxConfig_s { uint16_t rx_min_usec; uint16_t rx_max_usec; uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness) + uint8_t autoSmooth; // auto smooth rx input (0 = off, 1 = on) + uint8_t autoSmoothFactor; // auto smooth rx input factor (1 = no smoothing, 100 = lots of smoothing) uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active uint8_t rssi_source; #ifdef USE_SERIALRX_SRXL2 From 60af9db178528590b05aaa953c2931244a5c8b6d Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 11 Jul 2022 17:52:59 -0300 Subject: [PATCH 28/60] Drop the MPU-6050 Support --- src/main/CMakeLists.txt | 2 - src/main/drivers/accgyro/accgyro_mpu.h | 6 - src/main/drivers/accgyro/accgyro_mpu6050.c | 222 --------------------- src/main/drivers/accgyro/accgyro_mpu6050.h | 23 --- src/main/drivers/bus.h | 1 - src/main/fc/cli.c | 2 +- src/main/fc/settings.yaml | 2 +- src/main/sensors/acceleration.c | 14 -- src/main/sensors/acceleration.h | 1 - src/main/sensors/gyro.c | 10 - src/main/sensors/gyro.h | 1 - src/main/target/common_hardware.c | 4 - src/main/target/sanity_check.h | 7 +- 13 files changed, 3 insertions(+), 292 deletions(-) delete mode 100755 src/main/drivers/accgyro/accgyro_mpu6050.c delete mode 100755 src/main/drivers/accgyro/accgyro_mpu6050.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 04366c37c2..bf335ea032 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -88,8 +88,6 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_mpu.h drivers/accgyro/accgyro_mpu6000.c drivers/accgyro/accgyro_mpu6000.h - drivers/accgyro/accgyro_mpu6050.c - drivers/accgyro/accgyro_mpu6050.h drivers/accgyro/accgyro_mpu6500.c drivers/accgyro/accgyro_mpu6500.h drivers/accgyro/accgyro_mpu9250.c diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index a16701e97d..90f7094521 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -21,12 +21,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro/accgyro.h" -#define MPU_I2C_ADDRESS 0x68 - -// MPU6050 -#define MPU_RA_WHO_AM_I_LEGACY 0x00 - -#define MPUx0x0_WHO_AM_I_CONST (0x68) // MPU6000 and 6050 #define MPU6000_WHO_AM_I_CONST (0x68) #define MPU6500_WHO_AM_I_CONST (0x70) #define MPU9250_WHO_AM_I_CONST (0x71) diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c deleted file mode 100755 index 5c83f977d7..0000000000 --- a/src/main/drivers/accgyro/accgyro_mpu6050.c +++ /dev/null @@ -1,222 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * INAV is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with INAV. If not, see . - */ - -/* - * Authors: - * Dominic Clifton - Cleanflight implementation - * John Ihlein - Initial FF32 code - * Konstantin Sharlaimov - busDevice refactoring -*/ - -#include -#include - -#include "platform.h" -#include "build/debug.h" - -#include "common/axis.h" -#include "common/maths.h" - -#include "drivers/system.h" -#include "drivers/time.h" -#include "drivers/io.h" -#include "drivers/exti.h" -#include "drivers/bus.h" - -#include "drivers/sensor.h" -#include "drivers/accgyro/accgyro.h" -#include "drivers/accgyro/accgyro_mpu.h" -#include "drivers/accgyro/accgyro_mpu6050.h" - -#if defined(USE_IMU_MPU6050) - -#define BIT_H_RESET 0x80 -#define MPU_CLK_SEL_PLLGYROZ 0x03 -#define MPU_INQUIRY_MASK 0x7E - -typedef enum { - MPU6050_NONE = 0, - MPU6050_HALF_RESOLUTION = 1, - MPU6050_FULL_RESOLUTION = 2 -} mpuDetectionResult_e; - -static bool mpu6050InitDone = false; - -static void mpu6050AccAndGyroInit(gyroDev_t *gyro) -{ - const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs); - gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz; - - gyroIntExtiInit(gyro); - - busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION); - - if (!mpu6050InitDone) { - // Device Reset - busWrite(gyro->busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET); - delay(150); - - // Clock Source PPL with Z axis gyro reference - busWrite(gyro->busDev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - delayMicroseconds(15); - - // Accel Sample Rate 1kHz - // Gyroscope Output Rate = 1kHz when the DLPF is enabled - busWrite(gyro->busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]); - delayMicroseconds(15); - - // Accel and Gyro DLPF Setting - busWrite(gyro->busDev, MPU_RA_CONFIG, config->gyroConfigValues[0]); - delayMicroseconds(1); - - // Gyro +/- 2000 DPS Full Scale - busWrite(gyro->busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); - delayMicroseconds(15); - - // Accel +/- 16 G Full Scale - busWrite(gyro->busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); - delayMicroseconds(15); - - busWrite(gyro->busDev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS - delayMicroseconds(15); - -#ifdef USE_MPU_DATA_READY_SIGNAL - busWrite(gyro->busDev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); - delayMicroseconds(15); -#endif - - mpu6050InitDone = true; - } - - busSetSpeed(gyro->busDev, BUS_SPEED_FAST); -} - -static void mpu6050AccInit(accDev_t *acc) -{ - mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev); - if (ctx->chipMagicNumber == 0x6850) { - acc->acc_1G = 512 * 4; - } - else { - acc->acc_1G = 256 * 4; - } -} - -bool mpu6050AccDetect(accDev_t *acc) -{ - acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_MPU6050, acc->imuSensorToUse); - if (acc->busDev == NULL) { - return false; - } - - mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev); - if (ctx->chipMagicNumber == 0x6850 || ctx->chipMagicNumber == 0x6050) { - acc->initFn = mpu6050AccInit; - acc->readFn = mpuAccReadScratchpad; - acc->accAlign = acc->busDev->param; - return true; - } - - return false; -} - -static mpuDetectionResult_e mpu6050DeviceDetect(busDevice_t * dev) -{ - uint8_t in; - uint8_t readBuffer[6]; - uint8_t attemptsRemaining = 5; - - busSetSpeed(dev, BUS_SPEED_INITIALIZATION); - - busWrite(dev, MPU_RA_PWR_MGMT_1, BIT_H_RESET); - - do { - delay(150); - - busRead(dev, MPU_RA_WHO_AM_I, &in); - in &= MPU_INQUIRY_MASK; - if (in == MPUx0x0_WHO_AM_I_CONST) { - break; - } - if (!attemptsRemaining) { - return MPU6050_NONE; - } - } while (attemptsRemaining--); - - // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code - // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c - // determine product ID and accel revision - busReadBuf(dev, MPU_RA_XA_OFFS_H, readBuffer, 6); - uint8_t revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); - - if (revision) { - /* Congrats, these parts are better. */ - if (revision == 1) { - return MPU6050_HALF_RESOLUTION; - } else if (revision == 2) { - return MPU6050_FULL_RESOLUTION; - } else if ((revision == 3) || (revision == 7)) { - return MPU6050_FULL_RESOLUTION; - } else { - return MPU6050_NONE; - } - } else { - uint8_t productId; - - busRead(dev, MPU_RA_PRODUCT_ID, &productId); - revision = productId & 0x0F; - - if (!revision) { - return MPU6050_NONE; - } else if (revision == 4) { - return MPU6050_HALF_RESOLUTION; - } else { - return MPU6050_FULL_RESOLUTION; - } - } - - return MPU6050_NONE; -} - -bool mpu6050GyroDetect(gyroDev_t *gyro) -{ - gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MPU6050, gyro->imuSensorToUse, OWNER_MPU); - if (gyro->busDev == NULL) { - return false; - } - - mpuDetectionResult_e res = mpu6050DeviceDetect(gyro->busDev); - if (res == MPU6050_NONE) { - busDeviceDeInit(gyro->busDev); - return false; - } - - // Magic number for ACC detection to indicate that we have detected MPU6000 gyro - mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev); - ctx->chipMagicNumber = res == MPU6050_FULL_RESOLUTION ? 0x6850 : 0x6050; - - gyro->initFn = mpu6050AccAndGyroInit; - gyro->readFn = mpuGyroReadScratchpad; - gyro->intStatusFn = gyroCheckDataReady; - gyro->temperatureFn = mpuTemperatureReadScratchpad; - gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor - gyro->gyroAlign = gyro->busDev->param; - - return true; -} - -#endif diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.h b/src/main/drivers/accgyro/accgyro_mpu6050.h deleted file mode 100755 index eab20f3bf2..0000000000 --- a/src/main/drivers/accgyro/accgyro_mpu6050.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * INAV is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with INAV. If not, see . - */ - -#pragma once - -#include "drivers/sensor.h" - -bool mpu6050AccDetect(accDev_t *acc); -bool mpu6050GyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index b1c9aa720b..177a2e60ab 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -77,7 +77,6 @@ typedef enum { /* Combined ACC/GYRO chips */ DEVHW_MPU6000, - DEVHW_MPU6050, DEVHW_MPU6500, DEVHW_BMI160, DEVHW_BMI088_GYRO, diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a6ad8a4d0e..860452fb88 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -182,7 +182,7 @@ static const char * const blackboxIncludeFlagNames[] = { /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */ // sync with gyroSensor_e -static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"}; +static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"}; // sync this with sensors_e static const char * const sensorTypeNames[] = { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c382e50de..1def0d69ca 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4,7 +4,7 @@ tables: - name: gyro_lpf values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"] - name: acc_hardware - values: ["NONE", "AUTO", "MPU6050", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"] + values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C"] diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 9eb440f65e..c0fd157173 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -39,7 +39,6 @@ FILE_COMPILE_FOR_SPEED #include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro_mpu.h" #include "drivers/accgyro/accgyro_mpu6000.h" -#include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_mpu9250.h" @@ -119,19 +118,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) case ACC_AUTODETECT: FALLTHROUGH; -#ifdef USE_IMU_MPU6050 - case ACC_MPU6050: // MPU6050 - if (mpu6050AccDetect(dev)) { - accHardware = ACC_MPU6050; - break; - } - /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ - if (accHardwareToUse != ACC_AUTODETECT) { - break; - } - FALLTHROUGH; -#endif - #ifdef USE_IMU_MPU6000 case ACC_MPU6000: if (mpu6000AccDetect(dev)) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 88893a0ac7..aa9ef73754 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -35,7 +35,6 @@ typedef enum { ACC_NONE = 0, ACC_AUTODETECT, - ACC_MPU6050, ACC_MPU6000, ACC_MPU6500, ACC_MPU9250, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a66c9ae1d9..d086ffc862 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -41,7 +41,6 @@ FILE_COMPILE_FOR_SPEED #include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro_mpu.h" #include "drivers/accgyro/accgyro_mpu6000.h" -#include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_mpu9250.h" @@ -135,15 +134,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard case GYRO_AUTODETECT: FALLTHROUGH; -#ifdef USE_IMU_MPU6050 - case GYRO_MPU6050: - if (mpu6050GyroDetect(dev)) { - gyroHardware = GYRO_MPU6050; - break; - } - FALLTHROUGH; -#endif - #ifdef USE_IMU_MPU6000 case GYRO_MPU6000: if (mpu6000GyroDetect(dev)) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index e6cba22f4c..b98b141ee3 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -28,7 +28,6 @@ typedef enum { GYRO_NONE = 0, GYRO_AUTODETECT, - GYRO_MPU6050, GYRO_MPU6000, GYRO_MPU6500, GYRO_MPU9250, diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index f6841d5d8b..84670830ce 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -36,10 +36,6 @@ BUSDEV_REGISTER_SPI(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); #endif - #if defined(USE_IMU_MPU6050) - BUSDEV_REGISTER_I2C(busdev_mpu6050, DEVHW_MPU6050, MPU6050_I2C_BUS, MPU_ADDRESS, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_MPU6050_ALIGN); - #endif - #if defined(USE_IMU_MPU6500) #if defined(MPU6500_SPI_BUS) BUSDEV_REGISTER_SPI(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); diff --git a/src/main/target/sanity_check.h b/src/main/target/sanity_check.h index 983154a48e..5a1b7626e4 100644 --- a/src/main/target/sanity_check.h +++ b/src/main/target/sanity_check.h @@ -29,7 +29,7 @@ #error "Unnecessary USE_ACC and/or USE_GYRO" #endif -#if defined (USE_GYRO_MPU6000) || defined (USE_ACC_MPU6000) || defined (USE_GYRO_MPU6050) || defined (USE_ACC_MPU6050) +#if defined (USE_GYRO_MPU6000) || defined (USE_ACC_MPU6000) #error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx" #endif @@ -49,11 +49,6 @@ #error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx" #endif -// Make sure IMU alignments are migrated to IMU_xxx_ALIGN -#if defined (GYRO_MPU6050_ALIGN) || defined (ACC_MPU6050_ALIGN) -#error "Replace GYRO_MPU6050_ALIGN and ACC_MPU6050_ALIGN with IMU_MPU6050_ALIGN" -#endif - #if defined (GYRO_MPU6000_ALIGN) || defined (ACC_MPU6000_ALIGN) #error "Replace GYRO_MPU6000_ALIGN and ACC_MPU6000_ALIGN with IMU_MPU6000_ALIGN" #endif From 2057cc311135fafd021b6f4e3a24287ad15d4872 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 11 Jul 2022 21:09:34 -0300 Subject: [PATCH 29/60] [config_eeprom.c] Replace a function with another one that already exists in the library --- src/main/config/config_eeprom.c | 29 +++++++++-------------------- 1 file changed, 9 insertions(+), 20 deletions(-) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index f64b2bb8a6..645398e8da 100755 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -123,17 +123,6 @@ void initEEPROM(void) #endif } -static uint16_t updateCRC(uint16_t crc, const void *data, uint32_t length) -{ - const uint8_t *p = (const uint8_t *)data; - const uint8_t *pend = p + length; - - for (; p != pend; p++) { - crc = crc16_ccitt(crc, *p); - } - return crc; -} - // Scan the EEPROM config. Returns true if the config is valid. bool isEEPROMContentValid(void) { @@ -143,7 +132,7 @@ bool isEEPROMContentValid(void) if (header->format != EEPROM_CONF_VERSION) { return false; } - uint16_t crc = updateCRC(0, header, sizeof(*header)); + uint16_t crc = crc16_ccitt_update(0, header, sizeof(*header)); p += sizeof(*header); for (;;) { @@ -164,13 +153,13 @@ bool isEEPROMContentValid(void) return false; } - crc = updateCRC(crc, p, record->size); + crc = crc16_ccitt_update(crc, p, record->size); p += record->size; } const configFooter_t *footer = (const configFooter_t *)p; - crc = updateCRC(crc, footer, sizeof(*footer)); + crc = crc16_ccitt_update(crc, footer, sizeof(*footer)); p += sizeof(*footer); const uint16_t checkSum = *(uint16_t *)p; p += sizeof(checkSum); @@ -255,7 +244,7 @@ static bool writeSettingsToEEPROM(void) if (config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)) < 0) { return false; } - uint16_t crc = updateCRC(0, (uint8_t *)&header, sizeof(header)); + uint16_t crc = crc16_ccitt_update(0, (uint8_t *)&header, sizeof(header)); PG_FOREACH(reg) { const uint16_t regSize = pgSize(reg); configRecord_t record = { @@ -271,11 +260,11 @@ static bool writeSettingsToEEPROM(void) if (config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)) < 0) { return false; } - crc = updateCRC(crc, (uint8_t *)&record, sizeof(record)); + crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record)); if (config_streamer_write(&streamer, reg->address, regSize) < 0) { return false; } - crc = updateCRC(crc, reg->address, regSize); + crc = crc16_ccitt_update(crc, reg->address, regSize); } else { // write one instance for each profile for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) { @@ -285,12 +274,12 @@ static bool writeSettingsToEEPROM(void) if (config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)) < 0) { return false; } - crc = updateCRC(crc, (uint8_t *)&record, sizeof(record)); + crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record)); const uint8_t *address = reg->address + (regSize * profileIndex); if (config_streamer_write(&streamer, address, regSize) < 0) { return false; } - crc = updateCRC(crc, address, regSize); + crc = crc16_ccitt_update(crc, address, regSize); } } } @@ -302,7 +291,7 @@ static bool writeSettingsToEEPROM(void) if (config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer)) < 0) { return false; } - crc = updateCRC(crc, (uint8_t *)&footer, sizeof(footer)); + crc = crc16_ccitt_update(crc, (uint8_t *)&footer, sizeof(footer)); // append checksum now if (config_streamer_write(&streamer, (uint8_t *)&crc, sizeof(crc)) < 0) { From 91512e1e598d8b23518dc5740ce47a6736844f2f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 12 Jul 2022 09:41:17 +0100 Subject: [PATCH 30/60] Update osd.c --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b85d183fc4..95e282e687 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -197,7 +197,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 7); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); static int digitCount(int32_t value) From d9a187c44d132b9962a68a3957ff004caa3b6c35 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 12 Jul 2022 12:56:33 +0100 Subject: [PATCH 31/60] initial build --- src/main/fc/settings.yaml | 6 ++++++ src/main/navigation/navigation.c | 5 +++-- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fw_launch.c | 5 +++-- src/main/navigation/navigation_private.h | 2 -- 5 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 137a76a1f4..78963525cf 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2816,6 +2816,12 @@ groups: field: fw.launch_climb_angle min: 5 max: 45 + - name: nav_fw_launch_abort_deadband + description: "Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch." + default_value: 100 + field: fw.launch_abort_deadband + min: 2 + max: 250 - name: nav_fw_cruise_yaw_rate description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]" default_value: 20 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c35a2c92bf..601451df00 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -100,7 +100,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 1); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -203,6 +203,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees .launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg + .launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us .cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT, .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT, @@ -1833,7 +1834,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF } // abort NAV_LAUNCH_MODE by moving sticks with low throttle or throttle stick < launch idle throttle - if (abortLaunchAllowed() && isRollPitchStickDeflected(LAUNCH_ABORT_STICK_DEADBAND)) { + if (abortLaunchAllowed() && isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) { abortFixedWingLaunch(); return NAV_FSM_EVENT_SWITCH_TO_IDLE; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 0b63d05352..3259c9afbc 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -301,6 +301,7 @@ typedef struct navConfig_s { uint16_t launch_max_altitude; // cm, altitude where to consider launch ended uint8_t launch_climb_angle; // Target climb angle for launch (deg) uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] + uint8_t launch_abort_deadband; // roll/pitch stick movement deadband for launch abort uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled bool allow_manual_thr_increase; bool useFwNavYawControl; diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 8f509d37e2..0ad1de7f9c 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -260,7 +260,8 @@ static inline bool isLaunchMaxAltitudeReached(void) static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { - return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && isRollPitchStickDeflected(LAUNCH_ABORT_STICK_DEADBAND); + return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && + isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband); } static void resetPidsIfNeeded(void) { @@ -438,7 +439,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; - if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(LAUNCH_ABORT_STICK_DEADBAND)) { + if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) { return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state } else { diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 552be2f0de..ff3d8d5400 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -47,8 +47,6 @@ #define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points -#define LAUNCH_ABORT_STICK_DEADBAND 250 // pitch/roll stick deflection for launch abort (us) - #define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); From 083c28e5b83893b256ec7716960c4d2572e17d45 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 12 Jul 2022 13:47:18 +0100 Subject: [PATCH 32/60] Update Settings.md --- docs/Settings.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index 8f61e19060..73acafeec9 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2952,6 +2952,16 @@ Dive angle that airplane will use during final landing phase. During dive phase, --- +### nav_fw_launch_abort_deadband + +Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 2 | 250 | + +--- + ### nav_fw_launch_accel Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s From 973dab212203917dcfb103c347fdcb76c861be79 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Tue, 12 Jul 2022 20:11:03 -0300 Subject: [PATCH 33/60] Remove unused header file --- src/main/config/config_unittest.h | 100 ------------------------------ 1 file changed, 100 deletions(-) delete mode 100644 src/main/config/config_unittest.h diff --git a/src/main/config/config_unittest.h b/src/main/config/config_unittest.h deleted file mode 100644 index eee4b83109..0000000000 --- a/src/main/config/config_unittest.h +++ /dev/null @@ -1,100 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#ifdef SRC_MAIN_SCHEDULER_C_ -#ifdef UNIT_TEST - -cfTask_t *unittest_scheduler_selectedTask; -uint8_t unittest_scheduler_selectedTaskDynamicPriority; -uint16_t unittest_scheduler_waitingTasks; -uint32_t unittest_scheduler_timeToNextRealtimeTask; -bool unittest_outsideRealtimeGuardInterval; - -#define GET_SCHEDULER_LOCALS() \ - { \ - unittest_scheduler_selectedTask = selectedTask; \ - unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority; \ - unittest_scheduler_waitingTasks = waitingTasks; \ - unittest_scheduler_timeToNextRealtimeTask = timeToNextRealtimeTask; \ - } - -#else - -#define SET_SCHEDULER_LOCALS() {} -#define GET_SCHEDULER_LOCALS() {} - -#endif -#endif - - -#ifdef SRC_MAIN_FLIGHT_PID_C_ -#ifdef UNIT_TEST - -float unittest_pidLuxFloat_lastErrorForDelta[3]; -float unittest_pidLuxFloat_delta1[3]; -float unittest_pidLuxFloat_delta2[3]; -float unittest_pidLuxFloat_PTerm[3]; -float unittest_pidLuxFloat_ITerm[3]; -float unittest_pidLuxFloat_DTerm[3]; - -#define SET_PID_LUX_FLOAT_LOCALS(axis) \ - { \ - lastErrorForDelta[axis] = unittest_pidLuxFloat_lastErrorForDelta[axis]; \ - delta1[axis] = unittest_pidLuxFloat_delta1[axis]; \ - delta2[axis] = unittest_pidLuxFloat_delta2[axis]; \ - } - -#define GET_PID_LUX_FLOAT_LOCALS(axis) \ - { \ - unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \ - unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \ - unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \ - unittest_pidLuxFloat_PTerm[axis] = PTerm; \ - unittest_pidLuxFloat_ITerm[axis] = ITerm; \ - unittest_pidLuxFloat_DTerm[axis] = DTerm; \ - } - -int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3]; -int32_t unittest_pidMultiWiiRewrite_PTerm[3]; -int32_t unittest_pidMultiWiiRewrite_ITerm[3]; -int32_t unittest_pidMultiWiiRewrite_DTerm[3]; - -#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) \ - { \ - lastErrorForDelta[axis] = unittest_pidMultiWiiRewrite_lastErrorForDelta[axis]; \ - } - -#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) \ - { \ - unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \ - unittest_pidMultiWiiRewrite_PTerm[axis] = PTerm; \ - unittest_pidMultiWiiRewrite_ITerm[axis] = ITerm; \ - unittest_pidMultiWiiRewrite_DTerm[axis] = DTerm; \ - } - -#else - -#define SET_PID_LUX_FLOAT_LOCALS(axis) {} -#define GET_PID_LUX_FLOAT_LOCALS(axis) {} -#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) {} -#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) {} - -#endif // UNIT_TEST -#endif // SRC_MAIN_FLIGHT_PID_C_ - From 03b5a2807da8fd78ddf3d6f2cba6b2ffe754cc73 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Tue, 12 Jul 2022 20:13:52 -0300 Subject: [PATCH 34/60] [Building in Linux.md] Fix Typo --- docs/development/Building in Linux.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/Building in Linux.md b/docs/development/Building in Linux.md index 739ee3c3e5..4d73573cf3 100644 --- a/docs/development/Building in Linux.md +++ b/docs/development/Building in Linux.md @@ -148,7 +148,7 @@ It is unlikely that the typical user will need to employ these options, other th * Configure `cmake` to use `ninja` as the build system ``` - cd buid + cd build # add other cmake options as required. cmake -GNinja .. ``` From c9b4d889c7c8e891838d8e252495b7ea7cdeb5b4 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 13 Jul 2022 13:10:32 +0200 Subject: [PATCH 35/60] Remove empty lines --- src/main/fc/rc_smoothing.c | 24 ------------------------ 1 file changed, 24 deletions(-) diff --git a/src/main/fc/rc_smoothing.c b/src/main/fc/rc_smoothing.c index fba7178bf0..f20de090e6 100644 --- a/src/main/fc/rc_smoothing.c +++ b/src/main/fc/rc_smoothing.c @@ -134,27 +134,3 @@ void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs) rcCommand[stick] = pt3FilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]); } } - - - - - - - - - - - - - - - - - - - - - - - - From c2585ae4aa00a900594d3d4e48e8b22ad8af6cc8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 13 Jul 2022 14:39:24 +0200 Subject: [PATCH 36/60] Store rxUpdateRate in the blackbox log --- src/main/blackbox/blackbox.c | 7 +++++ src/main/fc/rc_smoothing.c | 50 +++++++++++++++++++++--------------- src/main/fc/rc_smoothing.h | 1 + 3 files changed, 38 insertions(+), 20 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 7fecd3dd8b..238a67db93 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -54,6 +54,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/settings.h" +#include "fc/rc_smoothing.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -433,6 +434,7 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)}, #endif + {"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)}, }; typedef enum BlackboxState { @@ -550,6 +552,7 @@ typedef struct blackboxSlowState_s { uint32_t escRPM; int8_t escTemperature; #endif + uint16_t rxUpdateRate; } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From rc_controls.c @@ -1284,6 +1287,8 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.escRPM); blackboxWriteSignedVB(slowHistory.escTemperature); #endif + blackboxWriteUnsignedVB(slowHistory.rxUpdateRate); + blackboxSlowFrameIterationTimer = 0; } @@ -1341,6 +1346,8 @@ static void loadSlowState(blackboxSlowState_t *slow) slow->escRPM = escSensor->rpm; slow->escTemperature = escSensor->temperature; #endif + + slow->rxUpdateRate = getRcUpdateFrequency(); } /** diff --git a/src/main/fc/rc_smoothing.c b/src/main/fc/rc_smoothing.c index f20de090e6..c47cbc8e7c 100644 --- a/src/main/fc/rc_smoothing.c +++ b/src/main/fc/rc_smoothing.c @@ -52,6 +52,11 @@ static pt3Filter_t rcSmoothFilter[4]; static float rcStickUnfiltered[4]; +static uint16_t rcUpdateFrequency; + +uint16_t getRcUpdateFrequency(void) { + return rcUpdateFrequency; +} static int32_t applyRcUpdateFrequencyMedianFilter(int32_t newReading) { @@ -102,34 +107,39 @@ void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs) return; } - if (isRXDataNew && rxConfig()->autoSmooth) { + if (isRXDataNew) { const timeDelta_t delta = cmpTimeUs(currentTimeUs, previousRcData); - const int32_t rcUpdateFrequency = applyRcUpdateFrequencyMedianFilter(1.0f / (delta * 0.000001f)); + rcUpdateFrequency = applyRcUpdateFrequencyMedianFilter(1.0f / (delta * 0.000001f)); + previousRcData = currentTimeUs; - const int nyquist = rcUpdateFrequency / 2; + /* + * If auto smoothing is enabled, update the filters + */ + if (rxConfig()->autoSmooth) { + const int nyquist = rcUpdateFrequency / 2; - int newFilterFrequency = scaleRange( - rxConfig()->autoSmoothFactor, - 1, - 100, - nyquist, - rcUpdateFrequency / 10 - ); - - // Do not allow filter frequency to go below RC_INTERPOLATION_MIN_FREQUENCY or above nuyquist frequency. - newFilterFrequency = constrain(newFilterFrequency, RC_INTERPOLATION_MIN_FREQUENCY, nyquist); + int newFilterFrequency = scaleRange( + rxConfig()->autoSmoothFactor, + 1, + 100, + nyquist, + rcUpdateFrequency / 10 + ); + + // Do not allow filter frequency to go below RC_INTERPOLATION_MIN_FREQUENCY or above nuyquist frequency. + newFilterFrequency = constrain(newFilterFrequency, RC_INTERPOLATION_MIN_FREQUENCY, nyquist); - if (newFilterFrequency != filterFrequency) { + if (newFilterFrequency != filterFrequency) { - for (int stick = 0; stick < 4; stick++) { - pt3FilterUpdateCutoff(&rcSmoothFilter[stick], pt3FilterGain(newFilterFrequency, dT)); + for (int stick = 0; stick < 4; stick++) { + pt3FilterUpdateCutoff(&rcSmoothFilter[stick], pt3FilterGain(newFilterFrequency, dT)); + } + filterFrequency = newFilterFrequency; } - filterFrequency = newFilterFrequency; } - previousRcData = currentTimeUs; - } - + } + for (int stick = 0; stick < 4; stick++) { rcCommand[stick] = pt3FilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]); } diff --git a/src/main/fc/rc_smoothing.h b/src/main/fc/rc_smoothing.h index 7b286c0bb7..53cd9a8a68 100644 --- a/src/main/fc/rc_smoothing.h +++ b/src/main/fc/rc_smoothing.h @@ -28,4 +28,5 @@ #include #include +uint16_t getRcUpdateFrequency(void); void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs); \ No newline at end of file From e4226acf31bcd52f7e8a1fb80dda429431140d98 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 13 Jul 2022 19:54:17 +0200 Subject: [PATCH 37/60] Report motor RPM via MSP --- src/main/fc/fc_msp.c | 14 ++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 ++ 2 files changed, 16 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1787c3b9be..2a363b3c89 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -116,6 +116,7 @@ #include "sensors/gyro.h" #include "sensors/opflow.h" #include "sensors/temperature.h" +#include "sensors/esc_sensor.h" #include "telemetry/telemetry.h" @@ -1521,6 +1522,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #endif +#ifdef USE_ESC_SENSOR + case MSP2_INAV_ESC_RPM: + { + uint8_t motorCount = getMotorCount(); + + for (uint8_t i = 0; i < motorCount; i++){ + const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry + sbufWriteU32(dst, escState->rpm); + } + } + break; +#endif + default: return false; } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 51311f4557..77aab6fe13 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -81,3 +81,5 @@ #define MSP2_INAV_MISC2 0x203A #define MSP2_INAV_LOGIC_CONDITIONS_SINGLE 0x203B + +#define MSP2_INAV_ESC_RPM 0x2040 \ No newline at end of file From 1c17a16b2b73be3252b98d65dab8f89fcfadf197 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 12 Jul 2022 11:11:13 +0200 Subject: [PATCH 38/60] Do not automatically persist the gyro calibration --- src/main/fc/config.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c34e890106..b8f665598d 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -421,16 +421,10 @@ void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) { gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X]; gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y]; gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z]; - // save the calibration - writeEEPROM(); - readEEPROM(); } void setGravityCalibrationAndWriteEEPROM(float getGravity) { gyroConfigMutable()->gravity_cmss_cal = getGravity; - // save the calibration - writeEEPROM(); - readEEPROM(); } void beeperOffSet(uint32_t mask) From 3faf76a6307e78c914dd41b3d7c587a46db57c2d Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 13 Jul 2022 19:33:09 +0100 Subject: [PATCH 39/60] Update navigation_fw_launch.c --- src/main/navigation/navigation_fw_launch.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index ba0d423c0d..1515f46004 100644 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -430,7 +430,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t if (isThrottleLow()) { fwLaunch.currentStateTimeUs = currentTimeUs; fwLaunch.pitchAngle = 0; - if (isRollPitchStickDeflected(LAUNCH_ABORT_STICK_DEADBAND)) { + if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) { return FW_LAUNCH_EVENT_ABORT; } } else { From 675e1e276fc1a9705201dcff5076302438a24d76 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 13 Jul 2022 21:02:39 +0100 Subject: [PATCH 40/60] Make HUD offset more intuitive Updated HUD offset to be more intuitive. Positive values move the HUD up. Negative values move the HUD down. --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b85d183fc4..0316321d8d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1001,7 +1001,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) { *x = osdDisplayPort->cols / 2; *y = osdDisplayPort->rows / 2; - *y += osdConfig()->horizon_offset; + *y -= osdConfig()->horizon_offset; // positive horizon_offset moves the HUD up, negative moves down } /** From 08fdb7d1fbae23f73798b86b1ebb5f8721d1e2d7 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 14 Jul 2022 03:47:20 +0300 Subject: [PATCH 41/60] fixed wind direction icon --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 95e282e687..e8ab2753ea 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2802,8 +2802,8 @@ static bool osdDrawSingleElement(uint8_t item) if (valid) { uint16_t angle; horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle); - int16_t windDirection = osdGetHeadingAngle((int)angle - DECIDEGREES_TO_DEGREES(attitude.values.yaw)); - buff[1] = SYM_DIRECTION + (windDirection * 2 / 90); + int16_t windDirection = osdGetHeadingAngle( DECIDEGREES_TO_DEGREES(attitude.values.yaw) - CENTIDEGREES_TO_DEGREES((int)angle) + 22); + buff[1] = SYM_DIRECTION + (windDirection*2 / 90); } else { horizontalWindSpeed = 0; buff[1] = SYM_BLANK; From 3b48832feb806e90e6f46f846918a425b701712b Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 14 Jul 2022 03:48:34 +0300 Subject: [PATCH 42/60] fixed wind estimator --- src/main/flight/wind_estimator.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index e0f0de6586..7fe49a4594 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -102,8 +102,8 @@ void updateWindEstimator(timeUs_t currentTimeUs) // Fuselage direction in earth frame fuselageDirection[X] = rMat[0][0]; - fuselageDirection[Y] = rMat[1][0]; - fuselageDirection[Z] = rMat[2][0]; + fuselageDirection[Y] = -rMat[1][0]; + fuselageDirection[Z] = -rMat[2][0]; timeDelta_t timeDelta = cmpTimeUs(currentTimeUs, lastUpdateUs); // scrap our data and start over if we're taking too long to get a direction change @@ -131,7 +131,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) // when turning, use the attitude response to estimate wind speed groundVelocityDiff[X] = groundVelocity[X] - lastGroundVelocity[X]; groundVelocityDiff[Y] = groundVelocity[Y] - lastGroundVelocity[Y]; - groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z]; + groundVelocityDiff[Z] = groundVelocity[Z] - lastGroundVelocity[Z]; // estimate airspeed it using equation 6 float V = (calc_length_pythagorean_3D(groundVelocityDiff[X], groundVelocityDiff[Y], groundVelocityDiff[Z])) / fast_fsqrtf(diffLengthSq); From d83a550aa831ad506c619eef2ff85f2940c70cc9 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 14 Jul 2022 04:36:22 +0300 Subject: [PATCH 43/60] fixed osd vertical wind icon --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 95e282e687..c49ec0ef4e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2824,7 +2824,7 @@ static bool osdDrawSingleElement(uint8_t item) bool valid = isEstimatedWindSpeedValid(); float verticalWindSpeed; if (valid) { - verticalWindSpeed = getEstimatedWindSpeed(Z); + verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU if (verticalWindSpeed < 0) { buff[1] = SYM_AH_DECORATION_DOWN; verticalWindSpeed = -verticalWindSpeed; From 1b3edee543475f6626fbbdccb1120d06ce6811d0 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 14 Jul 2022 04:37:02 +0300 Subject: [PATCH 44/60] fixed vertical wind speed usage --- src/main/flight/rth_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 104e795a9c..1597737a34 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -153,7 +153,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { uint16_t windHeading; // centidegrees const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading); - const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100; + const float verticalWindSpeed = -getEstimatedWindSpeed(Z) / 100; //from NED to NEU const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading); const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees); From 02d01ed1daa031f2acddeb28b7452e158fa5d91d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 14 Jul 2022 15:48:56 +0200 Subject: [PATCH 45/60] Proof of concept --- src/main/CMakeLists.txt | 2 + src/main/flight/dynamic_gyro_notch.h | 2 - .../flight/secondary_dynamic_gyro_notch.c | 83 +++++++++++++++++++ .../flight/secondary_dynamic_gyro_notch.h | 43 ++++++++++ src/main/sensors/gyro.c | 21 +++++ src/main/sensors/gyro.h | 1 + 6 files changed, 150 insertions(+), 2 deletions(-) create mode 100644 src/main/flight/secondary_dynamic_gyro_notch.c create mode 100644 src/main/flight/secondary_dynamic_gyro_notch.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index aadbaf2006..595ac4cbb1 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -331,6 +331,8 @@ main_sources(COMMON_SRC flight/rpm_filter.h flight/dynamic_gyro_notch.c flight/dynamic_gyro_notch.h + flight/secondary_dynamic_gyro_notch.c + flight/secondary_dynamic_gyro_notch.h flight/dynamic_lpf.c flight/dynamic_lpf.h flight/secondary_imu.c diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h index 40203a7bc5..b8adbfa0ea 100644 --- a/src/main/flight/dynamic_gyro_notch.h +++ b/src/main/flight/dynamic_gyro_notch.h @@ -37,8 +37,6 @@ typedef struct dynamicGyroNotchState_s { uint16_t frequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; float dynNotchQ; - float dynNotch1Ctr; - float dynNotch2Ctr; uint32_t looptime; uint8_t enabled; diff --git a/src/main/flight/secondary_dynamic_gyro_notch.c b/src/main/flight/secondary_dynamic_gyro_notch.c new file mode 100644 index 0000000000..cf6360c46b --- /dev/null +++ b/src/main/flight/secondary_dynamic_gyro_notch.c @@ -0,0 +1,83 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" + +#ifdef USE_DYNAMIC_FILTERS + +FILE_COMPILE_FOR_SPEED + +#include +#include "secondary_dynamic_gyro_notch.h" +#include "fc/config.h" +#include "build/debug.h" +#include "sensors/gyro.h" + +#define SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 150 + +void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state) { + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + state->filtersApplyFn[axis] = nullFilterApply; + } + + state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; + state->enabled = gyroConfig()->dynamicGyroNotchEnabled; + state->looptime = getLooptime(); + + if (state->enabled) { + /* + * Step 1 - init all filters even if they will not be used further down the road + */ + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + //Any initial notch Q is valid sice it will be updated immediately after + biquadFilterInit(&state->filters[axis], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[axis] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + + } +} + +void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) { + + if (state->enabled) { + + /* + * The secondary dynamic notch uses only the first detected frequency + * The rest of peaks are ignored + */ + state->frequency[axis] = frequency[0]; + + // Filter update happens only if peak was detected + if (frequency[0] > 0.0f) { + biquadFilterUpdate(&state->filters[axis], state->frequency[axis], state->looptime, state->dynNotchQ, FILTER_NOTCH); + } + } +} + +float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input) { + return state->filtersApplyFn[axis]((filter_t *)&state->filters[axis], input); +} + +#endif \ No newline at end of file diff --git a/src/main/flight/secondary_dynamic_gyro_notch.h b/src/main/flight/secondary_dynamic_gyro_notch.h new file mode 100644 index 0000000000..ac2e4975e5 --- /dev/null +++ b/src/main/flight/secondary_dynamic_gyro_notch.h @@ -0,0 +1,43 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include +#include "common/axis.h" +#include "common/filter.h" + +typedef struct secondaryDynamicGyroNotchState_s { + uint16_t frequency[XYZ_AXIS_COUNT]; + float dynNotchQ; + uint32_t looptime; + uint8_t enabled; + + biquadFilter_t filters[XYZ_AXIS_COUNT]; + filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT]; +} secondaryDynamicGyroNotchState_t; + +void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state); +void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]); +float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input); \ No newline at end of file diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b13def362f..34c3eaef39 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -92,6 +92,7 @@ STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT]; EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState; EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; +EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState; #endif @@ -298,6 +299,9 @@ bool gyroInit(void) #ifdef USE_DYNAMIC_FILTERS // Dynamic notch running at PID frequency dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); + + secondaryDynamicGyroNotchFiltersInit(&secondaryDynamicGyroNotchState); + gyroDataAnalyseStateInit( &gyroAnalyseState, gyroConfig()->dynamicGyroNotchMinHz, @@ -449,6 +453,16 @@ void FAST_CODE NOINLINE gyroFilter() gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf); } + + /** + * Secondary dynamic notch filter. + * In some cases, noise amplitude is high enough not to be filtered by the primary filter. + * This happens on the first frequency with the biggest aplitude + */ + DEBUG_SET(DEBUG_ALWAYS, axis, gyroADCf); + secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); + DEBUG_SET(DEBUG_ALWAYS, axis + 3, gyroADCf); + #endif #ifdef USE_GYRO_KALMAN @@ -470,6 +484,13 @@ void FAST_CODE NOINLINE gyroFilter() gyroAnalyseState.filterUpdateAxis, gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis] ); + + secondaryDynamicGyroNotchFiltersUpdate( + &secondaryDynamicGyroNotchState, + gyroAnalyseState.filterUpdateAxis, + gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis] + ); + } } #endif diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b98b141ee3..d2c310f1a9 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -24,6 +24,7 @@ #include "config/parameter_group.h" #include "drivers/sensor.h" #include "flight/dynamic_gyro_notch.h" +#include "flight/secondary_dynamic_gyro_notch.h" typedef enum { GYRO_NONE = 0, From 49782e4be7abd5db31fd2ce4f4f15f0fc2b6dc05 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 14 Jul 2022 21:30:45 -0300 Subject: [PATCH 46/60] Remove some NO optimizations made by MCU_FLASH_SIZE --- src/main/fc/cli.c | 8 -------- src/main/target/common.h | 14 -------------- src/main/target/common_post.h | 4 ++-- 3 files changed, 2 insertions(+), 24 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 860452fb88..198641a0c7 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -125,10 +125,6 @@ bool cliMode = false; #include "telemetry/telemetry.h" #include "build/debug.h" -#if MCU_FLASH_SIZE > 128 -#define PLAY_SOUND -#endif - extern timeDelta_t cycleTime; // FIXME dependency on mw.c extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; @@ -2956,7 +2952,6 @@ static void cliMotor(char *cmdline) cliPrintLinef("motor %d: %d", motor_index, motor_disarmed[motor_index]); } -#ifdef PLAY_SOUND static void cliPlaySound(char *cmdline) { int i; @@ -2989,7 +2984,6 @@ static void cliPlaySound(char *cmdline) cliPrintLinef("Playing sound %d: %s", i, name); beeper(beeperModeForTableIndex(i)); } -#endif static void cliProfile(char *cmdline) { @@ -3907,9 +3901,7 @@ const clicmd_t cmdTable[] = { #ifdef USE_USB_MSC CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc), #endif -#ifdef PLAY_SOUND CLI_COMMAND_DEF("play_sound", NULL, "[]\r\n", cliPlaySound), -#endif CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("battery_profile", "change battery profile", diff --git a/src/main/target/common.h b/src/main/target/common.h index ad40b43eae..7c9c282812 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -66,8 +66,6 @@ #define SCHEDULER_DELAY_LIMIT 100 #endif -#if (MCU_FLASH_SIZE > 256) - #if defined(MAG_I2C_BUS) || defined(VCM5883_I2C_BUS) #define USE_MAG_VCM5883 #endif @@ -152,11 +150,6 @@ #define USE_POWER_LIMITS -#else // MCU_FLASH_SIZE < 256 -#define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR -#endif - -#if (MCU_FLASH_SIZE > 128) #define NAV_FIXED_WING_LANDING #define USE_SAFE_HOME #define USE_AUTOTUNE_FIXED_WING @@ -191,18 +184,11 @@ // Wind estimator #define USE_WIND_ESTIMATOR -#else // MCU_FLASH_SIZE < 128 - -#define SKIP_TASK_STATISTICS - -#endif - //Designed to free space of F722 and F411 MCUs #if (MCU_FLASH_SIZE > 512) #define USE_VTX_FFPV #define USE_PITOT_VIRTUAL - #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT #define USE_HOTT_TEXTMODE diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 426bbecd12..3c991d30cf 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -31,11 +31,11 @@ extern uint8_t __config_end; // Enable MSP_DISPLAYPORT for F3 targets without builtin OSD, // since it's used to display CMS on MWOSD -#if !defined(USE_MSP_DISPLAYPORT) && (MCU_FLASH_SIZE > 128) && !defined(USE_OSD) +#if !defined(USE_MSP_DISPLAYPORT) && !defined(USE_OSD) #define USE_MSP_DISPLAYPORT #endif -#if defined(USE_OSD) && (MCU_FLASH_SIZE > 256) +#if defined(USE_OSD) #define USE_CANVAS #endif From f604865fe53310b0e87dd929a604ace405ef8745 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 14 Jul 2022 21:41:39 -0300 Subject: [PATCH 47/60] remove others definition --- src/main/common/log.h | 16 +++++----------- src/main/fc/cli.c | 8 -------- src/main/scheduler/scheduler.c | 20 -------------------- src/main/scheduler/scheduler.h | 4 ---- 4 files changed, 5 insertions(+), 43 deletions(-) diff --git a/src/main/common/log.h b/src/main/common/log.h index 1622063d35..3bbc074409 100644 --- a/src/main/common/log.h +++ b/src/main/common/log.h @@ -47,13 +47,7 @@ void logInit(void); void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) __attribute__ ((format (printf, 3, 4))); void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t size); -// LOG_* macro definitions - -#if !defined(LOG_LEVEL_MAXIMUM) -#define LOG_LEVEL_MAXIMUM LOG_LEVEL_DEBUG -#endif - -#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_ERROR +#if defined(USE_LOG) #define LOG_E(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, fmt, ##__VA_ARGS__) #define LOG_BUFFER_E(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, buf, size) #else @@ -61,7 +55,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t #define LOG_BUFFER_E(...) #endif -#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_WARNING +#if defined(USE_LOG) #define LOG_W(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, fmt, ##__VA_ARGS__) #define LOG_BUF_W(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, buf, size) #else @@ -69,7 +63,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t #define LOG_BUF_W(...) #endif -#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_INFO +#if defined(USE_LOG) #define LOG_I(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, fmt, ##__VA_ARGS__) #define LOG_BUF_I(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, buf, size) #else @@ -77,7 +71,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t #define LOG_BUF_I(...) #endif -#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_VERBOSE +#if defined(USE_LOG) #define LOG_V(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, fmt, ##__VA_ARGS__) #define LOG_BUF_V(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, buf, size) #else @@ -85,7 +79,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t #define LOG_BUF_V(...) #endif -#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_DEBUG +#if defined(USE_LOG) #define LOG_D(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, fmt, ##__VA_ARGS__) #define LOG_BUF_D(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, buf, size) #else diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 198641a0c7..bea83a1711 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3473,7 +3473,6 @@ static void cliStatus(char *cmdline) } } -#ifndef SKIP_TASK_STATISTICS static void cliTasks(char *cmdline) { UNUSED(cmdline); @@ -3502,7 +3501,6 @@ static void cliTasks(char *cmdline) cliPrintLinef("Task check function %13d %7d %25d", (uint32_t)checkFuncInfo.maxExecutionTime, (uint32_t)checkFuncInfo.averageExecutionTime, (uint32_t)checkFuncInfo.totalExecutionTime / 1000); cliPrintLinef("Total (excluding SERIAL) %21d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10); } -#endif static void cliVersion(char *cmdline) { @@ -3535,7 +3533,6 @@ static void cliMemory(char *cmdline) } } -#if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES) static void cliResource(char *cmdline) { UNUSED(cmdline); @@ -3554,7 +3551,6 @@ static void cliResource(char *cmdline) } } } -#endif static void backupConfigs(void) { @@ -3906,9 +3902,7 @@ const clicmd_t cmdTable[] = { "[]", cliProfile), CLI_COMMAND_DEF("battery_profile", "change battery profile", "[]", cliBatteryProfile), -#if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES) CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), -#endif CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), #ifdef USE_SECONDARY_IMU CLI_COMMAND_DEF("imu2", "Secondary IMU", NULL, cliImu2), @@ -3943,9 +3937,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), #endif CLI_COMMAND_DEF("status", "show status", NULL, cliStatus), -#ifndef SKIP_TASK_STATISTICS CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks), -#endif #ifdef USE_TEMPERATURE_SENSOR CLI_COMMAND_DEF("temp_sensor", "change temp sensor settings", NULL, cliTempSensor), #endif diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 75b532e515..028ed1b662 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -131,7 +131,6 @@ void taskSystem(timeUs_t currentTimeUs) } } -#ifndef SKIP_TASK_STATISTICS #define TASK_MOVING_SUM_COUNT 32 FASTRAM timeUs_t checkFuncMaxExecutionTime; FASTRAM timeUs_t checkFuncTotalExecutionTime; @@ -155,7 +154,6 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo) taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / TASK_MOVING_SUM_COUNT; taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime; } -#endif void rescheduleTask(cfTaskId_e taskId, timeDelta_t newPeriodUs) { @@ -193,9 +191,6 @@ timeDelta_t getTaskDeltaTime(cfTaskId_e taskId) void schedulerResetTaskStatistics(cfTaskId_e taskId) { -#ifdef SKIP_TASK_STATISTICS - UNUSED(taskId); -#else if (taskId == TASK_SELF) { currentTask->movingSumExecutionTime = 0; currentTask->totalExecutionTime = 0; @@ -205,7 +200,6 @@ void schedulerResetTaskStatistics(cfTaskId_e taskId) cfTasks[taskId].totalExecutionTime = 0; cfTasks[taskId].totalExecutionTime = 0; } -#endif } void schedulerInit(void) @@ -237,13 +231,11 @@ void FAST_CODE NOINLINE scheduler(void) task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; waitingTasks++; } else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, currentTimeBeforeCheckFuncCallUs - task->lastExecutedAt)) { -#ifndef SKIP_TASK_STATISTICS const timeUs_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCallUs; checkFuncMovingSumExecutionTime -= checkFuncMovingSumExecutionTime / TASK_MOVING_SUM_COUNT; checkFuncMovingSumExecutionTime += checkFuncExecutionTime; checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime); -#endif task->lastSignaledAt = currentTimeBeforeCheckFuncCallUs; task->taskAgeCycles = 1; task->dynamicPriority = 1 + task->staticPriority; @@ -289,32 +281,20 @@ void FAST_CODE NOINLINE scheduler(void) // Execute task const timeUs_t currentTimeBeforeTaskCall = micros(); selectedTask->taskFunc(currentTimeBeforeTaskCall); - -#ifndef SKIP_TASK_STATISTICS const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall; selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT; selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); -#endif -#if defined(SCHEDULER_DEBUG) - DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler -#endif } if (!selectedTask || forcedRealTimeTask) { // Execute system real-time callbacks and account for them to SYSTEM account const timeUs_t currentTimeBeforeTaskCall = micros(); taskRunRealtimeCallbacks(currentTimeBeforeTaskCall); - -#ifndef SKIP_TASK_STATISTICS selectedTask = &cfTasks[TASK_SYSTEM]; const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall; selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT; selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); -#endif -#if defined(SCHEDULER_DEBUG) - DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs); -#endif } } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 98aadbcb44..92118eef55 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -19,8 +19,6 @@ #include "common/time.h" -//#define SCHEDULER_DEBUG - typedef enum { TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle TASK_PRIORITY_LOW = 1, @@ -149,10 +147,8 @@ typedef struct { /* Statistics */ timeUs_t movingSumExecutionTime; // moving sum over 32 samples -#ifndef SKIP_TASK_STATISTICS timeUs_t maxExecutionTime; timeUs_t totalExecutionTime; // total time consumed by task since boot -#endif } cfTask_t; extern cfTask_t cfTasks[TASK_COUNT]; From 43bdb0f0572e0b18d9efe72021931ba8af7c65bf Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 15 Jul 2022 08:49:34 +0200 Subject: [PATCH 48/60] Fix secondary gyro --- src/main/sensors/gyro.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 34c3eaef39..dd093fc4ae 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -460,7 +460,7 @@ void FAST_CODE NOINLINE gyroFilter() * This happens on the first frequency with the biggest aplitude */ DEBUG_SET(DEBUG_ALWAYS, axis, gyroADCf); - secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); + gyroADCf = secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); DEBUG_SET(DEBUG_ALWAYS, axis + 3, gyroADCf); #endif From f346c57f1c00505115b8f1aee16414814cbd5689 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 17 Jul 2022 13:12:26 +0300 Subject: [PATCH 49/60] reverted wind direction icon side change --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e8ab2753ea..8a7584fced 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2802,7 +2802,7 @@ static bool osdDrawSingleElement(uint8_t item) if (valid) { uint16_t angle; horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle); - int16_t windDirection = osdGetHeadingAngle( DECIDEGREES_TO_DEGREES(attitude.values.yaw) - CENTIDEGREES_TO_DEGREES((int)angle) + 22); + int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22); buff[1] = SYM_DIRECTION + (windDirection*2 / 90); } else { horizontalWindSpeed = 0; From a54e501d439af0e4e0b20da8a8940f603df736dd Mon Sep 17 00:00:00 2001 From: PWN-GH Date: Tue, 19 Jul 2022 05:08:30 -0700 Subject: [PATCH 50/60] Added checkout with a tag instead of a branch Changed default git command to checkout with a tag instead of a branch, but kept the old command as a comment. (i.e. release_5.0.0 branch does not exist) Added where to find tags (Just in case). Updated version numbers of iNav and ARM toolchain. --- ...Building in Windows 10 or 11 with MSYS2.md | 22 +++++++++++++------ 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/docs/development/Building in Windows 10 or 11 with MSYS2.md b/docs/development/Building in Windows 10 or 11 with MSYS2.md index 2593e25c61..b561a9baa5 100644 --- a/docs/development/Building in Windows 10 or 11 with MSYS2.md +++ b/docs/development/Building in Windows 10 or 11 with MSYS2.md @@ -38,9 +38,17 @@ cd /c/Workspace # you can also check out your own fork here which makes contributing easier git clone https://github.com/iNavFlight/inav cd inav -# switch to release you want or skip next 2 lines if you want latest +``` + +(Optional) Switch to a release +``` +# switch to a release you want or skip next 2 lines if you want latest git fetch origin -git checkout -b release_2.6.1 origin/release_2.6.1 +# tags/5.0.0 is the release tag, local_5.0.0 is the name of a local branch you will create. +# tags can be found on https://github.com/iNavFlight/inav/tags as well as the releases page +git checkout tags/5.0.0 -b local_5.0.0 +# you can also checkout with a branch if applicable: +# git checkout -b release_5.1.0 origin/release_5.1.0 ``` Now create the build and xpack directories and get the toolkit version you need for your inav version ``` @@ -54,13 +62,13 @@ This will give you the version you need for any given release or master branch. https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/ ``` -# for version 2.6.1, version needed is 9.2.1 -wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v9.2.1-1.1/xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip -unzip xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip +# for version 5.0.0, version needed is 10.2.1 +wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v10.2.1-1.1/xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip +unzip xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip ``` This is important, put the toolkit first before your path so that it is picked up ahead of any other versions that may be present on your system ``` -export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-9.2.1-1.1/bin:$PATH +export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-10.2.1-1.1/bin:$PATH cd /c/Workspace/inav/build ``` You may need to run rm -rf * in build directory if you had any failed previous runs now run cmake @@ -68,7 +76,7 @@ You may need to run rm -rf * in build directory if you had any failed previous r # while inside the build directory cmake .. ``` -Once that's done you can compile the firmware for your controller +Once that's done you can compile the firmware for your flight controller ``` make DALRCF405 ``` From 64aebb362cbcd810a53bd94cb45a8240b2253886 Mon Sep 17 00:00:00 2001 From: PWN-GH Date: Tue, 19 Jul 2022 05:13:15 -0700 Subject: [PATCH 51/60] Update Building in Windows 10 or 11 with MSYS2.md --- .../development/Building in Windows 10 or 11 with MSYS2.md | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/docs/development/Building in Windows 10 or 11 with MSYS2.md b/docs/development/Building in Windows 10 or 11 with MSYS2.md index b561a9baa5..00620bbcc4 100644 --- a/docs/development/Building in Windows 10 or 11 with MSYS2.md +++ b/docs/development/Building in Windows 10 or 11 with MSYS2.md @@ -40,11 +40,10 @@ git clone https://github.com/iNavFlight/inav cd inav ``` -(Optional) Switch to a release +(Optional) Switch to a release instead of master ``` -# switch to a release you want or skip next 2 lines if you want latest git fetch origin -# tags/5.0.0 is the release tag, local_5.0.0 is the name of a local branch you will create. +# on the next line, tags/5.0.0 is the release's tag, and local_5.0.0 is the name of a local branch you will create. # tags can be found on https://github.com/iNavFlight/inav/tags as well as the releases page git checkout tags/5.0.0 -b local_5.0.0 # you can also checkout with a branch if applicable: @@ -62,7 +61,7 @@ This will give you the version you need for any given release or master branch. https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/ ``` -# for version 5.0.0, version needed is 10.2.1 +# for iNav version 5.0.0, tookchain version needed is 10.2.1 wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v10.2.1-1.1/xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip unzip xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip ``` From 8deb1054ba8edf6ee909f13149863b9668c96e87 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 23 Jul 2022 12:08:37 +0200 Subject: [PATCH 52/60] Make 3D matrix filter configurable --- src/main/fc/settings.yaml | 16 ++++++ src/main/flight/dynamic_gyro_notch.h | 2 +- .../flight/secondary_dynamic_gyro_notch.c | 51 +++++++++++++++---- src/main/sensors/gyro.c | 4 +- src/main/sensors/gyro.h | 13 +++++ 5 files changed, 73 insertions(+), 13 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 07a093bb4d..b9b078b81f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -187,6 +187,9 @@ tables: - name: rth_trackback_mode values: ["OFF", "ON", "FS"] enum: rthTrackbackMode_e + - name: dynamic_gyro_notch_mode + values: ["2D", "3D_R", "3D_P", "3D_Y", "3D_RP", "3D_RY", "3D_PY", "3D"] + enum: dynamicGyroNotchMode_e constants: RPYL_PID_MIN: 0 @@ -285,6 +288,19 @@ groups: condition: USE_DYNAMIC_FILTERS min: 30 max: 250 + - name: dynamic_gyro_notch_mode + description: "Gyro dynamic notch type" + default_value: "2D" + table: dynamic_gyro_notch_mode + field: dynamicGyroNotchMode + condition: USE_DYNAMIC_FILTERS + - name: dynamic_gyro_notch_3d_q + description: "Q factor for 3D dynamic notches" + default_value: 200 + field: dynamicGyroNotch3dQ + condition: USE_DYNAMIC_FILTERS + min: 1 + max: 1000 - name: gyro_to_use condition: USE_DUAL_GYRO min: 0 diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h index b8adbfa0ea..b3337da921 100644 --- a/src/main/flight/dynamic_gyro_notch.h +++ b/src/main/flight/dynamic_gyro_notch.h @@ -31,7 +31,7 @@ #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350 /* - * Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters + * Number of peaks to detect with Dynamic Notch Filter aka Matrix Filter. This is equal to the number of dynamic notch filters */ #define DYN_NOTCH_PEAK_COUNT 3 typedef struct dynamicGyroNotchState_s { diff --git a/src/main/flight/secondary_dynamic_gyro_notch.c b/src/main/flight/secondary_dynamic_gyro_notch.c index cf6360c46b..b5070a9f22 100644 --- a/src/main/flight/secondary_dynamic_gyro_notch.c +++ b/src/main/flight/secondary_dynamic_gyro_notch.c @@ -42,21 +42,50 @@ void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *stat state->filtersApplyFn[axis] = nullFilterApply; } - state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; - state->enabled = gyroConfig()->dynamicGyroNotchEnabled; + state->dynNotchQ = gyroConfig()->dynamicGyroNotch3dQ / 100.0f; + state->enabled = gyroConfig()->dynamicGyroNotchMode != DYNAMIC_NOTCH_MODE_2D; state->looptime = getLooptime(); - if (state->enabled) { - /* - * Step 1 - init all filters even if they will not be used further down the road + if ( + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_R || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D + ) { + /* + * Enable ROLL filter */ - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - //Any initial notch Q is valid sice it will be updated immediately after - biquadFilterInit(&state->filters[axis], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); - state->filtersApplyFn[axis] = (filterApplyFnPtr)biquadFilterApplyDF1; - } - + biquadFilterInit(&state->filters[FD_ROLL], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[FD_ROLL] = (filterApplyFnPtr)biquadFilterApplyDF1; } + + if ( + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_P || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D + ) { + /* + * Enable PITCH filter + */ + biquadFilterInit(&state->filters[FD_PITCH], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[FD_PITCH] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + + if ( + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_Y || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D + ) { + /* + * Enable YAW filter + */ + biquadFilterInit(&state->filters[FD_YAW], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[FD_YAW] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + + } void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 34c3eaef39..a229571fd6 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -96,7 +96,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -117,6 +117,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT, .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT, .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT, + .dynamicGyroNotchMode = SETTING_DYNAMIC_GYRO_NOTCH_MODE_DEFAULT, + .dynamicGyroNotch3dQ = SETTING_DYNAMIC_GYRO_NOTCH_3D_Q_DEFAULT, #endif #ifdef USE_GYRO_KALMAN .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT, diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index d2c310f1a9..f6a3f71b8f 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -40,6 +40,17 @@ typedef enum { GYRO_FAKE } gyroSensor_e; +typedef enum { + DYNAMIC_NOTCH_MODE_2D = 0, + DYNAMIC_NOTCH_MODE_R, + DYNAMIC_NOTCH_MODE_P, + DYNAMIC_NOTCH_MODE_Y, + DYNAMIC_NOTCH_MODE_RP, + DYNAMIC_NOTCH_MODE_RY, + DYNAMIC_NOTCH_MODE_PY, + DYNAMIC_NOTCH_MODE_3D +} dynamicGyroNotchMode_e; + typedef struct gyro_s { bool initialized; uint32_t targetLooptime; @@ -69,6 +80,8 @@ typedef struct gyroConfig_s { uint16_t dynamicGyroNotchQ; uint16_t dynamicGyroNotchMinHz; uint8_t dynamicGyroNotchEnabled; + uint8_t dynamicGyroNotchMode; + uint16_t dynamicGyroNotch3dQ; #endif #ifdef USE_GYRO_KALMAN uint16_t kalman_q; From 408123ba80a05edc88474303f2c0901d69b5ea69 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 23 Jul 2022 12:17:29 +0200 Subject: [PATCH 53/60] Docs update --- docs/Settings.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index af0ea39fd7..ac854bd979 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -752,6 +752,16 @@ Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, --- +### dynamic_gyro_notch_3d_q + +Q factor for 3D dynamic notches + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 1 | 1000 | + +--- + ### dynamic_gyro_notch_enabled Enable/disable dynamic gyro notch also known as Matrix Filter @@ -772,6 +782,16 @@ Minimum frequency for dynamic notches. Default value of `150` works best with 5" --- +### dynamic_gyro_notch_mode + +Gyro dynamic notch type + +| Default | Min | Max | +| --- | --- | --- | +| 2D | | | + +--- + ### dynamic_gyro_notch_q Q factor for dynamic notches From b48adf56b90d24e302b85b61791648c241771d9a Mon Sep 17 00:00:00 2001 From: PWN-GH Date: Sun, 24 Jul 2022 00:48:05 -0700 Subject: [PATCH 54/60] Replaced iNav with INAV and correct typos --- .../Building in Windows 10 or 11 with MSYS2.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/development/Building in Windows 10 or 11 with MSYS2.md b/docs/development/Building in Windows 10 or 11 with MSYS2.md index 00620bbcc4..7ab7b05c93 100644 --- a/docs/development/Building in Windows 10 or 11 with MSYS2.md +++ b/docs/development/Building in Windows 10 or 11 with MSYS2.md @@ -1,6 +1,6 @@ # General Info -This is a guide on how to use Windows MSYS2 distribution and building platform to build iNav firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com +This is a guide on how to use Windows MSYS2 distribution and building platform to build INAV firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com Some of those benefits are described here: @@ -49,7 +49,7 @@ git checkout tags/5.0.0 -b local_5.0.0 # you can also checkout with a branch if applicable: # git checkout -b release_5.1.0 origin/release_5.1.0 ``` -Now create the build and xpack directories and get the toolkit version you need for your inav version +Now create the build and xpack directories and get the toolkit version you need for your INAV version ``` mkdir build cd build @@ -61,7 +61,7 @@ This will give you the version you need for any given release or master branch. https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/ ``` -# for iNav version 5.0.0, tookchain version needed is 10.2.1 +# for INAV version 5.0.0, toolchain version needed is 10.2.1 wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v10.2.1-1.1/xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip unzip xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip ``` @@ -79,9 +79,9 @@ Once that's done you can compile the firmware for your flight controller ``` make DALRCF405 ``` -To get a list of available targets in iNav, see the target src folder +To get a list of available targets in INAV, see the target src folder https://github.com/tednv/inav/tree/master/src/main/target The generated hex file will be in /c/Workspace/inav/build folder -At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building iNav 2.1 and 2.2, and now I'm getting back into it so figured I would share my method +At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building INAV 2.1 and 2.2, and now I'm getting back into it so figured I would share my method From 3df49a704fa4bb99206f8b8717174c94abbe3e1b Mon Sep 17 00:00:00 2001 From: PWN-GH Date: Sun, 24 Jul 2022 00:51:27 -0700 Subject: [PATCH 55/60] Replaced targets link to official INAV repo Replaced targets link from https://github.com/tednv/inav/tree/master/src/main/target to official INAV repo at https://github.com/inavflight/inav/tree/master/src/main/target --- docs/development/Building in Windows 10 or 11 with MSYS2.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/Building in Windows 10 or 11 with MSYS2.md b/docs/development/Building in Windows 10 or 11 with MSYS2.md index 7ab7b05c93..1df77913b3 100644 --- a/docs/development/Building in Windows 10 or 11 with MSYS2.md +++ b/docs/development/Building in Windows 10 or 11 with MSYS2.md @@ -80,7 +80,7 @@ Once that's done you can compile the firmware for your flight controller make DALRCF405 ``` To get a list of available targets in INAV, see the target src folder -https://github.com/tednv/inav/tree/master/src/main/target +[https://github.com/tednv/inav/tree/master/src/main/target](https://github.com/inavflight/inav/tree/master/src/main/target) The generated hex file will be in /c/Workspace/inav/build folder From e37d5088587c07296a473acd4c7dc1360f52f86a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 24 Jul 2022 11:01:01 +0200 Subject: [PATCH 56/60] Remove not needed debug --- src/main/sensors/gyro.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 827a5b4ebe..bb81200b20 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -461,9 +461,7 @@ void FAST_CODE NOINLINE gyroFilter() * In some cases, noise amplitude is high enough not to be filtered by the primary filter. * This happens on the first frequency with the biggest aplitude */ - DEBUG_SET(DEBUG_ALWAYS, axis, gyroADCf); gyroADCf = secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); - DEBUG_SET(DEBUG_ALWAYS, axis + 3, gyroADCf); #endif From 51ba70294270699d5ca31ea66b7ee80a2f21e71a Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 30 Jul 2022 10:12:12 +0100 Subject: [PATCH 57/60] Update PPM --- docs/Rx.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/Rx.md b/docs/Rx.md index 6a0603410a..df1ed532a3 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -9,6 +9,8 @@ There are 2 basic types of receivers: ## PPM Receivers +**Only supported in inav 3.x and below** + PPM is sometimes known as PPM SUM or CPPM. 12 channels via a single input pin, not as accurate or jitter free as methods that use serial communications, but readily available. From f8a4dc91ed06fc2d0e23f3fd78cca28a01fb25eb Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 1 Aug 2022 08:44:10 +0100 Subject: [PATCH 58/60] Updated profiles.md doc Updated profiles.md doc to cover how to stop profiles automatically changing while editing. --- docs/Profiles.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/docs/Profiles.md b/docs/Profiles.md index aac3438aad..9495342996 100644 --- a/docs/Profiles.md +++ b/docs/Profiles.md @@ -36,6 +36,12 @@ Or using the speed to change profiles. In this example: [![Using speed to change profiles](https://i.imgur.com/WjkuhhWl.png)](https://i.imgur.com/WjkuhhW.png) +#### Configurator use with profile changing logic. + +If you have logic conditions that change the profiles. You may find that if you manually change the profile using the drop down boxes in the top right of Configurator; that they switch back to a different profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `set profile` operations. Remember to re-enable these switches after you have made your changes. + +[![Disabled SET PROFILE switches](https://i.imgur.com/AeH9ll7l.png)](https://i.imgur.com/AeH9ll7.png) + ## Profile Contents The values contained within a profile can be seen by using the CLI `dump profile` command. From da521b0fdeb47d6d8de893fdb58622733a381e57 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Mon, 1 Aug 2022 17:56:14 +0200 Subject: [PATCH 59/60] Add USER3 mode (#8276) --- src/main/fc/fc_msp_box.c | 3 +++ src/main/fc/rc_modes.h | 1 + src/main/io/piniobox.h | 1 + src/main/programming/logic_condition.c | 6 +++++- src/main/programming/logic_condition.h | 1 + 5 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 49fc107eda..57ba288f70 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -86,6 +86,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXBRAKING, .boxName = "MC BRAKING", .permanentId = 46 }, { .boxId = BOXUSER1, .boxName = "USER1", .permanentId = BOX_PERMANENT_ID_USER1 }, { .boxId = BOXUSER2, .boxName = "USER2", .permanentId = BOX_PERMANENT_ID_USER2 }, + { .boxId = BOXUSER3, .boxName = "USER3", .permanentId = BOX_PERMANENT_ID_USER3 }, { .boxId = BOXLOITERDIRCHN, .boxName = "LOITER CHANGE", .permanentId = 49 }, { .boxId = BOXMSPRCOVERRIDE, .boxName = "MSP RC OVERRIDE", .permanentId = 50 }, { .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 51 }, @@ -313,6 +314,7 @@ void initActiveBoxIds(void) // USER modes are only used for PINIO at the moment ADD_ACTIVE_BOX(BOXUSER1); ADD_ACTIVE_BOX(BOXUSER2); + ADD_ACTIVE_BOX(BOXUSER3); #endif #if defined(USE_OSD) && defined(OSD_LAYOUT_COUNT) @@ -390,6 +392,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER3)), BOXUSER3); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)), BOXLOITERDIRCHN); #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 16f9d70d76..e72e18a84e 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -74,6 +74,7 @@ typedef enum { BOXAUTOLEVEL = 45, BOXPLANWPMISSION = 46, BOXSOARING = 47, + BOXUSER3 = 48, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/piniobox.h b/src/main/io/piniobox.h index 638fd04b4e..a74f77cfe8 100644 --- a/src/main/io/piniobox.h +++ b/src/main/io/piniobox.h @@ -29,6 +29,7 @@ #define BOX_PERMANENT_ID_USER1 47 #define BOX_PERMANENT_ID_USER2 48 +#define BOX_PERMANENT_ID_USER3 49 #define BOX_PERMANENT_ID_NONE 255 // A permanent ID for no box mode diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 8b13cd8cf2..6649796211 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -683,6 +683,10 @@ static int logicConditionGetFlightModeOperandValue(int operand) { return IS_RC_MODE_ACTIVE(BOXUSER2); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3: + return IS_RC_MODE_ACTIVE(BOXUSER3); + break; + default: return 0; break; @@ -861,4 +865,4 @@ bool isFlightAxisRateOverrideActive(uint8_t axis) { } else { return false; } -} \ No newline at end of file +} diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 7c67cdc639..0025602984 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -147,6 +147,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1, // 9 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2, // 10 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD, // 11 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3, // 12 } logicFlightModeOperands_e; typedef enum { From 359030d8c569bd49d48b2d9468fe4c1947e62df3 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Mon, 1 Aug 2022 18:38:04 +0200 Subject: [PATCH 60/60] Add target: NEUTRONRCH7BT (#8277) --- src/main/target/NEUTRONRCH7BT/CMakeLists.txt | 1 + src/main/target/NEUTRONRCH7BT/config.c | 35 ++++ src/main/target/NEUTRONRCH7BT/target.c | 43 +++++ src/main/target/NEUTRONRCH7BT/target.h | 192 +++++++++++++++++++ 4 files changed, 271 insertions(+) create mode 100644 src/main/target/NEUTRONRCH7BT/CMakeLists.txt create mode 100644 src/main/target/NEUTRONRCH7BT/config.c create mode 100644 src/main/target/NEUTRONRCH7BT/target.c create mode 100644 src/main/target/NEUTRONRCH7BT/target.h diff --git a/src/main/target/NEUTRONRCH7BT/CMakeLists.txt b/src/main/target/NEUTRONRCH7BT/CMakeLists.txt new file mode 100644 index 0000000000..5849c3c6e2 --- /dev/null +++ b/src/main/target/NEUTRONRCH7BT/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(NEUTRONRCH7BT) diff --git a/src/main/target/NEUTRONRCH7BT/config.c b/src/main/target/NEUTRONRCH7BT/config.c new file mode 100644 index 0000000000..c5bf93db09 --- /dev/null +++ b/src/main/target/NEUTRONRCH7BT/config.c @@ -0,0 +1,35 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP; + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3; + // compassConfigMutable()->mag_align = XXX; +} diff --git a/src/main/target/NEUTRONRCH7BT/target.c b/src/main/target/NEUTRONRCH7BT/target.c new file mode 100644 index 0000000000..b65dc90634 --- /dev/null +++ b/src/main/target/NEUTRONRCH7BT/target.c @@ -0,0 +1,43 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NEUTRONRCH7BT/target.h b/src/main/target/NEUTRONRCH7BT/target.h new file mode 100644 index 0000000000..a8ca94f3fa --- /dev/null +++ b/src/main/target/NEUTRONRCH7BT/target.h @@ -0,0 +1,192 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "NH7BT" +#define USBD_PRODUCT_STRING "NeuronRC H7 BT" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED + +// *************** IMU generic *********************** +// #define USE_DUAL_GYRO +// #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL + +// *************** SPI1 IMU0 BMI270 ***************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_IMU_BMI270 + +#define IMU_BMI270_ALIGN CW270_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PC15 +#define BMI270_EXTI_PIN PB2 + +// *************** SPI4 IMU1 BMI270 ***************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +// #define USE_IMU_BMI270 + +// #define IMU_BMI270_ALIGN CW0_DEG_FLIP +// #define BMI270_SPI_BUS BUS_SPI4 +// #define BMI270_CS_PIN PE11 +// #define BMI270_EXTI_PIN PE15 + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 // I2C1 is the broken out I2C bus +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 // I2C2 is not broken out +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +// UART3 is connected to the BT chip +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 // VBAT1 +#define ADC_CHANNEL_2_PIN PC1 // CURR1 +#define ADC_CHANNEL_3_PIN PC4 // AirS +#define ADC_CHANNEL_4_PIN PA4 // VBAT2 +#define ADC_CHANNEL_5_PIN PA7 // CURR2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 // Vsw control +#define PINIO2_PIN PD11 // Cam1/Cam2 switch control +#define PINIO3_PIN PC3 // BT Mode + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#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 MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT +#define USE_ESC_SENSOR