diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index db04999587..98291b5deb 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -134,7 +134,7 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){ rMat[2][1] = 2.0f * (qP.yz - -qP.wx); rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy; -#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) && !defined(SET_IMU_FROM_EULER) +#if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) && !defined(SET_IMU_FROM_EULER) rMat[1][0] = -2.0f * (qP.xy - -qP.wz); rMat[2][0] = -2.0f * (qP.xz + -qP.wy); #endif @@ -472,7 +472,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) } #endif -#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) +#if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) UNUSED(imuMahonyAHRSupdate); UNUSED(imuIsAccelerometerHealthy); UNUSED(useAcc); diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 5cf455ca50..dc3f000b94 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -1094,7 +1094,7 @@ static void cliSerial(char *cmdline) } -#ifndef SKIP_SERIAL_PASSTHROUGH +#if defined(USE_SERIAL_PASSTHROUGH) #ifdef USE_PINIO static void cbCtrlLine(void *context, uint16_t ctrl) { @@ -3608,7 +3608,7 @@ static void cliStatus(char *cmdline) cliPrintLinefeed(); } -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) static void cliTasks(char *cmdline) { UNUSED(cmdline); @@ -4522,7 +4522,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), #endif CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial), -#ifndef SKIP_SERIAL_PASSTHROUGH +#if defined(USE_SERIAL_PASSTHROUGH) CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] [DTR PINIO]: passthrough to serial", cliSerialPassthrough), #endif #ifdef USE_SERVOS @@ -4539,7 +4539,7 @@ const clicmd_t cmdTable[] = { "\treverse r|n", cliServoMix), #endif CLI_COMMAND_DEF("status", "show status", NULL, cliStatus), -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks), #endif #ifdef USE_TIMER_MGMT diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 4d5d41d689..a1445c928a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -1064,7 +1064,7 @@ const clivalue_t valueTable[] = { #endif // PG_SYSTEM_CONFIG -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) }, #endif { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) }, diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 58e49e0633..18ae74665d 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -526,7 +526,7 @@ static void showSensorsPage(void) } -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) static void showTasksPage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; @@ -576,7 +576,7 @@ static const pageEntry_t pages[PAGE_COUNT] = { { PAGE_RX, "RX", showRxPage, PAGE_FLAGS_NONE }, { PAGE_BATTERY, "BATTERY", showBatteryPage, PAGE_FLAGS_NONE }, { PAGE_SENSORS, "SENSORS", showSensorsPage, PAGE_FLAGS_NONE }, -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) { PAGE_TASKS, "TASKS", showTasksPage, PAGE_FLAGS_NONE }, #endif #ifdef ENABLE_DEBUG_DASHBOARD_PAGE diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h index 2212b84d22..7d5d4dd18d 100644 --- a/src/main/io/dashboard.h +++ b/src/main/io/dashboard.h @@ -43,7 +43,7 @@ typedef enum { PAGE_SENSORS, PAGE_RX, PAGE_PROFILE, -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) PAGE_TASKS, #endif #ifdef USE_GPS diff --git a/src/main/io/serial.c b/src/main/io/serial.c index f0612f783c..c5379e6c72 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -505,7 +505,7 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort) }; } -#if defined(USE_GPS) || ! defined(SKIP_SERIAL_PASSTHROUGH) +#if defined(USE_GPS) || defined(USE_SERIAL_PASSTHROUGH) // Default data consumer for serialPassThrough. static void nopConsumer(uint8_t data) { diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 8a9f277db9..a3caf4a392 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -138,7 +138,7 @@ void taskSystemLoad(timeUs_t currentTimeUs) #endif } -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) #define MOVING_SUM_COUNT 32 timeUs_t checkFuncMaxExecutionTime; timeUs_t checkFuncTotalExecutionTime; @@ -206,9 +206,7 @@ void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse) void schedulerResetTaskStatistics(cfTaskId_e taskId) { -#ifdef SKIP_TASK_STATISTICS - UNUSED(taskId); -#else +#if defined(USE_TASK_STATISTICS) if (taskId == TASK_SELF) { currentTask->movingSumExecutionTime = 0; currentTask->totalExecutionTime = 0; @@ -218,19 +216,21 @@ void schedulerResetTaskStatistics(cfTaskId_e taskId) cfTasks[taskId].totalExecutionTime = 0; cfTasks[taskId].maxExecutionTime = 0; } +#else + UNUSED(taskId); #endif } void schedulerResetTaskMaxExecutionTime(cfTaskId_e taskId) { -#ifdef SKIP_TASK_STATISTICS - UNUSED(taskId); -#else +#if defined(USE_TASK_STATISTICS) if (taskId == TASK_SELF) { currentTask->maxExecutionTime = 0; } else if (taskId < TASK_COUNT) { cfTasks[taskId].maxExecutionTime = 0; } +#else + UNUSED(taskId); #endif } @@ -279,7 +279,7 @@ FAST_CODE void scheduler(void) #if defined(SCHEDULER_DEBUG) DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall); #endif -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) if (calculateTaskStatistics) { const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall; checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT; @@ -328,9 +328,7 @@ FAST_CODE void scheduler(void) selectedTask->dynamicPriority = 0; // Execute task -#ifdef SKIP_TASK_STATISTICS - selectedTask->taskFunc(currentTimeUs); -#else +#if defined(USE_TASK_STATISTICS) if (calculateTaskStatistics) { const timeUs_t currentTimeBeforeTaskCall = micros(); selectedTask->taskFunc(currentTimeBeforeTaskCall); @@ -338,11 +336,12 @@ FAST_CODE void scheduler(void) selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT; selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); - } else { + } else +#endif + { selectedTask->taskFunc(currentTimeUs); } -#endif #if defined(SCHEDULER_DEBUG) DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler } else { diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 477d7cd6be..ff6509a6bf 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -156,7 +156,7 @@ typedef struct { timeUs_t lastExecutedAt; // last time of invocation timeUs_t lastSignaledAt; // time of invocation event for event-driven tasks -#ifndef SKIP_TASK_STATISTICS +#if defined(USE_TASK_STATISTICS) // Statistics timeUs_t movingSumExecutionTime; // moving sum over 32 samples timeUs_t maxExecutionTime; diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 9153e95ac9..7ea23e995f 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -108,7 +108,6 @@ #undef USE_MAG #ifdef CC3D_OPBL -#define SKIP_CLI_COMMAND_HELP //#undef USE_SERVOS #undef USE_BARO #undef USE_RANGEFINDER diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index bd4360ab45..af0e65110f 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -103,8 +103,6 @@ #ifdef USE_SERIAL_RX #undef USE_SERIAL_RX #endif -//#undef SKIP_TASK_STATISTICS - #else #define DEFAULT_RX_FEATURE FEATURE_RX_PPM @@ -114,7 +112,7 @@ #define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP -#define SKIP_SERIAL_PASSTHROUGH +#undef USE_SERIAL_PASSTHROUGH #undef USE_CLI // Since the CJMCU PCB has holes for 4 motors in each corner we can save same flash space by disabling support for other mixers. diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 976056f112..dfeccecb8c 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -114,7 +114,7 @@ void updateState(const fdm_packet* pkt) { fakeGyroSet(fakeGyroDev, x, y, z); // printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]); -#if defined(SKIP_IMU_CALC) +#if !defined(USE_IMU_CALC) #if defined(SET_IMU_FROM_EULER) // set from Euler double qw = pkt->imu_orientation_quat[0]; diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index da1daa596b..ee83d961be 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -33,7 +33,7 @@ // use simulatior's attitude directly // disable this if wants to test AHRS algorithm -#define SKIP_IMU_CALC +#undef USE_IMU_CALC //#define SIMULATOR_ACC_SYNC //#define SIMULATOR_GYRO_SYNC diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 49dd029380..5969883f8a 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -124,7 +124,10 @@ #define USE_BRUSHED_ESC_AUTODETECT // Detect if brushed motors are connected and set defaults appropriately to avoid motors spinning on boot #define USE_CLI +#define USE_SERIAL_PASSTHROUGH +#define USE_TASK_STATISTICS #define USE_GYRO_REGISTER_DUMP // Adds gyroregisters command to cli to dump configured register values +#define USE_IMU_CALC #define USE_PPM #define USE_PWM #define USE_SERIAL_RX