diff --git a/Makefile b/Makefile index 827e74632f..b8cfc77eab 100644 --- a/Makefile +++ b/Makefile @@ -16,7 +16,6 @@ # # The target to build, see BASE_TARGETS below -DEFAULT_TARGET ?= STM32F405 TARGET ?= CONFIG ?= @@ -124,12 +123,6 @@ FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH) # import config handling include $(MAKE_SCRIPT_DIR)/config.mk -ifeq ($(CONFIG),) -ifeq ($(TARGET),) -TARGET := $(DEFAULT_TARGET) -SKIPCHECKS := yes -endif -endif # default xtal value HSE_VALUE ?= 8000000 @@ -141,7 +134,9 @@ TARGET_PLATFORM := $(notdir $(patsubst %/,%,$(subst target/$(TARGET)/,, $(di TARGET_PLATFORM_DIR := $(PLATFORM_DIR)/$(TARGET_PLATFORM) LINKER_DIR := $(TARGET_PLATFORM_DIR)/link +ifneq ($(TARGET),) include $(TARGET_PLATFORM_DIR)/target/$(TARGET)/target.mk +endif REVISION := norevision ifeq ($(shell git diff --shortstat),) @@ -182,6 +177,8 @@ endif VPATH := $(VPATH):$(MAKE_SCRIPT_DIR) +ifneq ($(TARGET),) + # start specific includes ifeq ($(TARGET_MCU),) $(error No TARGET_MCU specified. Is the target.mk valid for $(TARGET)?) @@ -202,9 +199,6 @@ SIZE_OPTIMISED_SRC := include $(TARGET_PLATFORM_DIR)/mk/$(TARGET_MCU_FAMILY).mk -# openocd specific includes -include $(MAKE_SCRIPT_DIR)/openocd.mk - # Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already. ifeq ($(TARGET_FLASH_SIZE),) ifneq ($(MCU_FLASH_SIZE),) @@ -221,8 +215,12 @@ DEVICE_FLAGS := $(DEVICE_FLAGS) -DHSE_VALUE=$(HSE_VALUE) endif TARGET_DIR = $(TARGET_PLATFORM_DIR)/target/$(TARGET) +endif # TARGET specified -.DEFAULT_GOAL := hex +# openocd specific includes +include $(MAKE_SCRIPT_DIR)/openocd.mk + +.DEFAULT_GOAL := all INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(ROOT)/lib/main/MAVLink @@ -234,7 +232,7 @@ VPATH := $(VPATH):$(TARGET_DIR) include $(MAKE_SCRIPT_DIR)/source.mk -ifneq ($(SKIPCHECKS),yes) +ifneq ($(TARGET),) ifneq ($(filter-out $(SRC),$(SPEED_OPTIMISED_SRC)),) $(error Speed optimised sources not valid: $(strip $(filter-out $(SRC),$(SPEED_OPTIMISED_SRC)))) endif @@ -642,7 +640,7 @@ help: Makefile mk/tools.mk @echo "Or:" @echo " make [V=] [OPTIONS=\"\"] [EXTRA_FLAGS=\"\"]" @echo "" - @echo "To pupulate configuration targets:" + @echo "To populate configuration targets:" @echo " make configs" @echo "" @echo "Valid TARGET values are: $(BASE_TARGETS)" diff --git a/mk/source.mk b/mk/source.mk index 70e4d00b57..e4c60d55cb 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -1,7 +1,9 @@ PG_SRC = \ pg/adc.c \ - pg/alt_hold.c \ - pg/autopilot.c \ + pg/alt_hold_multirotor.c \ + pg/alt_hold_wing.c \ + pg/autopilot_multirotor.c \ + pg/autopilot_wing.c \ pg/beeper.c \ pg/beeper_dev.c \ pg/board.c \ @@ -15,17 +17,20 @@ PG_SRC = \ pg/gimbal.c \ pg/gps.c \ pg/gps_lap_timer.c \ - pg/gps_rescue.c \ + pg/gps_rescue_multirotor.c \ + pg/gps_rescue_wing.c \ pg/gyrodev.c \ pg/max7456.c \ pg/mco.c \ pg/motor.c \ pg/msp.c \ pg/pg.c \ + pg/pilot.c \ pg/piniobox.c \ pg/pinio.c \ pg/pin_pull_up_down.c \ - pg/pos_hold.c \ + pg/pos_hold_multirotor.c \ + pg/pos_hold_wing.c \ pg/rcdevice.c \ pg/rpm_filter.c \ pg/rx.c \ @@ -153,11 +158,14 @@ COMMON_SRC = \ fc/rc_adjustments.c \ fc/rc_controls.c \ fc/rc_modes.c \ - flight/alt_hold.c \ - flight/autopilot.c \ + flight/alt_hold_multirotor.c \ + flight/alt_hold_wing.c \ + flight/autopilot_multirotor.c \ + flight/autopilot_wing.c \ flight/dyn_notch_filter.c \ flight/failsafe.c \ - flight/gps_rescue.c \ + flight/gps_rescue_multirotor.c \ + flight/gps_rescue_wing.c \ flight/imu.c \ flight/mixer.c \ flight/mixer_init.c \ @@ -165,7 +173,8 @@ COMMON_SRC = \ flight/pid.c \ flight/pid_init.c \ flight/position.c \ - flight/pos_hold.c \ + flight/pos_hold_multirotor.c \ + flight/pos_hold_wing.c \ flight/rpm_filter.c \ flight/servos.c \ flight/servos_tricopter.c \ @@ -209,7 +218,8 @@ COMMON_SRC = \ cms/cms_menu_blackbox.c \ cms/cms_menu_failsafe.c \ cms/cms_menu_firmware.c \ - cms/cms_menu_gps_rescue.c \ + cms/cms_menu_gps_rescue_multirotor.c \ + cms/cms_menu_gps_rescue_wing.c \ cms/cms_menu_gps_lap_timer.c \ cms/cms_menu_imu.c \ cms/cms_menu_ledstrip.c \ @@ -512,7 +522,8 @@ SIZE_OPTIMISED_SRC += \ cms/cms_menu_blackbox.c \ cms/cms_menu_failsafe.c \ cms/cms_menu_firmware.c \ - cms/cms_menu_gps_rescue.c \ + cms/cms_menu_gps_rescue_multirotor.c \ + cms/cms_menu_gps_rescue_wing.c \ cms/cms_menu_gps_lap_timer.c \ cms/cms_menu_imu.c \ cms/cms_menu_ledstrip.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index de74df3c30..c8f359b924 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -81,8 +81,9 @@ #include "pg/alt_hold.h" #include "pg/autopilot.h" #include "pg/motor.h" -#include "pg/rx.h" +#include "pg/pilot.h" #include "pg/pos_hold.h" +#include "pg/rx.h" #include "rx/rx.h" @@ -744,7 +745,7 @@ static void writeIntraframe(void) if (testBlackboxCondition(CONDITION(ACC))) { blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT); } - + if (testBlackboxCondition(CONDITION(ATTITUDE))) { blackboxWriteSigned16VBArray(blackboxCurrent->imuAttitudeQuaternion, XYZ_AXIS_COUNT); } @@ -929,7 +930,7 @@ static void writeInterframe(void) if (testBlackboxCondition(CONDITION(ACC))) { blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT); } - + if (testBlackboxCondition(CONDITION(ATTITUDE))) { blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, imuAttitudeQuaternion), XYZ_AXIS_COUNT); } @@ -1243,14 +1244,14 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->axisPID_F[i] = lrintf(pidData[i].F); #ifdef USE_WING blackboxCurrent->axisPID_S[i] = lrintf(pidData[i].S); -#endif +#endif blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i] * blackboxHighResolutionScale); blackboxCurrent->gyroUnfilt[i] = lrintf(gyro.gyroADC[i] * blackboxHighResolutionScale); #if defined(USE_ACC) blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]); STATIC_ASSERT(offsetof(quaternion_t, w) == 0, "Code expects quaternion in w, x, y, z order"); - blackboxCurrent->imuAttitudeQuaternion[i] = lrintf(imuAttitudeQuaternion.v[i + 1] * 0x7FFF); //Scale to int16 by value 0x7FFF = 2^15 - 1; Use i+1 index for x,y,z components access, [0] - w + blackboxCurrent->imuAttitudeQuaternion[i] = lrintf(imuAttitudeQuaternion.v[i + 1] * 0x7FFF); //Scale to int16 by value 0x7FFF = 2^15 - 1; Use i+1 index for x,y,z components access, [0] - w #endif #ifdef USE_MAG blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]); @@ -1694,6 +1695,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf); +#ifndef USE_WING BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", apConfig()->landing_altitude_m); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", apConfig()->hover_throttle); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", apConfig()->throttle_min); @@ -1708,6 +1710,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_A, "%d", apConfig()->position_A); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_CUTOFF, "%d", apConfig()->position_cutoff); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d", apConfig()->max_angle); +#endif // !USE_WING #ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware); @@ -1784,6 +1787,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed) #ifdef USE_GPS_RESCUE +#ifndef USE_WING BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minStartDistM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->initialClimbM) @@ -1808,21 +1812,25 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP) - #ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag) #endif // USE_MAG +#endif // !USE_WING #endif // USE_GPS_RESCUE #endif // USE_GPS #ifdef USE_ALTITUDE_HOLD +#ifndef USE_WING BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, "%d", altHoldConfig()->alt_hold_adjust_rate); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", altHoldConfig()->alt_hold_deadband); -#endif +#endif // !USE_WING +#endif // USE_ALTITUDE_HOLD #ifdef USE_POSITION_HOLD +#ifndef USE_WING BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", posHoldConfig()->pos_hold_deadband); +#endif // !USE_WING #endif #ifdef USE_WING diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 926b5ada5a..a174fe8cf5 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -139,6 +139,7 @@ bool cliMode = false; #include "pg/max7456.h" #include "pg/mco.h" #include "pg/motor.h" +#include "pg/pilot.h" #include "pg/pinio.h" #include "pg/pin_pull_up_down.h" #include "pg/pg.h" @@ -209,6 +210,7 @@ static bool signatureUpdated = false; #endif // USE_BOARD_INFO static const char* const emptyName = "-"; +static const char* const invalidName = "INVALID"; #define MAX_CHANGESET_ID_LENGTH 8 #define MAX_DATE_LENGTH 20 @@ -1274,7 +1276,7 @@ static void cliAux(const char *cmdName, char *cmdline) static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig, const serialConfig_t *serialConfigDefault, const char *headingStr) { - const char *format = "serial %d %d %ld %ld %ld %ld"; + const char *format = "serial %s %d %ld %ld %ld %ld"; headingStr = cliPrintSectionHeading(dumpMask, false, headingStr); for (unsigned i = 0; i < ARRAYLEN(serialConfig->portConfigs); i++) { if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { @@ -1285,7 +1287,7 @@ static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig equalsDefault = !memcmp(&serialConfig->portConfigs[i], &serialConfigDefault->portConfigs[i], sizeof(serialConfig->portConfigs[i])); headingStr = cliPrintSectionHeading(dumpMask, !equalsDefault, headingStr); cliDefaultPrintLinef(dumpMask, equalsDefault, format, - serialConfigDefault->portConfigs[i].identifier, + serialName(serialConfigDefault->portConfigs[i].identifier, invalidName), serialConfigDefault->portConfigs[i].functionMask, baudRates[serialConfigDefault->portConfigs[i].msp_baudrateIndex], baudRates[serialConfigDefault->portConfigs[i].gps_baudrateIndex], @@ -1294,7 +1296,7 @@ static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig ); } cliDumpPrintLinef(dumpMask, equalsDefault, format, - serialConfig->portConfigs[i].identifier, + serialName(serialConfig->portConfigs[i].identifier, invalidName), serialConfig->portConfigs[i].functionMask, baudRates[serialConfig->portConfigs[i].msp_baudrateIndex], baudRates[serialConfig->portConfigs[i].gps_baudrateIndex], @@ -1306,40 +1308,58 @@ static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig static void cliSerial(const char *cmdName, char *cmdline) { - const char *format = "serial %d %d %ld %ld %ld %ld"; + const char *format = "serial %s %d %ld %ld %ld %ld"; if (isEmpty(cmdline)) { printSerial(DUMP_MASTER, serialConfig(), NULL, NULL); return; } + serialPortConfig_t portConfig; memset(&portConfig, 0 , sizeof(portConfig)); uint8_t validArgumentCount = 0; - const char *ptr = cmdline; - - int val = atoi(ptr++); - serialPortConfig_t *currentConfig = serialFindPortConfigurationMutable(val); - - if (currentConfig) { - portConfig.identifier = val; - validArgumentCount++; + char *ptr = cmdline; + char *tok = strsep(&ptr, " "); + serialPortIdentifier_e identifier = findSerialPortByName(tok, strcasecmp); + if (identifier == SERIAL_PORT_NONE) { + char *eptr; + identifier = strtoul(tok, &eptr, 10); + if (*eptr) { + // parsing ended before end of token indicating an invalid identifier + identifier = SERIAL_PORT_NONE; + } else { + // correction for legacy configuration where UART1 == 0 + if (identifier >= SERIAL_PORT_LEGACY_START_IDENTIFIER && identifier < SERIAL_PORT_START_IDENTIFIER) { + identifier += SERIAL_PORT_UART1; + } + } } - ptr = nextArg(ptr); - if (ptr) { - val = strtoul(ptr, NULL, 10); + serialPortConfig_t *currentConfig = serialFindPortConfigurationMutable(identifier); + + if (!currentConfig) { + cliShowParseError(cmdName); + return; + } + + portConfig.identifier = identifier; + validArgumentCount++; + + tok = strsep(&ptr, " "); + if (tok) { + int val = strtoul(tok, NULL, 10); portConfig.functionMask = val; validArgumentCount++; } for (int i = 0; i < 4; i ++) { - ptr = nextArg(ptr); - if (!ptr) { + tok = strsep(&ptr, " "); + if (!tok) { break; } - val = atoi(ptr); + int val = atoi(tok); uint8_t baudRateIndex = lookupBaudRateIndex(val); if (baudRates[baudRateIndex] != (uint32_t) val) { @@ -1384,14 +1404,13 @@ static void cliSerial(const char *cmdName, char *cmdline) memcpy(currentConfig, &portConfig, sizeof(portConfig)); cliDumpPrintLinef(0, false, format, - portConfig.identifier, + serialName(portConfig.identifier, invalidName), portConfig.functionMask, baudRates[portConfig.msp_baudrateIndex], baudRates[portConfig.gps_baudrateIndex], baudRates[portConfig.telemetry_baudrateIndex], baudRates[portConfig.blackbox_baudrateIndex] ); - } #if defined(USE_SERIAL_PASSTHROUGH) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index fc4e9a703a..1bfe829b79 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -95,6 +95,7 @@ #include "pg/msp.h" #include "pg/pg.h" #include "pg/pg_ids.h" +#include "pg/pilot.h" #include "pg/pinio.h" #include "pg/piniobox.h" #include "pg/pos_hold.h" @@ -1073,6 +1074,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_GPS_NMEA_CUSTOM_COMMANDS, VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, NMEA_CUSTOM_COMMANDS_MAX_LENGTH, STRING_FLAGS_NONE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, nmeaCustomCommands) }, #ifdef USE_GPS_RESCUE +#ifndef USE_WING // PG_GPS_RESCUE { PARAM_NAME_GPS_RESCUE_MIN_START_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 30 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minStartDistM) }, { PARAM_NAME_GPS_RESCUE_ALT_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) }, @@ -1102,6 +1104,7 @@ const clivalue_t valueTable[] = { #ifdef USE_MAG { PARAM_NAME_GPS_RESCUE_USE_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) }, #endif // USE_MAG +#endif // !USE_WING #endif // USE_GPS_RESCUE #ifdef USE_GPS_LAP_TIMER @@ -1118,14 +1121,18 @@ const clivalue_t valueTable[] = { { "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) }, #ifdef USE_ALTITUDE_HOLD +#ifndef USE_WING { PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) }, { PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 70 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_deadband) }, -#endif +#endif // !USE_WING +#endif // USE_ALTITUDE_HOLD #ifdef USE_POSITION_HOLD +#ifndef USE_WING { PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_without_mag) }, { PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_deadband) }, -#endif +#endif // !USE_WING +#endif // USE_POSITION_HOLD // PG_PID_CONFIG { PARAM_NAME_PID_PROCESS_DENOM, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) }, @@ -1866,6 +1873,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_ALTITUDE_D_LPF, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) }, // PG_AUTOPILOT +#ifndef USE_WING { PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, landing_altitude_m) }, { PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(apConfig_t, hover_throttle) }, { PARAM_NAME_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(apConfig_t, throttle_min) }, @@ -1880,6 +1888,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_POSITION_A, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_A) }, { PARAM_NAME_POSITION_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_AUTOPILOT, offsetof(apConfig_t, position_cutoff) }, { PARAM_NAME_AP_MAX_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 70 }, PG_AUTOPILOT, offsetof(apConfig_t, max_angle) }, +#endif // !USE_WING // PG_MODE_ACTIVATION_CONFIG #if defined(USE_CUSTOM_BOX_NAMES) diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue_multirotor.c similarity index 99% rename from src/main/cms/cms_menu_gps_rescue.c rename to src/main/cms/cms_menu_gps_rescue_multirotor.c index 9b2cbbb948..a6520d0734 100644 --- a/src/main/cms/cms_menu_gps_rescue.c +++ b/src/main/cms/cms_menu_gps_rescue_multirotor.c @@ -25,6 +25,8 @@ #include "platform.h" +#ifndef USE_WING + #ifdef USE_CMS_GPS_RESCUE_MENU #include "cli/settings.h" @@ -242,3 +244,5 @@ CMS_Menu cmsx_menuGpsRescue = { }; #endif + +#endif // !USE_WING diff --git a/src/main/cms/cms_menu_gps_rescue_wing.c b/src/main/cms/cms_menu_gps_rescue_wing.c new file mode 100644 index 0000000000..0e2e0a78b5 --- /dev/null +++ b/src/main/cms/cms_menu_gps_rescue_wing.c @@ -0,0 +1,114 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_WING + +#ifdef USE_CMS_GPS_RESCUE_MENU + +#include "cli/settings.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_gps_rescue.h" + +#include "config/feature.h" + +#include "config/config.h" + +#include "flight/position.h" + +#include "pg/autopilot.h" +#include "pg/gps_rescue.h" + +static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp) +{ + UNUSED(pDisp); + + return NULL; +} + +static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_Entry *self) +{ + UNUSED(pDisp); + UNUSED(self); + + return NULL; +} + +const OSD_Entry cms_menuGpsRescuePidEntries[] = +{ + {"--- GPS RESCUE PID---", OME_Label, NULL, NULL}, + + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +CMS_Menu cms_menuGpsRescuePid = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUGPSRPID", + .GUARD_type = OME_MENU, +#endif + .onEnter = cms_menuGpsRescuePidOnEnter, + .onExit = cms_menuGpsRescuePidOnExit, + .onDisplayUpdate = NULL, + .entries = cms_menuGpsRescuePidEntries, +}; + +static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp) +{ + UNUSED(pDisp); + + return NULL; +} + +static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entry *self) +{ + UNUSED(pDisp); + UNUSED(self); + + return NULL; +} + +const OSD_Entry cmsx_menuGpsRescueEntries[] = +{ + {"--- GPS RESCUE ---", OME_Label, NULL, NULL}, + + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +CMS_Menu cmsx_menuGpsRescue = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUGPSRES", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuGpsRescueOnEnter, + .onExit = cmsx_menuGpsRescueOnExit, + .onDisplayUpdate = NULL, + .entries = cmsx_menuGpsRescueEntries, +}; + +#endif + +#endif // USE_WING diff --git a/src/main/config/config.c b/src/main/config/config.c index de895e7484..aedfdb12d1 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -106,13 +106,6 @@ pidProfile_t *currentPidProfile; #define RX_SPI_DEFAULT_PROTOCOL 0 #endif -PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 2); - -PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig, - .craftName = { 0 }, - .pilotName = { 0 }, -); - PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 3); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, diff --git a/src/main/config/config.h b/src/main/config/config.h index 94118a8611..17f35ee36c 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -25,20 +25,11 @@ #include "pg/pg.h" -#define MAX_NAME_LENGTH 16u - typedef enum { CONFIGURATION_STATE_UNCONFIGURED = 0, CONFIGURATION_STATE_CONFIGURED, } configurationState_e; -typedef struct pilotConfig_s { - char craftName[MAX_NAME_LENGTH + 1]; - char pilotName[MAX_NAME_LENGTH + 1]; -} pilotConfig_t; - -PG_DECLARE(pilotConfig_t, pilotConfig); - typedef struct systemConfig_s { uint8_t pidProfileIndex; uint8_t activeRateProfile; diff --git a/src/main/drivers/serial_pinconfig.c b/src/main/drivers/serial_pinconfig.c index 71a1bf57ea..d72c8dbbff 100644 --- a/src/main/drivers/serial_pinconfig.c +++ b/src/main/drivers/serial_pinconfig.c @@ -40,6 +40,9 @@ typedef struct serialDefaultPin_s { } serialDefaultPin_t; static const serialDefaultPin_t serialDefaultPin[] = { +#ifdef USE_UART0 + { SERIAL_PORT_UART0, IO_TAG(UART0_RX_PIN), IO_TAG(UART0_TX_PIN), IO_TAG(INVERTER_PIN_UART0) }, +#endif #ifdef USE_UART1 { SERIAL_PORT_USART1, IO_TAG(UART1_RX_PIN), IO_TAG(UART1_TX_PIN), IO_TAG(INVERTER_PIN_UART1) }, #endif diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index 6ffa2e386b..bc6f6d37ff 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -44,15 +44,18 @@ static const struct serialPortVTable tcpVTable; // Forward static tcpPort_t tcpSerialPorts[SERIAL_PORT_COUNT]; static bool tcpPortInitialized[SERIAL_PORT_COUNT]; static bool tcpStart = false; + bool tcpIsStart(void) { return tcpStart; } + static void onData(dyad_Event *e) { tcpPort_t* s = (tcpPort_t*)(e->udata); tcpDataIn(s, (uint8_t*)e->data, e->size); } + static void onClose(dyad_Event *e) { tcpPort_t* s = (tcpPort_t*)(e->udata); @@ -63,6 +66,7 @@ static void onClose(dyad_Event *e) s->connected = false; } } + static void onAccept(dyad_Event *e) { tcpPort_t* s = (tcpPort_t*)(e->udata); @@ -81,6 +85,7 @@ static void onAccept(dyad_Event *e) dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata); dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata); } + static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id) { if (tcpPortInitialized[id]) { @@ -93,6 +98,7 @@ static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id) // TODO: clean up & re-init return NULL; } + if (pthread_mutex_init(&s->rxLock, NULL) != 0) { fprintf(stderr, "RX mutex init failed - %d\n", errno); // TODO: clean up & re-init @@ -118,17 +124,19 @@ static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id) return s; } -serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options) +serialPort_t *serTcpOpen(serialPortIdentifier_e identifier, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options) { tcpPort_t *s = NULL; -#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8) - if (id >= 0 && id < SERIAL_PORT_COUNT) { - s = tcpReconfigure(&tcpSerialPorts[id], id); + int id = findSerialPortIndexByIdentifier(identifier); + + if (id >= 0 && id < (int)ARRAYLEN(tcpSerialPorts)) { + s = tcpReconfigure(&tcpSerialPorts[id], id); } -#endif - if (!s) + + if (!s) { return NULL; + } s->port.vTable = &tcpVTable; diff --git a/src/main/drivers/serial_tcp.h b/src/main/drivers/serial_tcp.h index 4e70f242b0..a391e10891 100644 --- a/src/main/drivers/serial_tcp.h +++ b/src/main/drivers/serial_tcp.h @@ -23,6 +23,7 @@ #include #include #include "dyad.h" +#include "io/serial.h" #define RX_BUFFER_SIZE 1400 #define TX_BUFFER_SIZE 1400 @@ -41,7 +42,7 @@ typedef struct { uint8_t id; } tcpPort_t; -serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options); +serialPort_t *serTcpOpen(serialPortIdentifier_e id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options); // tcpPort API void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size); diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 24bfb66c8a..d4c530d889 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -71,6 +71,10 @@ UART_BUFFER(UART_RX_BUFFER_ATTRIBUTE, n, R); struct dummy_s \ /**/ +#ifdef USE_UART0 +UART_BUFFERS(0); +#endif + #ifdef USE_UART1 UART_BUFFERS(1); #endif @@ -136,6 +140,9 @@ uartDeviceIdx_e uartDeviceIdxFromIdentifier(serialPortIdentifier_e identifier) // table is for UART only to save space (LPUART is handled separately) #define _R(id, dev) [id] = (dev) + 1 static const uartDeviceIdx_e uartMap[] = { +#ifdef USE_UART0 + _R(SERIAL_PORT_UART0, UARTDEV_0), +#endif #ifdef USE_UART1 _R(SERIAL_PORT_USART1, UARTDEV_1), #endif diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h index e6b15d27a3..94b679e505 100644 --- a/src/main/drivers/serial_uart_impl.h +++ b/src/main/drivers/serial_uart_impl.h @@ -132,6 +132,9 @@ // compressed index of UART/LPUART. Direct index into uartDevice[] typedef enum { UARTDEV_INVALID = -1, +#ifdef USE_UART0 + UARTDEV_0, +#endif #ifdef USE_UART1 UARTDEV_1, #endif @@ -285,6 +288,10 @@ void uartTxMonitor(uartPort_t *s); UART_BUFFER(extern, n, T); struct dummy_s \ /**/ +#ifdef USE_UART0 +UART_BUFFERS_EXTERN(0); +#endif + #ifdef USE_UART1 UART_BUFFERS_EXTERN(1); #endif diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 04c96c0c2b..8e838b97be 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -226,6 +226,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_LED_PROFILE, .mode = ADJUSTMENT_MODE_SELECT, .data = { .switchPositions = 3 } + }, { + .adjustmentFunction = ADJUSTMENT_LED_DIMMER, + .mode = ADJUSTMENT_MODE_SELECT, + .data = { .switchPositions = 100 } } }; @@ -264,6 +268,8 @@ static const char * const adjustmentLabels[] = { "ROLL F", "YAW F", "OSD PROFILE", + "LED PROFILE", + "LED DIMMER", }; static int adjustmentRangeNameIndex = 0; @@ -641,6 +647,13 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui if (getLedProfile() != position) { setLedProfile(position); } +#endif + break; + case ADJUSTMENT_LED_DIMMER: +#ifdef USE_LED_STRIP + if (getLedBrightness() != position) { + setLedBrightness(position); + } #endif break; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 669205f554..57855ba24e 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -62,6 +62,7 @@ typedef enum { ADJUSTMENT_YAW_F, ADJUSTMENT_OSD_PROFILE, ADJUSTMENT_LED_PROFILE, + ADJUSTMENT_LED_DIMMER, ADJUSTMENT_FUNCTION_COUNT } adjustmentFunction_e; diff --git a/src/main/flight/alt_hold.h b/src/main/flight/alt_hold.h index c82d6c9b6e..875006c24f 100644 --- a/src/main/flight/alt_hold.h +++ b/src/main/flight/alt_hold.h @@ -17,15 +17,5 @@ #pragma once -#include "pg/alt_hold.h" - -#ifdef USE_ALTITUDE_HOLD -#include "common/time.h" - -#define ALTHOLD_TASK_RATE_HZ 100 // hz - -void altHoldInit(void); -void updateAltHold(timeUs_t currentTimeUs); -bool isAltHoldActive(void); - -#endif +#include "flight/alt_hold_multirotor.h" +#include "flight/alt_hold_wing.h" diff --git a/src/main/flight/alt_hold.c b/src/main/flight/alt_hold_multirotor.c similarity index 97% rename from src/main/flight/alt_hold.c rename to src/main/flight/alt_hold_multirotor.c index 97c301e9c0..7c089cb165 100644 --- a/src/main/flight/alt_hold.c +++ b/src/main/flight/alt_hold_multirotor.c @@ -16,6 +16,9 @@ */ #include "platform.h" + +#ifndef USE_WING + #include "math.h" #ifdef USE_ALTITUDE_HOLD @@ -90,8 +93,8 @@ void altHoldProcessTransitions(void) { void altHoldUpdateTargetAltitude(void) { // User can adjust the target altitude with throttle, but only when - // - throttle is outside deadband, and - // - throttle is not low (zero), and + // - throttle is outside deadband, and + // - throttle is not low (zero), and // - deadband is not configured to zero float stickFactor = 0.0f; @@ -154,3 +157,5 @@ bool isAltHoldActive(void) { return altHold.isActive; } #endif + +#endif // !USE_WING diff --git a/src/main/flight/alt_hold_multirotor.h b/src/main/flight/alt_hold_multirotor.h new file mode 100644 index 0000000000..c18cd9a6e6 --- /dev/null +++ b/src/main/flight/alt_hold_multirotor.h @@ -0,0 +1,35 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifndef USE_WING + +#include "pg/alt_hold.h" + +#ifdef USE_ALTITUDE_HOLD +#include "common/time.h" + +#define ALTHOLD_TASK_RATE_HZ 100 // hz + +void altHoldInit(void); +void updateAltHold(timeUs_t currentTimeUs); +bool isAltHoldActive(void); + +#endif + +#endif // !USE_WING diff --git a/src/main/flight/alt_hold_wing.c b/src/main/flight/alt_hold_wing.c new file mode 100644 index 0000000000..05b0db18c2 --- /dev/null +++ b/src/main/flight/alt_hold_wing.c @@ -0,0 +1,59 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#include "platform.h" + +#ifdef USE_WING + +#include "math.h" + +#ifdef USE_ALTITUDE_HOLD + +#include "build/debug.h" +#include "common/maths.h" +#include "config/config.h" + +#include "fc/rc.h" +#include "fc/runtime_config.h" + +#include "flight/autopilot.h" +#include "flight/failsafe.h" +#include "flight/position.h" + +#include "rx/rx.h" +#include "pg/autopilot.h" + +#include "alt_hold.h" + +void altHoldReset(void) +{ +} + +void altHoldInit(void) +{ +} + +void updateAltHold(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); +} + +bool isAltHoldActive(void) { + return false; +} + +#endif // USE_ALTITUDE_HOLD +#endif // USE_WING diff --git a/src/main/flight/alt_hold_wing.h b/src/main/flight/alt_hold_wing.h new file mode 100644 index 0000000000..12da834876 --- /dev/null +++ b/src/main/flight/alt_hold_wing.h @@ -0,0 +1,35 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifdef USE_WING + +#include "pg/alt_hold.h" + +#ifdef USE_ALTITUDE_HOLD +#include "common/time.h" + +#define ALTHOLD_TASK_RATE_HZ 100 // hz + +void altHoldInit(void); +void updateAltHold(timeUs_t currentTimeUs); +bool isAltHoldActive(void); + +#endif + +#endif // USE_WING diff --git a/src/main/flight/autopilot.h b/src/main/flight/autopilot.h index dbdaaea120..98860bedae 100644 --- a/src/main/flight/autopilot.h +++ b/src/main/flight/autopilot.h @@ -19,16 +19,5 @@ #include "io/gps.h" -extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES - -void autopilotInit(void); -void resetAltitudeControl(void); -void setSticksActiveStatus(bool areSticksActive); -void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz); -void posControlOutput(void); -bool positionControl(void); -void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep); - -bool isBelowLandingAltitude(void); -float getAutopilotThrottle(void); -bool isAutopilotInControl(void); +#include "flight/autopilot_multirotor.h" +#include "flight/autopilot_wing.h" diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot_multirotor.c similarity index 99% rename from src/main/flight/autopilot.c rename to src/main/flight/autopilot_multirotor.c index 8a41219b5f..c70635599a 100644 --- a/src/main/flight/autopilot.c +++ b/src/main/flight/autopilot_multirotor.c @@ -21,6 +21,9 @@ #include #include "platform.h" + +#ifndef USE_WING + #include "build/debug.h" #include "common/axis.h" #include "common/filter.h" @@ -395,3 +398,5 @@ bool isAutopilotInControl(void) { return !ap.sticksActive; } + +#endif // !USE_WING diff --git a/src/main/flight/autopilot_multirotor.h b/src/main/flight/autopilot_multirotor.h new file mode 100644 index 0000000000..6fe44bbd2a --- /dev/null +++ b/src/main/flight/autopilot_multirotor.h @@ -0,0 +1,38 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifndef USE_WING + +#include "io/gps.h" + +extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES + +void autopilotInit(void); +void resetAltitudeControl(void); +void setSticksActiveStatus(bool areSticksActive); +void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz); +void posControlOutput(void); +bool positionControl(void); +void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep); + +bool isBelowLandingAltitude(void); +float getAutopilotThrottle(void); +bool isAutopilotInControl(void); + +#endif // !USE_WING diff --git a/src/main/flight/autopilot_wing.c b/src/main/flight/autopilot_wing.c new file mode 100644 index 0000000000..936933f446 --- /dev/null +++ b/src/main/flight/autopilot_wing.c @@ -0,0 +1,91 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_WING + +#include "build/debug.h" +#include "common/axis.h" +#include "common/filter.h" +#include "common/maths.h" +#include "common/vector.h" +#include "fc/rc.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/position.h" +#include "rx/rx.h" +#include "sensors/gyro.h" + +#include "pg/autopilot.h" +#include "autopilot.h" + +float autopilotAngle[RP_AXIS_COUNT]; + +void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz) +{ + // from pos_hold.c (or other client) when initiating position hold at target location + UNUSED(initialTargetLocation); + UNUSED(taskRateHz); +} + +void autopilotInit(void) +{ +} + +void resetAltitudeControl (void) { +} + +void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep) +{ + UNUSED(targetAltitudeCm); + UNUSED(taskIntervalS); + UNUSED(targetAltitudeStep); +} + +void setSticksActiveStatus(bool areSticksActive) +{ + UNUSED(areSticksActive); +} + +bool positionControl(void) +{ + return false; +} + +bool isBelowLandingAltitude(void) +{ + return false; +} + +float getAutopilotThrottle(void) +{ + return 0.0f; +} + +bool isAutopilotInControl(void) +{ + return false; +} + +#endif // USE_WING diff --git a/src/main/flight/autopilot_wing.h b/src/main/flight/autopilot_wing.h new file mode 100644 index 0000000000..db7efdda4d --- /dev/null +++ b/src/main/flight/autopilot_wing.h @@ -0,0 +1,37 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifdef USE_WING + +#include "io/gps.h" + +extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES + +void autopilotInit(void); +void resetAltitudeControl(void); +void setSticksActiveStatus(bool areSticksActive); +void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz); +bool positionControl(void); +void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep); + +bool isBelowLandingAltitude(void); +float getAutopilotThrottle(void); +bool isAutopilotInControl(void); + +#endif // USE_WING diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index c2e60e34b8..5dbe982e9d 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -17,41 +17,5 @@ #pragma once -#include - -#include "common/axis.h" - -#include "pg/gps_rescue.h" - -#define TASK_GPS_RESCUE_RATE_HZ 100 // in sync with altitude task rate - -#ifdef USE_MAG -#define GPS_RESCUE_USE_MAG true -#else -#define GPS_RESCUE_USE_MAG false -#endif - -typedef enum { - RESCUE_SANITY_OFF = 0, - RESCUE_SANITY_ON, - RESCUE_SANITY_FS_ONLY, - RESCUE_SANITY_COUNT -} gpsRescueSanity_e; - -typedef enum { - GPS_RESCUE_ALT_MODE_MAX = 0, - GPS_RESCUE_ALT_MODE_FIXED, - GPS_RESCUE_ALT_MODE_CURRENT, - GPS_RESCUE_ALT_MODE_COUNT -} gpsRescueAltitudeMode_e; - -extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES - -void gpsRescueInit(void); -void gpsRescueUpdate(void); -float gpsRescueGetYawRate(void); -bool gpsRescueIsConfigured(void); -bool gpsRescueIsAvailable(void); -bool gpsRescueIsDisabled(void); -bool gpsRescueDisableMag(void); -float gpsRescueGetImuYawCogGain(void); +#include "flight/gps_rescue_multirotor.h" +#include "flight/gps_rescue_wing.h" diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue_multirotor.c similarity index 99% rename from src/main/flight/gps_rescue.c rename to src/main/flight/gps_rescue_multirotor.c index e70715b369..c88295d9dc 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue_multirotor.c @@ -21,6 +21,7 @@ #include "platform.h" +#ifndef USE_WING #ifdef USE_GPS_RESCUE #include "build/debug.h" @@ -138,7 +139,7 @@ void gpsRescueInit(void) rescueState.intent.velocityPidCutoffModifier = 1.0f; gain = pt1FilterGain(cutoffHz, 1.0f); pt1FilterInit(&velocityDLpf, gain); - cutoffHz *= 4.0f; + cutoffHz *= 4.0f; gain = pt3FilterGain(cutoffHz, taskIntervalSeconds); pt3FilterInit(&velocityUpsampleLpf, gain); } @@ -887,3 +888,5 @@ bool gpsRescueDisableMag(void) } #endif #endif + +#endif // !USE_WING diff --git a/src/main/flight/gps_rescue_multirotor.h b/src/main/flight/gps_rescue_multirotor.h new file mode 100644 index 0000000000..dfb2087b83 --- /dev/null +++ b/src/main/flight/gps_rescue_multirotor.h @@ -0,0 +1,61 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifndef USE_WING + +#include + +#include "common/axis.h" + +#include "pg/gps_rescue.h" + +#define TASK_GPS_RESCUE_RATE_HZ 100 // in sync with altitude task rate + +#ifdef USE_MAG +#define GPS_RESCUE_USE_MAG true +#else +#define GPS_RESCUE_USE_MAG false +#endif + +typedef enum { + RESCUE_SANITY_OFF = 0, + RESCUE_SANITY_ON, + RESCUE_SANITY_FS_ONLY, + RESCUE_SANITY_COUNT +} gpsRescueSanity_e; + +typedef enum { + GPS_RESCUE_ALT_MODE_MAX = 0, + GPS_RESCUE_ALT_MODE_FIXED, + GPS_RESCUE_ALT_MODE_CURRENT, + GPS_RESCUE_ALT_MODE_COUNT +} gpsRescueAltitudeMode_e; + +extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES + +void gpsRescueInit(void); +void gpsRescueUpdate(void); +float gpsRescueGetYawRate(void); +bool gpsRescueIsConfigured(void); +bool gpsRescueIsAvailable(void); +bool gpsRescueIsDisabled(void); +bool gpsRescueDisableMag(void); +float gpsRescueGetImuYawCogGain(void); + +#endif // !USE_WING diff --git a/src/main/flight/gps_rescue_wing.c b/src/main/flight/gps_rescue_wing.c new file mode 100644 index 0000000000..218e0c41b6 --- /dev/null +++ b/src/main/flight/gps_rescue_wing.c @@ -0,0 +1,100 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_WING +#ifdef USE_GPS_RESCUE + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/filter.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "config/config.h" +#include "drivers/time.h" + +#include "fc/core.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "flight/autopilot.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/pid.h" +#include "flight/position.h" + +#include "io/gps.h" +#include "rx/rx.h" +#include "pg/autopilot.h" +#include "sensors/acceleration.h" + +#include "gps_rescue.h" + +float gpsRescueAngle[RP_AXIS_COUNT] = { 0, 0 }; + +void gpsRescueInit(void) +{ +} + +void gpsRescueUpdate(void) +// runs at gpsRescueTaskIntervalSeconds, and runs whether or not rescue is active +{ +} + +float gpsRescueGetYawRate(void) +{ + return 0.0f; // the control yaw value for rc.c to be used while flightMode gps_rescue is active. +} + +float gpsRescueGetImuYawCogGain(void) +{ + return 1.0f; +} + +bool gpsRescueIsConfigured(void) +{ + return false; +} + +bool gpsRescueIsAvailable(void) +{ + return false; +} + +bool gpsRescueIsDisabled(void) +{ + return true; +} + +#ifdef USE_MAG +bool gpsRescueDisableMag(void) +{ + return true; +} +#endif + +#endif // USE_GPS_RESCUE + +#endif // USE_WING diff --git a/src/main/flight/gps_rescue_wing.h b/src/main/flight/gps_rescue_wing.h new file mode 100644 index 0000000000..fbaa001236 --- /dev/null +++ b/src/main/flight/gps_rescue_wing.h @@ -0,0 +1,41 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifdef USE_WING + +#include + +#include "common/axis.h" + +#include "pg/gps_rescue.h" + +#define TASK_GPS_RESCUE_RATE_HZ 100 // in sync with altitude task rate + +extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES + +void gpsRescueInit(void); +void gpsRescueUpdate(void); +float gpsRescueGetYawRate(void); +bool gpsRescueIsConfigured(void); +bool gpsRescueIsAvailable(void); +bool gpsRescueIsDisabled(void); +bool gpsRescueDisableMag(void); +float gpsRescueGetImuYawCogGain(void); + +#endif // USE_WING diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 094ff0aea4..4d5f0dad80 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -570,7 +570,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_ #ifdef USE_GPS_RESCUE angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch #endif -#ifdef USE_POSITION_HOLD +#if defined(USE_POSITION_HOLD) && !defined(USE_WING) if (FLIGHT_MODE(POS_HOLD_MODE)) { angleFeedforward = 0.0f; // otherwise the lag of the PT3 carries recent stick inputs into the hold if (isAutopilotInControl()) { @@ -1127,7 +1127,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim || FLIGHT_MODE(ALT_HOLD_MODE) // todo - check if this is needed #endif #ifdef USE_POSITION_HOLD - || FLIGHT_MODE(POS_HOLD_MODE) + || FLIGHT_MODE(POS_HOLD_MODE) #endif ; levelMode_e levelMode; diff --git a/src/main/flight/pos_hold.h b/src/main/flight/pos_hold.h index 97ddb7bdbd..a777c05c0d 100644 --- a/src/main/flight/pos_hold.h +++ b/src/main/flight/pos_hold.h @@ -17,17 +17,5 @@ #pragma once -// #include "pg/pos_hold.h" - -#ifdef USE_POSITION_HOLD -#include "common/time.h" -#include "io/gps.h" - -#define POSHOLD_TASK_RATE_HZ 100 // hz - -void posHoldInit(void); -void updatePosHold(timeUs_t currentTimeUs); - -bool posHoldFailure(void); - -#endif +#include "flight/pos_hold_multirotor.h" +#include "flight/pos_hold_wing.h" diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold_multirotor.c similarity index 95% rename from src/main/flight/pos_hold.c rename to src/main/flight/pos_hold_multirotor.c index 38e07a8455..a3723885e4 100644 --- a/src/main/flight/pos_hold.c +++ b/src/main/flight/pos_hold_multirotor.c @@ -17,6 +17,8 @@ #include "platform.h" +#ifndef USE_WING + #ifdef USE_POSITION_HOLD #include "math.h" @@ -55,7 +57,7 @@ void posHoldInit(void) void posHoldCheckSticks(void) { - // if failsafe is active, eg landing mode, don't update the original start point + // if failsafe is active, eg landing mode, don't update the original start point if (!failsafeIsActive() && posHold.useStickAdjustment) { const bool sticksDeflected = (getRcDeflectionAbs(FD_ROLL) > posHold.deadband) || (getRcDeflectionAbs(FD_PITCH) > posHold.deadband); setSticksActiveStatus(sticksDeflected); @@ -65,20 +67,20 @@ void posHoldCheckSticks(void) bool sensorsOk(void) { if (!STATE(GPS_FIX)) { - return false; + return false; } if ( #ifdef USE_MAG !compassIsHealthy() && #endif (!posHoldConfig()->pos_hold_without_mag || !canUseGPSHeading)) { - return false; + return false; } return true; } void updatePosHold(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); + UNUSED(currentTimeUs); if (FLIGHT_MODE(POS_HOLD_MODE)) { if (!posHold.isEnabled) { resetPositionControl(&gpsSol.llh, POSHOLD_TASK_RATE_HZ); // sets target location to current location @@ -104,3 +106,5 @@ bool posHoldFailure(void) { } #endif // USE_POS_HOLD + +#endif // !USE_WING diff --git a/src/main/flight/pos_hold_multirotor.h b/src/main/flight/pos_hold_multirotor.h new file mode 100644 index 0000000000..9b3e078419 --- /dev/null +++ b/src/main/flight/pos_hold_multirotor.h @@ -0,0 +1,37 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifndef USE_WING + +// #include "pg/pos_hold.h" + +#ifdef USE_POSITION_HOLD +#include "common/time.h" +#include "io/gps.h" + +#define POSHOLD_TASK_RATE_HZ 100 // hz + +void posHoldInit(void); +void updatePosHold(timeUs_t currentTimeUs); + +bool posHoldFailure(void); + +#endif // USE_POSITION_HOLD + +#endif // !USE_WING diff --git a/src/main/flight/pos_hold_wing.c b/src/main/flight/pos_hold_wing.c new file mode 100644 index 0000000000..fac189426f --- /dev/null +++ b/src/main/flight/pos_hold_wing.c @@ -0,0 +1,57 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#include "platform.h" + +#ifdef USE_WING + +#ifdef USE_POSITION_HOLD + +#include "math.h" +#include "build/debug.h" +#include "common/maths.h" + +#include "config/config.h" +#include "fc/core.h" +#include "fc/runtime_config.h" +#include "fc/rc.h" +#include "flight/autopilot.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/position.h" +#include "rx/rx.h" +#include "sensors/compass.h" + +#include "pg/pos_hold.h" +#include "pos_hold.h" + +void posHoldInit(void) +{ +} + +void updatePosHold(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); +} + +bool posHoldFailure(void) { + // used only to display warning in OSD if requested but failing + return true; +} + +#endif // USE_POS_HOLD + +#endif // USE_WING diff --git a/src/main/flight/pos_hold_wing.h b/src/main/flight/pos_hold_wing.h new file mode 100644 index 0000000000..3ab33e2c69 --- /dev/null +++ b/src/main/flight/pos_hold_wing.h @@ -0,0 +1,37 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifdef USE_WING + +// #include "pg/pos_hold.h" + +#ifdef USE_POSITION_HOLD +#include "common/time.h" +#include "io/gps.h" + +#define POSHOLD_TASK_RATE_HZ 100 // hz + +void posHoldInit(void); +void updatePosHold(timeUs_t currentTimeUs); + +bool posHoldFailure(void); + +#endif + +#endif // USE_WING diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index 3e7ea38890..08f46491e4 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -220,7 +220,8 @@ displayPort_t *displayPortMspInit(void) return &mspDisplayPort; } -void displayPortMspSetSerial(serialPortIdentifier_e serialPort) { +void displayPortMspSetSerial(serialPortIdentifier_e serialPort) +{ displayPortSerial = serialPort; } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 1f6ec0d45b..03dd8b4bf7 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -1588,4 +1588,16 @@ void setLedProfile(uint8_t profile) ledStripConfigMutable()->ledstrip_profile = profile; } } + +uint8_t getLedBrightness(void) +{ + return ledStripConfig()->ledstrip_brightness; +} + +void setLedBrightness(uint8_t brightness) +{ + if ( brightness <= 100 ) { + ledStripConfigMutable()->ledstrip_brightness = brightness; + } +} #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index fc6d467a6c..8e6aaccf17 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -239,3 +239,5 @@ void updateRequiredOverlay(void); uint8_t getLedProfile(void); void setLedProfile(uint8_t profile); +uint8_t getLedBrightness(void); +void setLedBrightness(uint8_t brightness); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 325d6ec152..a97d8260a5 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -70,6 +70,9 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = { #ifdef USE_VCP SERIAL_PORT_USB_VCP, #endif +#ifdef USE_UART0 + SERIAL_PORT_UART0, +#endif #ifdef USE_UART1 SERIAL_PORT_USART1, #endif @@ -115,6 +118,9 @@ const char* serialPortNames[SERIAL_PORT_COUNT] = { #ifdef USE_VCP "VCP", #endif +#ifdef USE_UART0 + "UART0", +#endif #ifdef USE_UART1 "UART1", #endif @@ -156,17 +162,24 @@ const char* serialPortNames[SERIAL_PORT_COUNT] = { #endif }; -const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, - 400000, 460800, 500000, 921600, 1000000, 1500000, 2000000, 2470000}; // see baudRate_e +const uint32_t baudRates[BAUD_COUNT] = { + 0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, + 400000, 460800, 500000, 921600, 1000000, 1500000, 2000000, 2470000 +}; // see baudRate_e static serialPortConfig_t* findInPortConfigs_identifier(const serialPortConfig_t cfgs[], size_t count, serialPortIdentifier_e identifier) { + if (identifier == SERIAL_PORT_NONE || identifier == SERIAL_PORT_ALL) { + return NULL; + } + for (unsigned i = 0; i < count; i++) { if (cfgs[i].identifier == identifier) { // drop const on return - wrapper function will add it back if necessary return (serialPortConfig_t*)&cfgs[i]; } } + return NULL; } diff --git a/src/main/io/serial.h b/src/main/io/serial.h index af10830fa2..4943597eaa 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -77,18 +77,29 @@ typedef enum { BAUD_COUNT } baudRate_e; -extern const uint32_t baudRates[]; +extern const uint32_t baudRates[BAUD_COUNT]; // serial port identifiers are now fixed, these values are used by MSP commands. typedef enum { SERIAL_PORT_ALL = -2, SERIAL_PORT_NONE = -1, - // prepare for transition to SERIAL_PORT_UART0 - SERIAL_PORT_UART_FIRST = 0, + SERIAL_PORT_LEGACY_START_IDENTIFIER = 0, + SERIAL_PORT_START_IDENTIFIER = 20, + SERIAL_PORT_USB_VCP = 20, + + SERIAL_PORT_SOFTSERIAL_FIRST = 30, + SERIAL_PORT_SOFTSERIAL1 = SERIAL_PORT_SOFTSERIAL_FIRST, + SERIAL_PORT_SOFTSERIAL2, + + SERIAL_PORT_LPUART_FIRST = 40, + SERIAL_PORT_LPUART1 = SERIAL_PORT_LPUART_FIRST, + #if SERIAL_UART_FIRST_INDEX == 0 + SERIAL_PORT_UART_FIRST = 50, SERIAL_PORT_UART0 = SERIAL_PORT_UART_FIRST, SERIAL_PORT_USART1, #else + SERIAL_PORT_UART_FIRST = 51, SERIAL_PORT_USART1 = SERIAL_PORT_UART_FIRST, #endif SERIAL_PORT_UART1 = SERIAL_PORT_USART1, @@ -108,14 +119,6 @@ typedef enum { SERIAL_PORT_USART10, SERIAL_PORT_UART10 = SERIAL_PORT_USART10, - SERIAL_PORT_USB_VCP = 20, - - SERIAL_PORT_SOFTSERIAL_FIRST = 30, - SERIAL_PORT_SOFTSERIAL1 = SERIAL_PORT_SOFTSERIAL_FIRST, - SERIAL_PORT_SOFTSERIAL2, - - SERIAL_PORT_LPUART_FIRST = 40, - SERIAL_PORT_LPUART1 = SERIAL_PORT_LPUART_FIRST, } serialPortIdentifier_e; // use value from target serial port normalization diff --git a/src/main/io/serial_resource.c b/src/main/io/serial_resource.c index 962e0d7d25..878968d0bf 100644 --- a/src/main/io/serial_resource.c +++ b/src/main/io/serial_resource.c @@ -60,9 +60,8 @@ serialType_e serialType(serialPortIdentifier_e identifier) } #endif #ifdef USE_SOFTSERIAL - if (identifier >= SERIAL_PORT_SOFTSERIAL_FIRST - && identifier < SERIAL_PORT_SOFTSERIAL_FIRST + SERIAL_SOFTSERIAL_MAX) { - // sotserials always start from first index, without holes + if (identifier >= SERIAL_PORT_SOFTSERIAL_FIRST && identifier < SERIAL_PORT_SOFTSERIAL_FIRST + SERIAL_SOFTSERIAL_MAX) { + // sotserials always start from 1, without holes return SERIALTYPE_SOFTSERIAL; } #endif diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index dcb6df0f75..d0bf9e563a 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -124,6 +124,7 @@ #include "pg/gps_rescue.h" #include "pg/gyrodev.h" #include "pg/motor.h" +#include "pg/pilot.h" #include "pg/pos_hold.h" #include "pg/rx.h" #include "pg/rx_spi.h" @@ -1158,12 +1159,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t break; case MSP_NAME: - { - const int nameLen = strlen(pilotConfig()->craftName); - for (int i = 0; i < nameLen; i++) { - sbufWriteU8(dst, pilotConfig()->craftName[i]); - } - } + sbufWriteString(dst, pilotConfig()->craftName); break; #ifdef USE_SERVOS @@ -1537,6 +1533,7 @@ case MSP_NAME: break; #ifdef USE_GPS_RESCUE +#ifndef USE_WING case MSP_GPS_RESCUE: sbufWriteU16(dst, gpsRescueConfig()->maxRescueAngle); sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM); @@ -1569,6 +1566,7 @@ case MSP_NAME: sbufWriteU16(dst, gpsRescueConfig()->velD); sbufWriteU16(dst, gpsRescueConfig()->yawP); break; +#endif // !USE_WING #endif #endif @@ -1806,7 +1804,7 @@ case MSP_NAME: case MSP_RC_DEADBAND: sbufWriteU8(dst, rcControlsConfig()->deadband); sbufWriteU8(dst, rcControlsConfig()->yaw_deadband); -#ifdef USE_POSITION_HOLD +#if defined(USE_POSITION_HOLD) && !defined(USE_WING) sbufWriteU8(dst, posHoldConfig()->pos_hold_deadband); #else sbufWriteU8(dst, 0); @@ -2610,9 +2608,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_ // type byte, then length byte followed by the actual characters sbufWriteU8(dst, textType); sbufWriteU8(dst, textLength); - for (unsigned int i = 0; i < textLength; i++) { - sbufWriteU8(dst, textVar[i]); - } + sbufWriteData(dst, textVar, textLength); } break; #ifdef USE_LED_STRIP @@ -2885,6 +2881,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #ifdef USE_GPS #ifdef USE_GPS_RESCUE +#ifndef USE_WING case MSP_SET_GPS_RESCUE: gpsRescueConfigMutable()->maxRescueAngle = sbufReadU16(src); gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src); @@ -2922,6 +2919,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, gpsRescueConfigMutable()->velD = sbufReadU16(src); gpsRescueConfigMutable()->yawP = sbufReadU16(src); break; +#endif // !USE_WING #endif #endif @@ -2977,7 +2975,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, case MSP_SET_RC_DEADBAND: rcControlsConfigMutable()->deadband = sbufReadU8(src); rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); -#ifdef USE_POSITION_HOLD +#if defined(USE_POSITION_HOLD) && !defined(USE_WING) posHoldConfigMutable()->pos_hold_deadband = sbufReadU8(src); #else sbufReadU8(src); @@ -3895,7 +3893,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, return MSP_RESULT_ERROR; } - portConfig->identifier = identifier; portConfig->functionMask = sbufReadU16(src); portConfig->msp_baudrateIndex = sbufReadU8(src); portConfig->gps_baudrateIndex = sbufReadU8(src); @@ -3923,7 +3920,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, return MSP_RESULT_ERROR; } - portConfig->identifier = identifier; portConfig->functionMask = sbufReadU32(src); portConfig->msp_baudrateIndex = sbufReadU8(src); portConfig->gps_baudrateIndex = sbufReadU8(src); @@ -3985,10 +3981,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #endif case MSP_SET_NAME: - memset(pilotConfigMutable()->craftName, 0, ARRAYLEN(pilotConfig()->craftName)); - for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) { - pilotConfigMutable()->craftName[i] = sbufReadU8(src); - } + memset(pilotConfigMutable()->craftName, 0, sizeof(pilotConfigMutable()->craftName)); + sbufReadData(src, pilotConfigMutable()->craftName, MIN(ARRAYLEN(pilotConfigMutable()->craftName) - 1, dataSize)); #ifdef USE_OSD osdAnalyzeActiveElements(); #endif @@ -4068,35 +4062,51 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, case MSP2_SET_TEXT: { // type byte, then length byte followed by the actual characters - const uint8_t textType = sbufReadU8(src); + const unsigned textType = sbufReadU8(src); char* textVar; - const uint8_t textLength = MIN(MAX_NAME_LENGTH, sbufReadU8(src)); + unsigned textSpace; switch (textType) { case MSP2TEXT_PILOT_NAME: textVar = pilotConfigMutable()->pilotName; + textSpace = sizeof(pilotConfigMutable()->pilotName) - 1; break; case MSP2TEXT_CRAFT_NAME: textVar = pilotConfigMutable()->craftName; + textSpace = sizeof(pilotConfigMutable()->craftName) - 1; break; case MSP2TEXT_PID_PROFILE_NAME: textVar = currentPidProfile->profileName; + textSpace = sizeof(currentPidProfile->profileName) - 1; break; case MSP2TEXT_RATE_PROFILE_NAME: textVar = currentControlRateProfile->profileName; + textSpace = sizeof(currentControlRateProfile->profileName) - 1; break; + case MSP2TEXT_CUSTOM_MSG_0: + case MSP2TEXT_CUSTOM_MSG_0 + 1: + case MSP2TEXT_CUSTOM_MSG_0 + 2: + case MSP2TEXT_CUSTOM_MSG_0 + 3: { + unsigned msgIdx = textType - MSP2TEXT_CUSTOM_MSG_0; + if (msgIdx < OSD_CUSTOM_MSG_COUNT) { + textVar = pilotConfigMutable()->message[msgIdx]; + textSpace = sizeof(pilotConfigMutable()->message[msgIdx]) - 1; + } else { + return MSP_RESULT_ERROR; + } + break; + } default: return MSP_RESULT_ERROR; } + const unsigned textLength = MIN(textSpace, sbufReadU8(src)); memset(textVar, 0, strlen(textVar)); - for (unsigned int i = 0; i < textLength; i++) { - textVar[i] = sbufReadU8(src); - } + sbufReadData(src, textVar, textLength); #ifdef USE_OSD if (textType == MSP2TEXT_PILOT_NAME || textType == MSP2TEXT_CRAFT_NAME) { diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h index f96cd6a817..beefb27f2f 100644 --- a/src/main/msp/msp_protocol_v2_betaflight.h +++ b/src/main/msp/msp_protocol_v2_betaflight.h @@ -38,3 +38,6 @@ #define MSP2TEXT_RATE_PROFILE_NAME 4 #define MSP2TEXT_BUILDKEY 5 #define MSP2TEXT_RELEASENAME 6 +#define MSP2TEXT_CUSTOM_MSG_0 7 // CUSTOM_MSG_MAX_NUM entries are allocated +#define CUSTOM_MSG_MAX_NUM 4 +// next new variable type must be >= MSP2TEXT_CUSTOM_MSG_0 + CUSTOM_MSG_MAX_NUM (11) diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h index c7cfbc19d5..c311bb1f45 100644 --- a/src/main/osd/osd.h +++ b/src/main/osd/osd.h @@ -190,6 +190,10 @@ typedef enum { OSD_GPS_LAP_TIME_PREVIOUS, OSD_GPS_LAP_TIME_BEST3, OSD_DEBUG2, + OSD_CUSTOM_MSG0, + OSD_CUSTOM_MSG1, + OSD_CUSTOM_MSG2, + OSD_CUSTOM_MSG3, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 51e43cff53..23ddd5c9e4 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -161,6 +161,7 @@ #include "osd/osd_warnings.h" #include "pg/motor.h" +#include "pg/pilot.h" #include "pg/stats.h" #include "rx/rx.h" @@ -810,6 +811,18 @@ static void osdElementCompassBar(osdElementParms_t *element) element->buff[9] = 0; } +//display custom message from MSPv2 +static void osdElementCustomMsg(osdElementParms_t *element) +{ + int msgIndex = element->item - OSD_CUSTOM_MSG0; + if (msgIndex < 0 || msgIndex >= OSD_CUSTOM_MSG_COUNT || pilotConfig()->message[msgIndex][0] == '\0') { + tfp_sprintf(element->buff, "CUSTOM_MSG%d", msgIndex + 1); + } else { + strncpy(element->buff, pilotConfig()->message[msgIndex], MAX_NAME_LENGTH); + element->buff[MAX_NAME_LENGTH] = 0; // terminate maximum-length string + } +} + #ifdef USE_ADC_INTERNAL static void osdElementCoreTemperature(osdElementParms_t *element) { @@ -1804,6 +1817,10 @@ static const uint8_t osdElementDisplayOrder[] = { OSD_MAH_DRAWN, OSD_WATT_HOURS_DRAWN, OSD_CRAFT_NAME, + OSD_CUSTOM_MSG0, + OSD_CUSTOM_MSG1, + OSD_CUSTOM_MSG2, + OSD_CUSTOM_MSG3, OSD_ALTITUDE, OSD_ROLL_PIDS, OSD_PITCH_PIDS, @@ -1902,6 +1919,10 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = { [OSD_ITEM_TIMER_2] = osdElementTimer, [OSD_FLYMODE] = osdElementFlymode, [OSD_CRAFT_NAME] = NULL, // only has background + [OSD_CUSTOM_MSG0] = osdElementCustomMsg, + [OSD_CUSTOM_MSG1] = osdElementCustomMsg, + [OSD_CUSTOM_MSG2] = osdElementCustomMsg, + [OSD_CUSTOM_MSG3] = osdElementCustomMsg, [OSD_THROTTLE_POS] = osdElementThrottlePosition, #ifdef USE_VTX_COMMON [OSD_VTX_CHANNEL] = osdElementVtxChannel, diff --git a/src/main/pg/alt_hold.h b/src/main/pg/alt_hold.h index 9c24f6c275..869cd4c8d1 100644 --- a/src/main/pg/alt_hold.h +++ b/src/main/pg/alt_hold.h @@ -21,13 +21,5 @@ #pragma once -#include - -#include "pg/pg.h" - -typedef struct altHoldConfig_s { - uint8_t alt_hold_adjust_rate; - uint8_t alt_hold_deadband; -} altHoldConfig_t; - -PG_DECLARE(altHoldConfig_t, altHoldConfig); +#include "pg/alt_hold_multirotor.h" +#include "pg/alt_hold_wing.h" diff --git a/src/main/pg/alt_hold.c b/src/main/pg/alt_hold_multirotor.c similarity index 96% rename from src/main/pg/alt_hold.c rename to src/main/pg/alt_hold_multirotor.c index 716fd18a50..c229fcf66a 100644 --- a/src/main/pg/alt_hold.c +++ b/src/main/pg/alt_hold_multirotor.c @@ -21,6 +21,8 @@ #include "platform.h" +#ifndef USE_WING + #ifdef USE_ALTITUDE_HOLD #include "flight/alt_hold.h" @@ -37,3 +39,5 @@ PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, .alt_hold_deadband = 20, // throttle deadband in percent of stick travel ); #endif + +#endif // USE_WING diff --git a/src/main/pg/alt_hold_multirotor.h b/src/main/pg/alt_hold_multirotor.h new file mode 100644 index 0000000000..88f0933b0c --- /dev/null +++ b/src/main/pg/alt_hold_multirotor.h @@ -0,0 +1,37 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#ifndef USE_WING + +#include + +#include "pg/pg.h" + +typedef struct altHoldConfig_s { + uint8_t alt_hold_adjust_rate; + uint8_t alt_hold_deadband; +} altHoldConfig_t; + +PG_DECLARE(altHoldConfig_t, altHoldConfig); + +#endif // !USE_WING diff --git a/src/main/pg/alt_hold_wing.c b/src/main/pg/alt_hold_wing.c new file mode 100644 index 0000000000..7b59cbce5a --- /dev/null +++ b/src/main/pg/alt_hold_wing.c @@ -0,0 +1,41 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_WING + +#ifdef USE_ALTITUDE_HOLD + +#include "flight/alt_hold.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "alt_hold.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 4); + +PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, +); +#endif + +#endif // USE_WING diff --git a/src/main/pg/alt_hold_wing.h b/src/main/pg/alt_hold_wing.h new file mode 100644 index 0000000000..d3ca1754a0 --- /dev/null +++ b/src/main/pg/alt_hold_wing.h @@ -0,0 +1,36 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#ifdef USE_WING + +#include + +#include "pg/pg.h" + +typedef struct altHoldConfig_s { + uint8_t dummy; +} altHoldConfig_t; + +PG_DECLARE(altHoldConfig_t, altHoldConfig); + +#endif // USE_WING diff --git a/src/main/pg/autopilot.h b/src/main/pg/autopilot.h index 018498dfba..720dcea2e3 100644 --- a/src/main/pg/autopilot.h +++ b/src/main/pg/autopilot.h @@ -21,26 +21,5 @@ #pragma once -#include - -#include "pg/pg.h" - -typedef struct apConfig_s { - uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres - uint16_t hover_throttle; // value used at the start of a rescue or position hold - uint16_t throttle_min; - uint16_t throttle_max; - uint8_t altitude_P; - uint8_t altitude_I; - uint8_t altitude_D; - uint8_t altitude_F; - uint8_t position_P; - uint8_t position_I; - uint8_t position_D; - uint8_t position_A; - uint8_t position_cutoff; - uint8_t max_angle; -} apConfig_t; - -PG_DECLARE(apConfig_t, apConfig); - +#include "pg/autopilot_multirotor.h" +#include "pg/autopilot_wing.h" diff --git a/src/main/pg/autopilot.c b/src/main/pg/autopilot_multirotor.c similarity index 97% rename from src/main/pg/autopilot.c rename to src/main/pg/autopilot_multirotor.c index 6b0fa1564b..898cfdc205 100644 --- a/src/main/pg/autopilot.c +++ b/src/main/pg/autopilot_multirotor.c @@ -21,6 +21,8 @@ #include "platform.h" +#ifndef USE_WING + #include "flight/autopilot.h" #include "pg/pg.h" @@ -46,3 +48,5 @@ PG_RESET_TEMPLATE(apConfig_t, apConfig, .position_cutoff = 80, .max_angle = 50, ); + +#endif // !USE_WING diff --git a/src/main/pg/autopilot_multirotor.h b/src/main/pg/autopilot_multirotor.h new file mode 100644 index 0000000000..6e1f6cfecd --- /dev/null +++ b/src/main/pg/autopilot_multirotor.h @@ -0,0 +1,49 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#ifndef USE_WING + +#include + +#include "pg/pg.h" + +typedef struct apConfig_s { + uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres + uint16_t hover_throttle; // value used at the start of a rescue or position hold + uint16_t throttle_min; + uint16_t throttle_max; + uint8_t altitude_P; + uint8_t altitude_I; + uint8_t altitude_D; + uint8_t altitude_F; + uint8_t position_P; + uint8_t position_I; + uint8_t position_D; + uint8_t position_A; + uint8_t position_cutoff; + uint8_t max_angle; +} apConfig_t; + +PG_DECLARE(apConfig_t, apConfig); + +#endif // !USE_WING diff --git a/src/main/pg/autopilot_wing.c b/src/main/pg/autopilot_wing.c new file mode 100644 index 0000000000..06dc476c1c --- /dev/null +++ b/src/main/pg/autopilot_wing.c @@ -0,0 +1,38 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_WING + +#include "flight/autopilot.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "autopilot.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(apConfig_t, apConfig, PG_AUTOPILOT, 2); + +PG_RESET_TEMPLATE(apConfig_t, apConfig, +); + +#endif // USE_WING diff --git a/src/main/pg/autopilot_wing.h b/src/main/pg/autopilot_wing.h new file mode 100644 index 0000000000..ee01e0a6f9 --- /dev/null +++ b/src/main/pg/autopilot_wing.h @@ -0,0 +1,36 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#ifdef USE_WING + +#include + +#include "pg/pg.h" + +typedef struct apConfig_s { + uint8_t dummy; +} apConfig_t; + +PG_DECLARE(apConfig_t, apConfig); + +#endif // USE_WING diff --git a/src/main/pg/gps_rescue.h b/src/main/pg/gps_rescue.h index b2bfc3c83d..558d891127 100644 --- a/src/main/pg/gps_rescue.h +++ b/src/main/pg/gps_rescue.h @@ -1,50 +1,21 @@ /* - * This file is part of Cleanflight and Betaflight. + * This file is part of Betaflight. * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . + * along with Betaflight. If not, see . */ #pragma once -#include - -#include "pg/pg.h" - -typedef struct gpsRescue_s { - - uint16_t maxRescueAngle; // degrees - uint16_t returnAltitudeM; // meters - uint16_t descentDistanceM; // meters - uint16_t groundSpeedCmS; // centimeters per second - uint8_t yawP; - uint8_t minSats; - uint8_t velP, velI, velD; - uint16_t minStartDistM; // meters - uint8_t sanityChecks; - uint8_t allowArmingWithoutFix; - uint8_t useMag; - uint8_t altitudeMode; - uint16_t ascendRate; - uint16_t descendRate; - uint16_t initialClimbM; // meters - uint8_t rollMix; - uint8_t disarmThreshold; - uint8_t pitchCutoffHz; - uint8_t imuYawGain; -} gpsRescueConfig_t; - -PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); +#include "pg/gps_rescue_multirotor.h" +#include "pg/gps_rescue_wing.h" diff --git a/src/main/pg/gps_rescue.c b/src/main/pg/gps_rescue_multirotor.c similarity index 97% rename from src/main/pg/gps_rescue.c rename to src/main/pg/gps_rescue_multirotor.c index 1fd66bbbeb..7767d9bc5c 100644 --- a/src/main/pg/gps_rescue.c +++ b/src/main/pg/gps_rescue_multirotor.c @@ -20,6 +20,8 @@ #include "platform.h" +#ifndef USE_WING + #ifdef USE_GPS_RESCUE #include "flight/gps_rescue.h" @@ -62,3 +64,5 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, ); #endif // USE_GPS_RESCUE + +#endif // !USE_WING diff --git a/src/main/pg/gps_rescue_multirotor.h b/src/main/pg/gps_rescue_multirotor.h new file mode 100644 index 0000000000..28ff8b52ae --- /dev/null +++ b/src/main/pg/gps_rescue_multirotor.h @@ -0,0 +1,54 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#ifndef USE_WING + +#include + +#include "pg/pg.h" + +typedef struct gpsRescue_s { + + uint16_t maxRescueAngle; // degrees + uint16_t returnAltitudeM; // meters + uint16_t descentDistanceM; // meters + uint16_t groundSpeedCmS; // centimeters per second + uint8_t yawP; + uint8_t minSats; + uint8_t velP, velI, velD; + uint16_t minStartDistM; // meters + uint8_t sanityChecks; + uint8_t allowArmingWithoutFix; + uint8_t useMag; + uint8_t altitudeMode; + uint16_t ascendRate; + uint16_t descendRate; + uint16_t initialClimbM; // meters + uint8_t rollMix; + uint8_t disarmThreshold; + uint8_t pitchCutoffHz; + uint8_t imuYawGain; +} gpsRescueConfig_t; + +PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); + +#endif // !USE_WING diff --git a/src/main/pg/gps_rescue_wing.c b/src/main/pg/gps_rescue_wing.c new file mode 100644 index 0000000000..cea370130a --- /dev/null +++ b/src/main/pg/gps_rescue_wing.c @@ -0,0 +1,40 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#include "platform.h" + +#ifdef USE_WING + +#ifdef USE_GPS_RESCUE + +#include "flight/gps_rescue.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "gps_rescue.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 7); + +PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, + .allowArmingWithoutFix = false, + .minSats = 8, +); + +#endif // USE_GPS_RESCUE + +#endif // USE_WING diff --git a/src/main/pg/gps_rescue_wing.h b/src/main/pg/gps_rescue_wing.h new file mode 100644 index 0000000000..b53ff6ba48 --- /dev/null +++ b/src/main/pg/gps_rescue_wing.h @@ -0,0 +1,33 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Betaflight. If not, see . + */ + +#pragma once + +#ifdef USE_WING + +#include + +#include "pg/pg.h" + +typedef struct gpsRescue_s { + uint8_t allowArmingWithoutFix; + uint8_t minSats; +} gpsRescueConfig_t; + +PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); + +#endif // USE_WING diff --git a/src/main/pg/pilot.c b/src/main/pg/pilot.c new file mode 100644 index 0000000000..4691cd00a4 --- /dev/null +++ b/src/main/pg/pilot.c @@ -0,0 +1,38 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "pilot.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 2); + +PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig, + .craftName = { 0 }, + .pilotName = { 0 }, + .message = { {0} }, +); diff --git a/src/main/pg/pilot.h b/src/main/pg/pilot.h new file mode 100644 index 0000000000..644016f7c0 --- /dev/null +++ b/src/main/pg/pilot.h @@ -0,0 +1,35 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#include "pg/pg.h" + +#define MAX_NAME_LENGTH 16u +#define OSD_CUSTOM_MSG_COUNT 4 + +typedef struct pilotConfig_s { + char craftName[MAX_NAME_LENGTH + 1]; + char pilotName[MAX_NAME_LENGTH + 1]; + char message[OSD_CUSTOM_MSG_COUNT][MAX_NAME_LENGTH + 1]; +} pilotConfig_t; + +PG_DECLARE(pilotConfig_t, pilotConfig); diff --git a/src/main/pg/pos_hold.h b/src/main/pg/pos_hold.h index eedf9e1a3d..c4019ad42f 100644 --- a/src/main/pg/pos_hold.h +++ b/src/main/pg/pos_hold.h @@ -21,13 +21,5 @@ #pragma once -#include - -#include "pg/pg.h" - -typedef struct posHoldConfig_s { - bool pos_hold_without_mag; - uint8_t pos_hold_deadband; -} posHoldConfig_t; - -PG_DECLARE(posHoldConfig_t, posHoldConfig); +#include "pg/pos_hold_multirotor.h" +#include "pg/pos_hold_wing.h" diff --git a/src/main/pg/pos_hold.c b/src/main/pg/pos_hold_multirotor.c similarity index 97% rename from src/main/pg/pos_hold.c rename to src/main/pg/pos_hold_multirotor.c index df291446ab..2a7a332241 100644 --- a/src/main/pg/pos_hold.c +++ b/src/main/pg/pos_hold_multirotor.c @@ -21,6 +21,8 @@ #include "platform.h" +#ifndef USE_WING + #ifdef USE_POSITION_HOLD #include "flight/pos_hold.h" @@ -37,3 +39,5 @@ PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, .pos_hold_deadband = 5, // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks ); #endif + +#endif // !USE_WING diff --git a/src/main/pg/pos_hold_multirotor.h b/src/main/pg/pos_hold_multirotor.h new file mode 100644 index 0000000000..6187d9ba62 --- /dev/null +++ b/src/main/pg/pos_hold_multirotor.h @@ -0,0 +1,37 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#ifndef USE_WING + +#include + +#include "pg/pg.h" + +typedef struct posHoldConfig_s { + bool pos_hold_without_mag; + uint8_t pos_hold_deadband; +} posHoldConfig_t; + +PG_DECLARE(posHoldConfig_t, posHoldConfig); + +#endif // !USE_WING diff --git a/src/main/pg/pos_hold_wing.c b/src/main/pg/pos_hold_wing.c new file mode 100644 index 0000000000..f90579f534 --- /dev/null +++ b/src/main/pg/pos_hold_wing.c @@ -0,0 +1,41 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_WING + +#ifdef USE_POSITION_HOLD + +#include "flight/pos_hold.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "pos_hold.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 0); + +PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, +); +#endif + +#endif // USE_WING diff --git a/src/main/pg/pos_hold_wing.h b/src/main/pg/pos_hold_wing.h new file mode 100644 index 0000000000..c194a488a9 --- /dev/null +++ b/src/main/pg/pos_hold_wing.h @@ -0,0 +1,36 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#ifdef USE_WING + +#include + +#include "pg/pg.h" + +typedef struct posHoldConfig_s { + uint8_t dummy; +} posHoldConfig_t; + +PG_DECLARE(posHoldConfig_t, posHoldConfig); + +#endif // !USE_WING diff --git a/src/main/pg/serial_uart.c b/src/main/pg/serial_uart.c index 355d28170a..3bec1a3804 100644 --- a/src/main/pg/serial_uart.c +++ b/src/main/pg/serial_uart.c @@ -46,6 +46,9 @@ typedef struct uartDmaopt_s { } uartDmaopt_t; static const uartDmaopt_t uartDmaopt[] = { +#ifdef USE_UART0 + { SERIAL_PORT_UART0, UART0_TX_DMA_OPT, UART0_RX_DMA_OPT }, +#endif #ifdef USE_UART1 { SERIAL_PORT_USART1, UART1_TX_DMA_OPT, UART1_RX_DMA_OPT }, #endif diff --git a/src/main/target/common_defaults_post.h b/src/main/target/common_defaults_post.h index 18ac19fd59..820eba0677 100644 --- a/src/main/target/common_defaults_post.h +++ b/src/main/target/common_defaults_post.h @@ -211,6 +211,15 @@ #endif #endif +#ifdef USE_UART0 +#ifndef UART0_TX_DMA_OPT +#define UART0_TX_DMA_OPT (DMA_OPT_UNUSED) +#endif +#ifndef UART0_RX_DMA_OPT +#define UART0_RX_DMA_OPT (DMA_OPT_UNUSED) +#endif +#endif + #ifdef USE_UART1 #ifndef UART1_TX_DMA_OPT #define UART1_TX_DMA_OPT (DMA_OPT_UNUSED) diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 37a55e7723..508453ab7e 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -555,14 +555,20 @@ #undef USED_TIMERS #endif -#if !defined(USE_RANGEFINDER) -#undef USE_RANGEFINDER_HCSR04 -#undef USE_RANGEFINDER_SRF10 -#undef USE_RANGEFINDER_HCSR04_I2C -#undef USE_RANGEFINDER_VL53L0X -#undef USE_RANGEFINDER_UIB -#undef USE_RANGEFINDER_TF +#if defined(USE_OPTICALFLOW_MT) +#ifndef USE_RANGEFINDER_MT +#define USE_RANGEFINDER_MT #endif +#ifndef USE_OPTICALFLOW +#define USE_OPTICALFLOW +#endif +#endif // USE_OPTICALFLOW_MT + +#if defined(USE_RANGEFINDER_HCSR04) || defined(USE_RANGEFINDER_SRF10) || defined(USE_RANGEFINDER_HCSR04_I2C) || defined(USE_RANGEFINDER_VL53L0X) || defined(USE_RANGEFINDER_UIB) || defined(USE_RANGEFINDER_TF) || defined(USE_RANGEFINDER_MT) +#ifndef USE_RANGEFINDER +#define USE_RANGEFINDER +#endif +#endif // USE_RANGEFINDER_XXX #ifndef USE_GPS_RESCUE #undef USE_CMS_GPS_RESCUE_MENU diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 0e7e6545d3..0db3a0d1dc 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -270,8 +270,9 @@ #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04 #define USE_RANGEFINDER_TF +#define USE_OPTICALFLOW_MT -#endif // TARGET_FLASH_SIZE > 512 +#endif // TARGET_FLASH_SIZE >= 1024 #endif // !defined(CLOUD_BUILD) @@ -482,3 +483,7 @@ #undef USE_RUNAWAY_TAKEOFF #endif // USE_WING + +#if defined(USE_POSITION_HOLD) && !defined(USE_GPS) +#error "USE_POSITION_HOLD requires USE_GPS to be defined" +#endif diff --git a/src/platform/STM32/timer_stm32h5xx.c b/src/platform/STM32/timer_stm32h5xx.c new file mode 100644 index 0000000000..e4d06b9335 --- /dev/null +++ b/src/platform/STM32/timer_stm32h5xx.c @@ -0,0 +1,232 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_TIMER + +#include "common/utils.h" + +#include "drivers/dma.h" +#include "drivers/io.h" +#include "timer_def.h" + +#include "stm32h5xx.h" +#include "drivers/rcc.h" +#include "drivers/timer.h" + +const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn}, + { .TIMx = TIM2, .rcc = RCC_APB1L(TIM2), .inputIrq = TIM2_IRQn}, + { .TIMx = TIM3, .rcc = RCC_APB1L(TIM3), .inputIrq = TIM3_IRQn}, + { .TIMx = TIM4, .rcc = RCC_APB1L(TIM4), .inputIrq = TIM4_IRQn}, + { .TIMx = TIM5, .rcc = RCC_APB1L(TIM5), .inputIrq = TIM5_IRQn}, + { .TIMx = TIM6, .rcc = RCC_APB1L(TIM6), .inputIrq = TIM6_DAC_IRQn}, + { .TIMx = TIM7, .rcc = RCC_APB1L(TIM7), .inputIrq = TIM7_IRQn}, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn}, + { .TIMx = TIM12, .rcc = RCC_APB1L(TIM12), .inputIrq = TIM8_BRK_TIM12_IRQn}, + { .TIMx = TIM13, .rcc = RCC_APB1L(TIM13), .inputIrq = TIM8_UP_TIM13_IRQn}, + { .TIMx = TIM14, .rcc = RCC_APB1L(TIM14), .inputIrq = TIM8_TRG_COM_TIM14_IRQn}, + { .TIMx = TIM15, .rcc = RCC_APB2(TIM15), .inputIrq = TIM15_IRQn}, + { .TIMx = TIM16, .rcc = RCC_APB2(TIM16), .inputIrq = TIM16_IRQn}, + { .TIMx = TIM17, .rcc = RCC_APB2(TIM17), .inputIrq = TIM17_IRQn}, +}; + +#if defined(USE_TIMER_MGMT) +const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = { +// Auto-generated from 'timer_def.h' +// Port A + DEF_TIM(TIM2, CH1, PA0, 0, 0, 0), // AF1 + DEF_TIM(TIM2, CH2, PA1, 0, 0, 0), + DEF_TIM(TIM2, CH3, PA2, 0, 0, 0), + DEF_TIM(TIM2, CH4, PA3, 0, 0, 0), + DEF_TIM(TIM2, CH1, PA5, 0, 0, 0), + DEF_TIM(TIM1, CH1N, PA7, 0, 0, 0), + DEF_TIM(TIM1, CH1, PA8, 0, 0, 0), + DEF_TIM(TIM1, CH2, PA9, 0, 0, 0), + DEF_TIM(TIM1, CH3, PA10, 0, 0, 0), + DEF_TIM(TIM1, CH4, PA11, 0, 0, 0), + DEF_TIM(TIM2, CH1, PA15, 0, 0, 0), + + DEF_TIM(TIM5, CH1, PA0, 0, 0, 0), // AF2 + DEF_TIM(TIM5, CH2, PA1, 0, 0, 0), + DEF_TIM(TIM5, CH3, PA2, 0, 0, 0), + DEF_TIM(TIM5, CH4, PA3, 0, 0, 0), + DEF_TIM(TIM3, CH1, PA6, 0, 0, 0), + DEF_TIM(TIM3, CH2, PA7, 0, 0, 0), + + DEF_TIM(TIM8, CH1N, PA5, 0, 0, 0), // AF3 + DEF_TIM(TIM8, CH1N, PA7, 0, 0, 0), + + DEF_TIM(TIM13, CH1, PA6, 0, 0, 0), // AF9 + DEF_TIM(TIM14, CH1, PA7, 0, 0, 0), + + DEF_TIM(TIM15, CH1N, PA1, 0, 0, 0), // AF4 + DEF_TIM(TIM15, CH1, PA2, 0, 0, 0), + DEF_TIM(TIM15, CH2, PA3, 0, 0, 0), + +// Port B + DEF_TIM(TIM1, CH2N, PB0, 0, 0, 0), // AF1 + DEF_TIM(TIM1, CH3N, PB1, 0, 0, 0), + DEF_TIM(TIM2, CH2, PB3, 0, 0, 0), + DEF_TIM(TIM16, CH1N, PB6, 0, 0, 0), + DEF_TIM(TIM17, CH1N, PB7, 0, 0, 0), + DEF_TIM(TIM16, CH1, PB8, 0, 0, 0), + DEF_TIM(TIM17, CH1, PB9, 0, 0, 0), + DEF_TIM(TIM2, CH3, PB10, 0, 0, 0), + DEF_TIM(TIM2, CH4, PB11, 0, 0, 0), + DEF_TIM(TIM1, CH1N, PB13, 0, 0, 0), + DEF_TIM(TIM1, CH2N, PB14, 0, 0, 0), + DEF_TIM(TIM1, CH3N, PB15, 0, 0, 0), + + DEF_TIM(TIM3, CH3, PB0, 0, 0, 0), // AF2 + DEF_TIM(TIM3, CH4, PB1, 0, 0, 0), + DEF_TIM(TIM3, CH1, PB4, 0, 0, 0), + DEF_TIM(TIM3, CH2, PB5, 0, 0, 0), + DEF_TIM(TIM4, CH1, PB6, 0, 0, 0), + DEF_TIM(TIM4, CH2, PB7, 0, 0, 0), + DEF_TIM(TIM4, CH3, PB8, 0, 0, 0), + DEF_TIM(TIM4, CH4, PB9, 0, 0, 0), + + // DEF_TIM(TIM12, CH1, PB14, 0, 0, 0), // AF9 (SDMMC2_D0) + // DEF_TIM(TIM12, CH2, PB15, 0, 0, 0), // AF9 (SDMMC2_D1) + +// Port C + DEF_TIM(TIM3, CH1, PC6, 0, 0, 0), // AF2 + DEF_TIM(TIM3, CH2, PC7, 0, 0, 0), + DEF_TIM(TIM3, CH3, PC8, 0, 0, 0), + DEF_TIM(TIM3, CH4, PC9, 0, 0, 0), + + DEF_TIM(TIM8, CH1, PC6, 0, 0, 0), // AF3 + DEF_TIM(TIM8, CH2, PC7, 0, 0, 0), + DEF_TIM(TIM8, CH3, PC8, 0, 0, 0), + DEF_TIM(TIM8, CH4, PC9, 0, 0, 0), + +// Port D + DEF_TIM(TIM1, CH4N, PD5, 0, 0, 0), // AF1 (ADDED) + + DEF_TIM(TIM4, CH1, PD12, 0, 0, 0), // AF2 + DEF_TIM(TIM4, CH2, PD13, 0, 0, 0), + DEF_TIM(TIM4, CH3, PD14, 0, 0, 0), + DEF_TIM(TIM4, CH4, PD15, 0, 0, 0), + +// Port E + DEF_TIM(TIM1, CH1N, PE8, 0, 0, 0), // AF1 + DEF_TIM(TIM1, CH1, PE9, 0, 0, 0), + DEF_TIM(TIM1, CH2N, PE10, 0, 0, 0), + DEF_TIM(TIM1, CH2, PE11, 0, 0, 0), + DEF_TIM(TIM1, CH3N, PE12, 0, 0, 0), + DEF_TIM(TIM1, CH3, PE13, 0, 0, 0), + DEF_TIM(TIM1, CH4, PE14, 0, 0, 0), + + DEF_TIM(TIM1, CH4N, PE15, 0, 0, 0), // AF3 (ADDED) + + DEF_TIM(TIM15, CH1N, PE4, 0, 0, 0), // AF4 + DEF_TIM(TIM15, CH1, PE5, 0, 0, 0), + DEF_TIM(TIM15, CH2, PE6, 0, 0, 0), + +// Port F + DEF_TIM(TIM16, CH1, PF6, 0, 0, 0), // AF1 + DEF_TIM(TIM17, CH1, PF7, 0, 0, 0), + DEF_TIM(TIM16, CH1N, PF8, 0, 0, 0), // AF3 + DEF_TIM(TIM17, CH1N, PF9, 0, 0, 0), + + DEF_TIM(TIM13, CH1, PF8, 0, 0, 0), // AF9 (WAS TIM13_CH1N) + DEF_TIM(TIM14, CH1, PF9, 0, 0, 0), // AF9 (WAS TIM14_CH1N) + +// Port H + + DEF_TIM(TIM1, CHN3, PH6, 0, 0, 0), // AF1 (ADDED) + DEF_TIM(TIM1, CH3, PH7, 0, 0, 0), + DEF_TIM(TIM1, CHN2, PH8, 0, 0, 0), + DEF_TIM(TIM1, CH2, PH9, 0, 0, 0), + DEF_TIM(TIM1, CHN1, PH10, 0, 0, 0), + DEF_TIM(TIM1, CH1, PH11, 0, 0, 0), + + DEF_TIM(TIM12, CH1, PH6, 0, 0, 0), // AF2 (ADDED) + DEF_TIM(TIM12, CH2, PH9, 0, 0, 0), + DEF_TIM(TIM5, CH1, PH10, 0, 0, 0), + DEF_TIM(TIM5, CH2, PH11, 0, 0, 0), + DEF_TIM(TIM5, CH3, PH12, 0, 0, 0), + + DEF_TIM(TIM8, CH1, PH6, 0, 0, 0), // AF3 (ADDED) + DEF_TIM(TIM8, CH1N, PH7, 0, 0, 0), + DEF_TIM(TIM8, CH2, PH8, 0, 0, 0), + DEF_TIM(TIM8, CH2N, PH9, 0, 0, 0), + DEF_TIM(TIM8, CH3, PH10, 0, 0, 0), + DEF_TIM(TIM8, CH3N, PH11, 0, 0, 0), + DEF_TIM(TIM8, CH1N, PH13, 0, 0, 0), + DEF_TIM(TIM8, CH2N, PH14, 0, 0, 0), + DEF_TIM(TIM8, CH3N, PH15, 0, 0, 0), + +// Port I + DEF_TIM(TIM5, CH4, PI0, 0, 0, 0), // AF2 (ADDED) + + DEF_TIM(TIM8, CH4, PI2, 0, 0, 0), // AF3 (ADDED) + DEF_TIM(TIM8, CH1, PI5, 0, 0, 0), + DEF_TIM(TIM8, CH2, PI6, 0, 0, 0), + DEF_TIM(TIM8, CH3, PI7, 0, 0, 0), +}; +#endif + + +uint32_t timerClock(const TIM_TypeDef *tim) +{ + int timpre; + uint32_t pclk; + uint32_t ppre; + + // This function is used to calculate the timer clock frequency. + // RM0481 (Rev 3) Table + + + RCC_ClkInitTypeDef clkConfig, uint32_t fLatency + HAL_RCC_GetClockConfig(&clkConfig, &fLatency); + + if ((uintptr_t)tim >= APB2PERIPH_BASE ) { // APB2 + pclk = HAL_RCC_GetPCLK2Freq(); + ppre = clkConfig.APB2CLKDivider; + } else { // all other timers are on APB1 + pclk = HAL_RCC_GetPCLK1Freq(); + ppre = clkConfig.APB1CLKDivider; + } + timpre = (RCC->CFGR & RCC_CFGR_TIMPRE) ? 1 : 0; +#define PC(m) (0x80 | (m)) // multiply pclk +#define HC(m) (0x00 | (m)) // multiply hclk + static const uint8_t timpreTab[2][8] = { // see RM0481 TIMPRE: timers clocks prescaler selection + // 1 1 1 1 2 4 8 16 + { HC(1), HC(1), HC(1), HC(1), HC(1), PC(2), PC(2), PC(2) }, // TIMPRE = 0 + { PC(2), PC(2), PC(2), PC(2), PC(2), PC(2), PC(4), PC(4) } // TIMPRE = 1 + } +#undef PC +#undef HC + int flagMult = timpreTab[timpre][ppre]; + + if (flagMult & 0x80) { // PCLK based + return pclk * (flagMult & 0x7f); + } else { + return HAL_RCC_GetHCLKFreq() * (flagMult & 0x7f); + } + + return pclk * periphToKernel[timpre][ppre]; + +} +#endif diff --git a/src/test/Makefile b/src/test/Makefile index ca4c302b83..b6d2153b97 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -37,8 +37,8 @@ alignsensor_unittest_SRC := \ $(USER_DIR)/common/vector.c althold_unittest_SRC := \ - $(USER_DIR)/flight/alt_hold.c \ - $(USER_DIR)/flight/autopilot.c \ + $(USER_DIR)/flight/alt_hold_multirotor.c \ + $(USER_DIR)/flight/autopilot_multirotor.c \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/common/vector.c \ $(USER_DIR)/common/filter.c \ @@ -56,8 +56,8 @@ arming_prevention_unittest_SRC := \ $(USER_DIR)/fc/rc_controls.c \ $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/fc/runtime_config.c \ - $(USER_DIR)/flight/autopilot.c \ - $(USER_DIR)/flight/gps_rescue.c + $(USER_DIR)/flight/autopilot_multirotor.c \ + $(USER_DIR)/flight/gps_rescue_multirotor.c arming_prevention_unittest_DEFINES := \ USE_GPS_RESCUE= diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index cb3243a771..c756929c8c 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -53,6 +53,7 @@ extern "C" { #include "pg/pg_ids.h" #include "pg/beeper.h" #include "pg/gps.h" + #include "pg/pilot.h" #include "pg/rx.h" #include "rx/rx.h" #include "scheduler/scheduler.h" @@ -62,7 +63,7 @@ extern "C" { void cliSet(const char *cmdName, char *cmdline); int cliGetSettingIndex(char *name, uint8_t length); void *cliGetValuePointer(const clivalue_t *value); - + const clivalue_t valueTable[] = { { .name = "array_unit_test", .type = VAR_INT8 | MODE_ARRAY | MASTER_VALUE, .config = { .array = { .length = 3}}, .pgn = PG_RESERVED_FOR_TESTING_1, .offset = 0 }, { .name = "str_unit_test", .type = VAR_UINT8 | MODE_STRING | MASTER_VALUE, .config = { .string = { 0, 16, 0 }}, .pgn = PG_RESERVED_FOR_TESTING_1, .offset = 0 }, @@ -131,7 +132,7 @@ TEST(CLIUnittest, TestCliSetArray) TEST(CLIUnittest, TestCliSetStringNoFlags) { - char *str = (char *)"str_unit_test = SAMPLE"; + char *str = (char *)"str_unit_test = SAMPLE"; cliSet("", str); const uint16_t index = cliGetSettingIndex(str, 13); @@ -159,8 +160,8 @@ TEST(CLIUnittest, TestCliSetStringNoFlags) TEST(CLIUnittest, TestCliSetStringWriteOnce) { - char *str1 = (char *)"wos_unit_test = SAMPLE"; - char *str2 = (char *)"wos_unit_test = ELPMAS"; + char *str1 = (char *)"wos_unit_test = SAMPLE"; + char *str2 = (char *)"wos_unit_test = ELPMAS"; cliSet("", str1); const uint16_t index = cliGetSettingIndex(str1, 13); @@ -369,6 +370,19 @@ void generateLedConfig(ledConfig_t *, char *, size_t) {} void serialSetCtrlLineStateDtrPin(serialPort_t *, ioTag_t ) {} void serialSetCtrlLineState(serialPort_t *, uint16_t ) {} +serialPortIdentifier_e findSerialPortByName(const char* portName, int (*cmp)(const char *portName, const char *candidate)) +{ + UNUSED(portName); + UNUSED(cmp); + return SERIAL_PORT_NONE; +} + +const char* serialName(serialPortIdentifier_e identifier, const char* notFound) +{ + UNUSED(identifier); + return notFound; +} + //void serialSetBaudRateCb(serialPort_t *, void (*)(serialPort_t *context, uint32_t baud), serialPort_t *) {} void rescheduleTask(taskId_e, timeDelta_t){} void schedulerSetNextStateTime(timeDelta_t ){} diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index c7eb278496..c01de00455 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -61,6 +61,7 @@ extern "C" { #include "pg/pg.h" #include "pg/pg_ids.h" + #include "pg/pilot.h" #include "pg/rx.h" #include "rx/rx.h" diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 0812c863cc..d11230d42b 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -58,6 +58,7 @@ extern "C" { #include "pg/gps_rescue.h" #include "pg/pg.h" #include "pg/pg_ids.h" + #include "pg/pilot.h" #include "pg/rx.h" #include "sensors/acceleration.h" diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 7be5ff2c60..862c43119d 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -654,6 +654,8 @@ bool isTryingToArm(void) { return false; } void resetTryingToArm(void) {} void setLedProfile(uint8_t profile) { UNUSED(profile); } uint8_t getLedProfile(void) { return 0; } +uint8_t getLedBrightness(void) { return 50; } +void setLedBrightness(uint8_t brightness) { UNUSED(brightness); } void compassStartCalibration(void) {} void pinioBoxTaskControl(void) {} void schedulerIgnoreTaskExecTime(void) {}