Merge remote-tracking branch 'origin/development' into dzikuvx-experimantal-gcc-optimizations

src/main/cms/cms_menu_osd.c

@@ -378,6 +378,7 @@ static const OSD_Entry menuOsdHud2Entries[] = {
     OSD_SETTING_ENTRY("RADAR MIN RANGE", SETTING_OSD_HUD_RADAR_RANGE_MIN),
     OSD_SETTING_ENTRY("RADAR MAX RANGE", SETTING_OSD_HUD_RADAR_RANGE_MAX),
     OSD_SETTING_ENTRY("RADAR DET. NEAREST", SETTING_OSD_HUD_RADAR_NEAREST),
+    OSD_SETTING_ENTRY("NEXT WAYPOINTS", SETTING_OSD_HUD_WP_DISP),
     OSD_BACK_ENTRY,
     OSD_END_ENTRY,
 };

src/main/common/maths.c

@@ -159,7 +159,7 @@ int constrain(int amt, int low, int high)
         return amt;
 }
 
-float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
+float constrainf(float amt, float low, float high)
 {
     if (amt < low)
         return low;

src/main/config/feature.c

@@ -34,7 +34,7 @@ bool featureConfigured(uint32_t mask)
     return featureConfig()->enabledFeatures & mask;
 }
 
-bool FAST_CODE NOINLINE feature(uint32_t mask)
+bool feature(uint32_t mask)
 {
     return activeFeaturesLatch & mask;
 }

src/main/drivers/osd_symbols.h

@@ -117,7 +117,7 @@
 #define SYM_3D_MPH                0x8A // 138 MPH 3D
 
 #define SYM_RPM                   0x8B // 139 RPM
-//                                0x8C // 140 -
+#define SYM_WAYPOINT              0x8C // 140 Waypoint
 //                                0x8D // 141 -
 //                                0x8E // 142 -
 //                                0x8F // 143 -

src/main/fc/runtime_config.c

@@ -80,7 +80,7 @@ uint32_t disableFlightMode(flightModeFlags_e mask)
     return flightModeFlags;
 }
 
-bool FAST_CODE NOINLINE sensors(uint32_t mask)
+bool sensors(uint32_t mask)
 {
     return enabledSensors & mask;
 }

src/main/fc/settings.yaml

@@ -202,7 +202,7 @@ groups:
       - name: dynamic_gyro_notch_min_hz
         field: dynamicGyroNotchMinHz
         condition: USE_DYNAMIC_FILTERS
-        min: 60
+        min: 30
         max: 1000
       - name: gyro_to_use
         condition: USE_DUAL_GYRO
@@ -885,7 +885,7 @@ groups:
       - name: rpm_gyro_min_hz
         field: gyro_min_hz
         type: uint8_t
-        min: 50
+        min: 30
         max: 200
       - name: rpm_gyro_q
         field: gyro_q
@@ -1938,8 +1938,8 @@ groups:
         max: 4
       - name: osd_hud_margin_v
         field: hud_margin_v
-        min: 0
+        min: 1
-        max: 4
+        max: 3
       - name: osd_hud_homing
         field: hud_homing
         type: bool
@@ -1962,6 +1962,10 @@ groups:
         field: hud_radar_nearest
         min: 0
         max: 4000
+      - name: osd_hud_wp_disp
+        field: hud_wp_disp
+        min: 0
+        max: 3
       - name: osd_left_sidebar_scroll
         field: left_sidebar_scroll
         table: osd_sidebar_scroll

src/main/flight/mixer.c

@@ -138,7 +138,7 @@ static void computeMotorCount(void)
     }
 }
 
-uint8_t FAST_CODE NOINLINE getMotorCount(void) {
+uint8_t getMotorCount(void) {
     return motorCount;
 }
 

src/main/flight/pid.c

@@ -574,7 +574,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
 }
 
 /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
-static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
+static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
     const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
 
@@ -596,7 +596,7 @@ bool isFixedWingItermLimitActive(float stickPosition)
     return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
 }
 
-static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, float rateError, float dT) {
+static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
     float newPTerm = rateError * pidState->kP;
 
     return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
@@ -940,7 +940,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
 }
 
-void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
+void checkItermLimitingActive(pidState_t *pidState)
 {
     bool shouldActivate;
     if (usedPidControllerType == PID_TYPE_PIFF) {
@@ -953,7 +953,7 @@ void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; 
 }
 
-void FAST_CODE NOINLINE pidController(float dT)
+void FAST_CODE pidController(float dT)
 {
     if (!pidFiltersConfigured) {
         return;
@@ -1119,9 +1119,9 @@ void pidInit(void)
     }
 }
 
-const pidBank_t FAST_CODE NOINLINE * pidBank(void) { 
+const pidBank_t * pidBank(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; 
 }
-pidBank_t FAST_CODE NOINLINE * pidBankMutable(void) { 
+pidBank_t * pidBankMutable(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }

src/main/flight/servos.c

@@ -444,17 +444,17 @@ void processServoAutotrim(void)
     }
 }
 
-bool FAST_CODE NOINLINE isServoOutputEnabled(void)
+bool isServoOutputEnabled(void)
 {
     return servoOutputEnabled;
 }
 
-void NOINLINE setServoOutputEnabled(bool flag)
+void setServoOutputEnabled(bool flag)
 {
     servoOutputEnabled = flag;
 }
 
-bool FAST_CODE NOINLINE isMixerUsingServos(void)
+bool isMixerUsingServos(void)
 {
     return mixerUsesServos;
 }

src/main/io/osd.c

@@ -792,7 +792,7 @@ static const char * navigationStateMessage(void)
             break;
         case MW_NAV_STATE_WP_ENROUTE:
             // TODO: Show WP number
-            return OSD_MESSAGE_STR("EN ROUTE TO WAYPOINT");
+            return OSD_MESSAGE_STR("TO WP");
         case MW_NAV_STATE_PROCESS_NEXT:
             return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
         case MW_NAV_STATE_DO_JUMP:
@@ -1639,13 +1639,14 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "3CRS";
             else if (FLIGHT_MODE(NAV_CRUISE_MODE))
                 p = "CRS ";
+            else if (FLIGHT_MODE(NAV_WP_MODE))
+                p = " WP ";
             else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
                 // If navigationRequiresAngleMode() returns false when ALTHOLD is active,
                 // it means it can be combined with ANGLE, HORIZON, ACRO, etc...
                 // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
                 p = " AH ";
-            } else if (FLIGHT_MODE(NAV_WP_MODE))
+            }
-                p = " WP ";
             else if (FLIGHT_MODE(ANGLE_MODE))
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
@@ -1712,14 +1713,18 @@ static bool osdDrawSingleElement(uint8_t item)
 
         if (STATE(GPS_FIX) && isImuHeadingValid()) {
 
-            if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0) {
+            if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
                     osdHudClear();
             }
 
+            // -------- POI : Home point
+
             if (osdConfig()->hud_homepoint) { // Display the home point (H)
-                osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, 5, SYM_HOME);
+                osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0);
             }
 
+            // -------- POI : Nearby aircrafts from ESP32 radar
+
             if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar
                 for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) {
                     if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped
@@ -1730,7 +1735,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) {
                             radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In °
                             radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100;
-                            osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, radar_pois[i].heading, radar_pois[i].lq, 65 + i);
+                            osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq);
                         }
                     }
                 }
@@ -1742,6 +1747,29 @@ static bool osdDrawSingleElement(uint8_t item)
                     }
                 }
             }
+
+            // -------- POI : Next waypoints from navigation
+
+            if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints
+                gpsLocation_t wp2;
+                int j;
+
+                tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount);
+                displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff);
+
+                for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
+                    j = posControl.activeWaypointIndex + i;
+                    if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) {
+                        wp2.lat = posControl.waypointList[j].lat;
+                        wp2.lon = posControl.waypointList[j].lon;
+                        wp2.alt = posControl.waypointList[j].alt;
+                        fpVector3_t poi;
+                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
+                        while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
+                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
+                    }
+                }
+            }
         }
 
         return true;
@@ -2714,9 +2742,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->hud_homing = 0;
     osdConfig->hud_homepoint = 0;
     osdConfig->hud_radar_disp = 0;
-    osdConfig->hud_radar_range_min = 10;
+    osdConfig->hud_radar_range_min = 3;
     osdConfig->hud_radar_range_max = 4000;
     osdConfig->hud_radar_nearest = 0;
+    osdConfig->hud_wp_disp = 0;
     osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
     osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
     osdConfig->sidebar_scroll_arrows = 0;

src/main/io/osd.h

@@ -231,6 +231,7 @@ typedef struct osdConfig_s {
     uint16_t hud_radar_range_min;
     uint16_t hud_radar_range_max;
     uint16_t hud_radar_nearest;
+    uint8_t hud_wp_disp;
     
     uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e
     uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e

src/main/io/osd_hud.c

@@ -36,7 +36,7 @@
 
 #ifdef USE_OSD
 
-#define HUD_DRAWN_MAXCHARS 40 // 6 POI (1 home, 5 radar) x 7 chars max for each minus 2 for H (no icons for heading and signal)
+#define HUD_DRAWN_MAXCHARS 54 // 8 POI (1 home, 4 radar, 3 WP) x 7 chars max for each, minus 2 for H
 
 static int8_t hud_drawn[HUD_DRAWN_MAXCHARS][2];
 static int8_t hud_drawn_pt;
@@ -113,16 +113,20 @@ int8_t radarGetNearestPOI(void)
 }
 
 /*
- * Display one POI on the hud, centered on crosshair position.
+ * Display a POI as a 3D-marker on the hud
- * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°), Signal 0 to 5, Symbol 0 to 480
+ * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°),
+ * Type = 0 : Home point
+ * Type = 1 : Radar POI, P1: Heading, P2: Signal
+ * Type = 2 : Waypoint, P1: WP number, P2: 1=WP+1, 2=WP+2, 3=WP+3
  */
-void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol)
+void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2)
 {
     int poi_x;
     int poi_y;
     uint8_t center_x;
     uint8_t center_y;
     bool poi_is_oos = 0;
+    char buff[3];
 
     uint8_t minX = osdConfig()->hud_margin_h + 2;
     uint8_t maxX = osdGetDisplayPort()->cols - osdConfig()->hud_margin_h - 3;
@@ -161,10 +165,10 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
         uint16_t c;
 
         poi_x = (error_x > 0 ) ? maxX : minX;
-        poi_y = center_y;
+        poi_y = center_y - 1;
 
         if (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK) {
-            poi_y = center_y - 2;
+            poi_y = center_y - 3;
             while (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK && poi_y < maxY - 3) { // Stacks the out-of-sight POI from top to bottom
                 poi_y += 2;
             }
@@ -180,21 +184,22 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
         }
     }
 
-    // POI marker (A B C ...)
+    // Markers
 
     osdHudWrite(poi_x, poi_y, poiSymbol, 1);
 
-    // Signal on the right, heading on the left
+    if (poiType == 1) { // POI from the ESP radar
-
+        error_x = hudWrap360(poiP1 - DECIDEGREES_TO_DEGREES(osdGetHeading()));
-    if (poiSignal < 5) { // 0 to 4 = signal bars, 5 = No LQ and no heading displayed
-        error_x = hudWrap360(poiHeading - DECIDEGREES_TO_DEGREES(osdGetHeading()));
         osdHudWrite(poi_x - 1, poi_y, SYM_DIRECTION + ((error_x + 22) / 45) % 8, 1);
-        osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiSignal, 1);
+        osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiP2, 1);
     }
+    else if (poiType == 2) { // Waypoint,
+        osdHudWrite(poi_x - 1, poi_y, SYM_HUD_ARROWS_U1 + poiP2, 1);
+        osdHudWrite(poi_x + 1, poi_y, poiP1, 1);
+        }
 
     // Distance
 
-    char buff[3];
     if ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) {
         osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3);
     }

src/main/io/osd_hud.h

@@ -23,7 +23,7 @@
 void osdHudClear(void);
 void osdHudDrawCrosshair(uint8_t px, uint8_t py);
 void osdHudDrawHoming(uint8_t px, uint8_t py);
-void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol);
+void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2);
 void osdHudDrawExtras(uint8_t poi_id);
 int8_t radarGetNearestPOI(void);
 

src/main/navigation/navigation_fw_launch.c

@@ -106,7 +106,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs)
     launchState.motorControlAllowed = false;
 }
 
-bool FAST_CODE isFixedWingLaunchDetected(void)
+bool isFixedWingLaunchDetected(void)
 {
     return launchState.launchDetected;
 }
@@ -117,7 +117,7 @@ void enableFixedWingLaunchController(timeUs_t currentTimeUs)
     launchState.motorControlAllowed = true;
 }
 
-bool FAST_CODE isFixedWingLaunchFinishedOrAborted(void)
+bool isFixedWingLaunchFinishedOrAborted(void)
 {
     return launchState.launchFinished;
 }

src/main/navigation/navigation_pos_estimator.c

@@ -823,7 +823,7 @@ void initializePositionEstimator(void)
  * Update estimator
  *  Update rate: loop rate (>100Hz)
  */
-void FAST_CODE NOINLINE updatePositionEstimator(void)
+void updatePositionEstimator(void)
 {
     static bool isInitialized = false;