Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

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

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-03-10 20:06:48 +01:00
commit 4fc53b4bcd
15 changed files with 82 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

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

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

2
src/main/config/feature.c
View file

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

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

@ -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 -

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

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

12
src/main/fc/settings.yaml
View file

@ -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
max: 4
min: 1
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

2
src/main/flight/mixer.c
View file

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

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

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

6
src/main/flight/servos.c
View file

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

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

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

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

@ -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

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

@ -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.
* Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°), Signal 0 to 5, Symbol 0 to 480
* Display a POI as a 3D-marker on the hud
* 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 (poiSignal < 5) { // 0 to 4 = signal bars, 5 = No LQ and no heading displayed
error_x = hudWrap360(poiHeading - DECIDEGREES_TO_DEGREES(osdGetHeading()));
if (poiType == 1) { // POI from the ESP radar
error_x = hudWrap360(poiP1 - 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);
}

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

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

4
src/main/navigation/navigation_fw_launch.c
View file

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

2
src/main/navigation/navigation_pos_estimator.c
View file

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