Merge branch 'iNavFlight:master' into update_building_with_msys2

src/main/common/log.h

@@ -47,45 +47,39 @@ 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
 #define LOG_E(...)
 #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
 #define LOG_W(...)
 #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
 #define LOG_I(...)
 #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
 #define LOG_V(...)
 #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

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)
 {
@@ -3479,7 +3473,6 @@ static void cliStatus(char *cmdline)
     }
 }
 
-#ifndef SKIP_TASK_STATISTICS
 static void cliTasks(char *cmdline)
 {
     UNUSED(cmdline);
@@ -3508,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)
 {
@@ -3541,7 +3533,6 @@ static void cliMemory(char *cmdline)
     }
 }
 
-#if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES)
 static void cliResource(char *cmdline)
 {
     UNUSED(cmdline);
@@ -3560,7 +3551,6 @@ static void cliResource(char *cmdline)
         }
     }
 }
-#endif
 
 static void backupConfigs(void)
 {
@@ -3907,16 +3897,12 @@ 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, "[<index>]\r\n", cliPlaySound),
-#endif
     CLI_COMMAND_DEF("profile", "change profile",
         "[<index>]", cliProfile),
     CLI_COMMAND_DEF("battery_profile", "change battery profile",
         "[<index>]", 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),
@@ -3951,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

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

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

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( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
+                buff[1] = SYM_DIRECTION + (windDirection*2 / 90);
             } else {
                 horizontalWindSpeed = 0;
                 buff[1] = SYM_BLANK;
@@ -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;

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

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

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

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