diff --git a/Makefile b/Makefile index 4dde8598f6..eafe82f7ad 100644 --- a/Makefile +++ b/Makefile @@ -806,7 +806,6 @@ ifeq ($(OSD_SLAVE),yes) TARGET_FLAGS := -DUSE_OSD_SLAVE $(TARGET_FLAGS) COMMON_SRC := $(COMMON_SRC) $(OSD_SLAVE_SRC) $(COMMON_DEVICE_SRC) else -TARGET_FLAGS := -DUSE_FC $(TARGET_FLAGS) COMMON_SRC := $(COMMON_SRC) $(FC_SRC) $(COMMON_DEVICE_SRC) endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d10116b56d..9265bd132e 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -66,7 +66,6 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#ifdef USE_FC #include "flight/altitude.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -74,7 +73,6 @@ #include "flight/navigation.h" #include "flight/pid.h" #include "flight/servos.h" -#endif #include "io/beeper.h" #include "io/gimbal.h" @@ -99,7 +97,7 @@ #include "telemetry/telemetry.h" -#ifdef USE_FC +#ifndef USE_OSD_SLAVE pidProfile_t *currentPidProfile; #endif @@ -121,7 +119,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); -#ifdef USE_FC +#ifndef USE_OSD_SLAVE PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .pidProfileIndex = 0, .activeRateProfile = 0, @@ -459,7 +457,7 @@ void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) ; } -#ifdef USE_FC +#ifndef USE_OSD_SLAVE uint8_t getCurrentPidProfileIndex(void) { return systemConfig()->pidProfileIndex; @@ -495,7 +493,7 @@ void resetConfigs(void) pgActivateProfile(0); -#ifdef USE_FC +#ifndef USE_OSD_SLAVE setPidProfile(0); setControlRateProfile(0); #endif @@ -507,7 +505,7 @@ void resetConfigs(void) void activateConfig(void) { -#ifdef USE_FC +#ifndef USE_OSD_SLAVE generateThrottleCurve(); resetAdjustmentStates(); @@ -531,7 +529,7 @@ void activateConfig(void) void validateAndFixConfig(void) { -#ifdef USE_FC +#ifndef USE_OSD_SLAVE if((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){ motorConfigMutable()->mincommand = 1000; } @@ -607,7 +605,7 @@ void validateAndFixConfig(void) #endif } -#ifdef USE_FC +#ifndef USE_OSD_SLAVE void validateAndFixGyroConfig(void) { // Prevent invalid notch cutoff @@ -687,7 +685,7 @@ void validateAndFixGyroConfig(void) void readEEPROM(void) { -#ifdef USE_FC +#ifndef USE_OSD_SLAVE suspendRxSignal(); #endif @@ -695,7 +693,7 @@ void readEEPROM(void) if (!loadEEPROM()) { failureMode(FAILURE_INVALID_EEPROM_CONTENTS); } -#ifdef USE_FC +#ifndef USE_OSD_SLAVE if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {// sanity check systemConfigMutable()->activeRateProfile = 0; } @@ -710,20 +708,20 @@ void readEEPROM(void) validateAndFixConfig(); activateConfig(); -#ifdef USE_FC +#ifndef USE_OSD_SLAVE resumeRxSignal(); #endif } void writeEEPROM(void) { -#ifdef USE_FC +#ifndef USE_OSD_SLAVE suspendRxSignal(); #endif writeConfigToEEPROM(); -#ifdef USE_FC +#ifndef USE_OSD_SLAVE resumeRxSignal(); #endif } @@ -749,7 +747,7 @@ void saveConfigAndNotify(void) beeperConfirmationBeeps(1); } -#ifdef USE_FC +#ifndef USE_OSD_SLAVE void changePidProfile(uint8_t pidProfileIndex) { if (pidProfileIndex >= MAX_PROFILE_COUNT) { diff --git a/src/main/fc/config.h b/src/main/fc/config.h index d993d96c19..8fcf6ab683 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -64,7 +64,7 @@ typedef enum { #define MAX_NAME_LENGTH 16 -#ifdef USE_FC +#ifndef USE_OSD_SLAVE typedef struct systemConfig_s { uint8_t pidProfileIndex; uint8_t activeRateProfile; diff --git a/src/main/fc/fc_hardfaults.c b/src/main/fc/fc_hardfaults.c index 0468c0048d..5164cc00ff 100644 --- a/src/main/fc/fc_hardfaults.c +++ b/src/main/fc/fc_hardfaults.c @@ -93,7 +93,7 @@ void HardFault_Handler(void) { LED2_ON; -#ifdef USE_FC +#ifndef USE_OSD_SLAVE // fall out of the sky uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredStateForMotors) == requiredStateForMotors) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1cd68ff805..a95c133ff4 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -61,7 +61,6 @@ #include "fc/rc_adjustments.h" #include "fc/runtime_config.h" -#ifdef USE_FC #include "flight/altitude.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -69,7 +68,6 @@ #include "flight/navigation.h" #include "flight/pid.h" #include "flight/servos.h" -#endif #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -90,16 +88,13 @@ #include "msp/msp_protocol.h" #include "msp/msp_serial.h" -#ifdef USE_FC #include "rx/msp.h" #include "rx/rx.h" -#endif #include "scheduler/scheduler.h" #include "sensors/battery.h" -#ifdef USE_FC #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/boardalignment.h" @@ -109,7 +104,6 @@ #include "sensors/sonar.h" #include "telemetry/telemetry.h" -#endif #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -118,7 +112,7 @@ static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; -#ifdef USE_FC +#ifndef USE_OSD_SLAVE static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXARM, "ARM", 0 }, { BOXANGLE, "ANGLE", 1 }, @@ -222,8 +216,8 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr if (mspPostProcessFn) { *mspPostProcessFn = esc4wayProcess; } - break; + #ifdef USE_ESCSERIAL case PROTOCOL_SIMONK: case PROTOCOL_BLHELI: @@ -250,7 +244,7 @@ static void mspRebootFn(serialPort_t *serialPort) { UNUSED(serialPort); -#ifdef USE_FC +#ifndef USE_OSD_SLAVE stopPwmAllMotors(); #endif systemReset(); @@ -259,7 +253,7 @@ static void mspRebootFn(serialPort_t *serialPort) while (true) ; } -#ifdef USE_FC +#ifndef USE_OSD_SLAVE const box_t *findBoxByBoxId(uint8_t boxId) { for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) { @@ -461,21 +455,20 @@ static void serializeSDCardSummaryReply(sbuf_t *dst) switch (afatfs_getFilesystemState()) { case AFATFS_FILESYSTEM_STATE_READY: state = MSP_SDCARD_STATE_READY; - break; + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: if (sdcard_isInitialized()) { state = MSP_SDCARD_STATE_FS_INIT; } else { state = MSP_SDCARD_STATE_CARD_INIT; } - break; + case AFATFS_FILESYSTEM_STATE_FATAL: case AFATFS_FILESYSTEM_STATE_UNKNOWN: default: state = MSP_SDCARD_STATE_FATAL; - break; } } @@ -641,6 +634,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU8(dst, (uint8_t)getBatteryState()); break; } + case MSP_VOLTAGE_METERS: // write out id and voltage meter values, once for each meter we support for (int i = 0; i < supportedVoltageMeterCount; i++) { @@ -693,25 +687,33 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce // that this situation may change and allows us to support configuration of any current sensor with // specialist configuration requirements. - sbufWriteU8(dst, 2); // current meters in payload (adc + virtual) + int currentMeterCount = 1; + +#ifdef USE_VIRTUAL_CURRENT_METER + currentMeterCount++; +#endif + sbufWriteU8(dst, currentMeterCount); const uint8_t adcSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes sbufWriteU8(dst, adcSensorSubframeLength); - sbufWriteU8(dst, CURRENT_METER_ID_BATTERY_1); // the id of the sensor + sbufWriteU8(dst, CURRENT_METER_ID_BATTERY_1); // the id of the meter sbufWriteU8(dst, CURRENT_SENSOR_ADC); // indicate the type of sensor that the next part of the payload is for sbufWriteU16(dst, currentSensorADCConfig()->scale); sbufWriteU16(dst, currentSensorADCConfig()->offset); +#ifdef USE_VIRTUAL_CURRENT_METER const int8_t virtualSensorSubframeLength = 1 + 1 + 2 + 2; // length of id, type, scale, offset, in bytes sbufWriteU8(dst, virtualSensorSubframeLength); - sbufWriteU8(dst, CURRENT_METER_ID_VIRTUAL_1); // the id of the sensor + sbufWriteU8(dst, CURRENT_METER_ID_VIRTUAL_1); // the id of the meter sbufWriteU8(dst, CURRENT_SENSOR_VIRTUAL); // indicate the type of sensor that the next part of the payload is for sbufWriteU16(dst, currentSensorVirtualConfig()->scale); sbufWriteU16(dst, currentSensorVirtualConfig()->offset); +#endif // if we had any other current sensors, this is where we would output any needed configuration break; } + case MSP_BATTERY_CONFIG: sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage); sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage); @@ -721,7 +723,6 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU8(dst, batteryConfig()->currentMeterSource); break; - case MSP_TRANSPONDER_CONFIG: #ifdef TRANSPONDER sbufWriteU8(dst, 1); //Transponder supported @@ -734,7 +735,6 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce break; case MSP_OSD_CONFIG: { - #define OSD_FLAGS_OSD_FEATURE (1 << 0) #define OSD_FLAGS_OSD_SLAVE (1 << 1) #define OSD_FLAGS_RESERVED_1 (1 << 2) @@ -823,7 +823,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro } #endif -#ifdef USE_FC +#ifndef USE_OSD_SLAVE static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) { UNUSED(mspPostProcessFn); @@ -897,6 +897,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU32(dst, servoParams(i)->reversedSources); } break; + case MSP_SERVO_MIX_RULES: for (int i = 0; i < MAX_SERVO_RULES; i++) { sbufWriteU8(dst, customServoMixers(i)->targetChannel); @@ -1375,7 +1376,7 @@ static mspResult_e mspOsdSlaveProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { } #endif -#ifdef USE_FC +#ifndef USE_OSD_SLAVE static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { uint32_t i; @@ -1774,6 +1775,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) GPS_speed = sbufReadU16(src); GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers break; + case MSP_SET_WP: wp_no = sbufReadU8(src); //get the wp number lat = sbufReadU32(src); @@ -1799,6 +1801,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; #endif + case MSP_SET_FEATURE_CONFIG: featureClearAll(); featureSet(sbufReadU32(src)); // features bitmap @@ -2018,10 +2021,12 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentSensorADCConfigMutable()->scale = sbufReadU16(src); currentSensorADCConfigMutable()->offset = sbufReadU16(src); break; +#ifdef USE_VIRTUAL_CURRENT_METER case CURRENT_METER_ID_VIRTUAL_1: currentSensorVirtualConfigMutable()->scale = sbufReadU16(src); currentSensorVirtualConfigMutable()->offset = sbufReadU16(src); break; +#endif default: return -1; @@ -2082,17 +2087,10 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) // !!TODO - replace this with a device independent implementation max7456WriteNvm(addr, font_data); } + break; #else return MSP_RESULT_ERROR; -/* - // just discard the data - sbufReadU8(src); - for (int i = 0; i < 54; i++) { - sbufReadU8(src); - } -*/ #endif - break; #endif // OSD || USE_OSD_SLAVE default: @@ -2119,7 +2117,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { ret = MSP_RESULT_ACK; -#ifdef USE_FC +#ifndef USE_OSD_SLAVE } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { ret = MSP_RESULT_ACK; #endif @@ -2203,7 +2201,7 @@ void mspFcProcessReply(mspPacket_t *reply) /* * Return a pointer to the process command function */ -#ifdef USE_FC +#ifndef USE_OSD_SLAVE void mspFcInit(void) { initActiveBoxIds(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index e3e418ba4d..2002d3899b 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -48,12 +48,10 @@ #include "fc/cli.h" #include "fc/fc_dispatch.h" -#ifdef USE_FC #include "flight/altitude.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" -#endif #include "io/beeper.h" #include "io/dashboard.h" @@ -91,7 +89,7 @@ void taskBstMasterProcess(timeUs_t currentTimeUs); #define TASK_PERIOD_MS(ms) ((ms) * 1000) #define TASK_PERIOD_US(us) (us) -#ifdef USE_FC +#ifndef USE_OSD_SLAVE static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); @@ -133,7 +131,7 @@ void taskBatteryAlerts(timeUs_t currentTimeUs) batteryUpdateAlarms(); } -#ifdef USE_FC +#ifndef USE_OSD_SLAVE static void taskUpdateRxMain(timeUs_t currentTimeUs) { processRx(currentTimeUs); @@ -247,7 +245,7 @@ void osdSlaveTasksInit(void) } #endif -#ifdef USE_FC +#ifndef USE_OSD_SLAVE void fcTasksInit(void) { schedulerInit(); @@ -359,7 +357,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_MEDIUM_HIGH, }, -#ifdef USE_FC +#ifndef USE_OSD_SLAVE [TASK_GYROPID] = { .taskName = "PID", .subTaskName = "GYRO", @@ -404,7 +402,7 @@ cfTask_t cfTasks[TASK_COUNT] = { #endif }, -#ifdef USE_FC +#ifndef USE_OSD_SLAVE [TASK_DISPATCH] = { .taskName = "DISPATCH", .taskFunc = dispatchProcess, @@ -432,7 +430,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .desiredPeriod = TASK_PERIOD_HZ(50), .staticPriority = TASK_PRIORITY_MEDIUM, }, -#ifdef USE_FC +#ifndef USE_OSD_SLAVE #ifdef BEEPER [TASK_BEEPER] = { @@ -498,7 +496,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef USE_FC +#ifndef USE_OSD_SLAVE #ifdef USE_DASHBOARD [TASK_DASHBOARD] = { .taskName = "DASHBOARD", @@ -527,7 +525,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef USE_FC +#ifndef USE_OSD_SLAVE #ifdef TELEMETRY [TASK_TELEMETRY] = { .taskName = "TELEMETRY", diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 94f87d3e5e..07db82efef 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -17,6 +17,7 @@ #pragma once +#ifndef USE_OSD_SLAVE #include #include "config/parameter_group.h" @@ -111,3 +112,4 @@ void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); void pidInit(const pidProfile_t *pidProfile); +#endif diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index be4c7481d7..ed289aaecb 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -73,7 +73,11 @@ static batteryState_e consumptionState; #ifdef BOARD_HAS_CURRENT_SENSOR #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #else +#ifdef USE_VIRTUAL_CURRENT_METER #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL +#else +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE +#endif #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER @@ -290,7 +294,7 @@ void batteryInit(void) break; case CURRENT_METER_VIRTUAL: -#ifdef USE_FC +#ifdef USE_VIRTUAL_CURRENT_METER currentMeterVirtualInit(); #endif break; @@ -341,7 +345,7 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) break; case CURRENT_METER_VIRTUAL: { -#ifdef USE_FC +#ifdef USE_VIRTUAL_CURRENT_METER throttleStatus_e throttleStatus = calculateThrottleStatus(); bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)); int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index 09dc3d36ce..67b170fd32 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -38,7 +38,7 @@ const uint8_t currentMeterIds[] = { CURRENT_METER_ID_BATTERY_1, -#ifdef USE_FC +#ifdef USE_VIRTUAL_CURRENT_METER CURRENT_METER_ID_VIRTUAL_1, #endif #ifdef USE_ESC_SENSOR @@ -95,7 +95,9 @@ PG_RESET_TEMPLATE(currentSensorADCConfig_t, currentSensorADCConfig, .offset = CURRENT_METER_OFFSET_DEFAULT, ); +#ifdef USE_VIRTUAL_CURRENT_METER PG_REGISTER(currentSensorVirtualConfig_t, currentSensorVirtualConfig, PG_CURRENT_SENSOR_VIRTUAL_CONFIG, 0); +#endif static int32_t currentMeterADCToCentiamps(const uint16_t src) { @@ -146,7 +148,7 @@ void currentMeterADCRead(currentMeter_t *meter) // VIRTUAL // -#ifdef USE_FC +#ifdef USE_VIRTUAL_CURRENT_METER currentSensorVirtualState_t currentMeterVirtualState; void currentMeterVirtualInit(void) @@ -234,7 +236,7 @@ void currentMeterRead(currentMeterId_e id, currentMeter_t *meter) if (id == CURRENT_METER_ID_BATTERY_1) { currentMeterADCRead(meter); } -#ifdef USE_FC +#ifdef USE_VIRTUAL_CURRENT_METER else if (id == CURRENT_METER_ID_VIRTUAL_1) { currentMeterVirtualRead(meter); } diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 6a9f59238f..7ba1a27f2e 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -72,6 +72,7 @@ #define USE_CLI #define USE_PPM #define USE_PWM +#define USE_VIRTUAL_CURRENT_METER #define SERIAL_RX #define USE_SERIALRX_CRSF // Team Black Sheep Crossfire protocol #define USE_SERIALRX_IBUS // FlySky and Turnigy receivers diff --git a/src/test/Makefile b/src/test/Makefile index efb393aeef..3344a7cd3a 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -190,7 +190,7 @@ COMMON_FLAGS = \ -ggdb3 \ -pthread \ -O0 \ - -DUNIT_TEST -DUSE_FC\ + -DUNIT_TEST \ -isystem $(GTEST_DIR)/inc \ -MMD -MP