diff --git a/.gitignore b/.gitignore index 63a87cbe41..7996fc5e51 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,6 @@ stm32.mak # artefacts for CLion /cmake-build-debug/ /CMakeLists.txt + +.vagrant +ubuntu*.log diff --git a/.travis.yml b/.travis.yml index 82f041a159..7731ac2a5e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -32,6 +32,7 @@ addons: - git - libc6-i386 - time + - libblocksruntime-dev # We use cpp for unit tests, and c for the main project. language: cpp diff --git a/README.md b/README.md index a5f0b30909..fc04392bf1 100644 --- a/README.md +++ b/README.md @@ -101,7 +101,7 @@ Big thanks to current and past contributors: * Blackman, Jason (blckmn) * ctzsnooze * Höglund, Anders (andershoglund) -* Ledvin, Peter (ledvinap) - **IO code awesomeness!** +* Ledvina, Petr (ledvinap) - **IO code awesomeness!** * kc10kevin * Keeble, Gary (MadmanK) * Keller, Michael (mikeller) - **Configurator brilliance** diff --git a/Vagrantfile b/Vagrantfile index ad8698227a..5188db64cc 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -16,6 +16,10 @@ Vagrant.configure(2) do |config| config.vm.provider "virtualbox" do |v| v.memory = 4096 + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-interval", 10000] + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-min-adjust", 100] + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", 1] + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-threshold", 10000] end # Enable provisioning with a shell script. Additional provisioners such as @@ -27,6 +31,7 @@ Vagrant.configure(2) do |config| apt-get update apt-get install -y git gcc-arm-embedded=6-2017q2-1~xenial1 apt-get install -y make python gcc clang + apt-get install -y libblocksruntime-dev SHELL end diff --git a/make/mcu/SITL.mk b/make/mcu/SITL.mk index 242b42450e..90210fa1a0 100644 --- a/make/mcu/SITL.mk +++ b/make/mcu/SITL.mk @@ -25,7 +25,6 @@ MCU_EXCLUDES = \ drivers/dma.c \ drivers/pwm_output.c \ drivers/timer.c \ - drivers/light_led.c \ drivers/system.c \ drivers/rcc.c \ drivers/serial_escserial.c \ diff --git a/make/source.mk b/make/source.mk index c668bbc113..bc6c3e4913 100644 --- a/make/source.mk +++ b/make/source.mk @@ -159,6 +159,7 @@ FC_SRC = \ telemetry/smartport.c \ telemetry/ltm.c \ telemetry/mavlink.c \ + telemetry/msp_shared.c \ telemetry/ibus.c \ telemetry/ibus_shared.c \ sensors/esc_sensor.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9d42a54435..162f2bca6e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -318,9 +318,6 @@ typedef struct blackboxSlowState_s { bool rxFlightChannelsValid; } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() -//From mixer.c: -extern float motorOutputHigh, motorOutputLow; - //From rc_controls.c extern boxBitmask_t rcModeActivationMask; @@ -1701,7 +1698,7 @@ void blackboxInit(void) // by default p_denom is 32 and a P-frame is written every 1ms // if p_denom is zero then no P-frames are logged if (blackboxConfig()->p_denom == 0) { - blackboxPInterval = 0; + blackboxPInterval = 0; // blackboxPInterval not used when p_denom is zero, so just set it to zero } else if (blackboxConfig()->p_denom > blackboxIInterval && blackboxIInterval >= 32) { blackboxPInterval = 1; } else { diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 7741f5be0d..ba078afe63 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -55,7 +55,7 @@ static struct { #endif // USE_SDCARD -void blackboxOpen() +void blackboxOpen(void) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); if (sharedBlackboxAndMspPort) { @@ -351,7 +351,7 @@ static void blackboxLogFileCreated(afatfsFilePtr_t file) } } -static void blackboxCreateLogFile() +static void blackboxCreateLogFile(void) { uint32_t remainder = blackboxSDCard.largestLogFileNumber + 1; @@ -372,7 +372,7 @@ static void blackboxCreateLogFile() * * Keep calling until the function returns true (open is complete). */ -static bool blackboxSDCardBeginLog() +static bool blackboxSDCardBeginLog(void) { fatDirectoryEntry_t *directoryEntry; @@ -511,7 +511,7 @@ bool isBlackboxDeviceFull(void) } } -unsigned int blackboxGetLogNumber() +unsigned int blackboxGetLogNumber(void) { #ifdef USE_SDCARD return blackboxSDCard.largestLogFileNumber; @@ -523,7 +523,7 @@ unsigned int blackboxGetLogNumber() * Call once every loop iteration in order to maintain the global blackboxHeaderBudget with the number of bytes we can * transmit this iteration. */ -void blackboxReplenishHeaderBudget() +void blackboxReplenishHeaderBudget(void) { int32_t freeSpace; diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 06d5ef0e0c..4cfc0ca810 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -53,7 +53,7 @@ bool blackboxDeviceBeginLog(void); bool blackboxDeviceEndLog(bool retainLog); bool isBlackboxDeviceFull(void); -unsigned int blackboxGetLogNumber(); +unsigned int blackboxGetLogNumber(void); -void blackboxReplenishHeaderBudget(); +void blackboxReplenishHeaderBudget(void); blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes); diff --git a/src/main/build/atomic.c b/src/main/build/atomic.c new file mode 100644 index 0000000000..540aacb2ad --- /dev/null +++ b/src/main/build/atomic.c @@ -0,0 +1,7 @@ +#include + +#include "atomic.h" + +#if defined(UNIT_TEST) +uint8_t atomic_BASEPRI; +#endif diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index 63d2e78ffe..e18d632b86 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -17,76 +17,133 @@ #pragma once -// only set_BASEPRI is implemented in device library. It does always create memory barrirer +#include + +#if !defined(UNIT_TEST) +// BASEPRI manipulation functions +// only set_BASEPRI is implemented in device library. It does always create memory barrier // missing versions are implemented here -// set BASEPRI and BASEPRI_MAX register, but do not create memory barrier +// set BASEPRI register, do not create memory barrier __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri) { __ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) ); } +// set BASEPRI_MAX register, do not create memory barrier __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri) { __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) ); } -#if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */ +// set BASEPRI_MAX register, with memory barrier +# if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */ __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX(uint32_t basePri) { __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) : "memory" ); } +# endif + #endif -// cleanup BASEPRI restore function, with global memory barrier +#if defined(UNIT_TEST) +// atomic related functions for unittest. + +extern uint8_t atomic_BASEPRI; + +static inline uint8_t __get_BASEPRI(void) +{ + return atomic_BASEPRI; +} + +// restore BASEPRI (called as cleanup function), with global memory barrier +static inline void __basepriRestoreMem(uint8_t *val) +{ + atomic_BASEPRI = *val; + asm volatile ("": : :"memory"); // compiler memory barrier +} + +// increase BASEPRI, with global memory barrier, returns true +static inline uint8_t __basepriSetMemRetVal(uint8_t prio) +{ + if(prio && (atomic_BASEPRI == 0 || atomic_BASEPRI > prio)) { + atomic_BASEPRI = prio; + } + asm volatile ("": : :"memory"); // compiler memory barrier + return 1; +} + +// restore BASEPRI (called as cleanup function), no memory barrier +static inline void __basepriRestore(uint8_t *val) +{ + atomic_BASEPRI = *val; +} + +// increase BASEPRI, no memory barrier, returns true +static inline uint8_t __basepriSetRetVal(uint8_t prio) +{ + if(prio && (atomic_BASEPRI == 0 || atomic_BASEPRI > prio)) { + atomic_BASEPRI = prio; + } + return 1; +} + +#else +// ARM BASEPRI manipulation + +// restore BASEPRI (called as cleanup function), with global memory barrier static inline void __basepriRestoreMem(uint8_t *val) { __set_BASEPRI(*val); } -// set BASEPRI_MAX function, with global memory barrier, returns true +// set BASEPRI_MAX, with global memory barrier, returns true static inline uint8_t __basepriSetMemRetVal(uint8_t prio) { __set_BASEPRI_MAX(prio); return 1; } -// cleanup BASEPRI restore function, no memory barrier +// restore BASEPRI (called as cleanup function), no memory barrier static inline void __basepriRestore(uint8_t *val) { __set_BASEPRI_nb(*val); } -// set BASEPRI_MAX function, no memory barrier, returns true +// set BASEPRI_MAX, no memory barrier, returns true static inline uint8_t __basepriSetRetVal(uint8_t prio) { __set_BASEPRI_MAX_nb(prio); return 1; } -// Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled -// Full memory barrier is placed at start and exit of block -#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \ +#endif + +// Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. +// All exit paths are handled. Implemented as for loop, does intercept break and continue +// Full memory barrier is placed at start and at exit of block +// __unused__ attribute is used to supress CLang warning +#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__ ((__cleanup__ (__basepriRestoreMem), __unused__)) = __get_BASEPRI(), \ __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 ) // Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier. -// Be careful when using this, you must use some method to prevent optimizer form breaking things +// Be careful when using this, you must use some method to prevent optimizer from breaking things // - lto is used for Cleanflight compilation, so function call is not memory barrier // - use ATOMIC_BARRIER or volatile to protect used variables // - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much // - gcc 5 and later works as intended, generating quite optimal code -#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \ +#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__ ((__cleanup__ (__basepriRestore), __unused__)) = __get_BASEPRI(), \ __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \ // ATOMIC_BARRIER // Create memory barrier // - at the beginning of containing block (value of parameter must be reread from memory) // - at exit of block (all exit paths) (parameter value if written into memory, but may be cached in register for subsequent use) -// On gcc 5 and higher, this protects only memory passed as parameter (any type should work) +// On gcc 5 and higher, this protects only memory passed as parameter (any type can be used) // this macro can be used only ONCE PER LINE, but multiple uses per block are fine #if (__GNUC__ > 6) -#warning "Please verify that ATOMIC_BARRIER works as intended" +# warning "Please verify that ATOMIC_BARRIER works as intended" // increment version number if BARRIER works // TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead // you should check that local variable scope with cleanup spans entire block @@ -98,15 +155,37 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) # define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__) #endif -// this macro uses local function for cleanup. CLang block can be substituded +#define ATOMIC_BARRIER_ENTER(dataPtr, refStr) \ + __asm__ volatile ("\t# barrier (" refStr ") enter\n" : "+m" (*(dataPtr))) + +#define ATOMIC_BARRIER_LEAVE(dataPtr, refStr) \ + __asm__ volatile ("\t# barrier (" refStr ") leave\n" : "m" (*(dataPtr))) + +#if defined(__clang__) +// CLang version, using Objective C-style block +// based on https://stackoverflow.com/questions/24959440/rewrite-gcc-cleanup-macro-with-nested-function-for-clang +typedef void (^__cleanup_block)(void); +static inline void __do_cleanup(__cleanup_block * b) { (*b)(); } + +#define ATOMIC_BARRIER(data) \ + typeof(data) *__UNIQL(__barrier) = &data; \ + ATOMIC_BARRIER_ENTER(__UNIQL(__barrier), #data); \ + __cleanup_block __attribute__((cleanup(__do_cleanup) __unused__)) __UNIQL(__cleanup) = \ + ^{ ATOMIC_BARRIER_LEAVE(__UNIQL(__barrier), #data); }; \ + do {} while(0) \ +/**/ +#else +// gcc version, uses local function for cleanup. #define ATOMIC_BARRIER(data) \ __extension__ void __UNIQL(__barrierEnd)(typeof(data) **__d) { \ - __asm__ volatile ("\t# barrier(" #data ") end\n" : : "m" (**__d)); \ + ATOMIC_BARRIER_LEAVE(*__d, #data); \ } \ - typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \ - __asm__ volatile ("\t# barrier (" #data ") start\n" : "+m" (*__UNIQL(__barrier))) + typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \ + ATOMIC_BARRIER_ENTER(__UNIQL(__barrier), #data); \ + do {} while(0) \ +/**/ +#endif - -// define these wrappers for atomic operations, use gcc buildins +// define these wrappers for atomic operations, using gcc builtins #define ATOMIC_OR(ptr, val) __sync_fetch_and_or(ptr, val) #define ATOMIC_AND(ptr, val) __sync_fetch_and_and(ptr, val) diff --git a/src/main/build/version.h b/src/main/build/version.h index b5cc9e7e72..d03f86fa39 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -36,3 +36,5 @@ extern const char* const buildDate; // "MMM DD YYYY" MMM = Jan/Feb/... #define BUILD_TIME_LENGTH 8 extern const char* const buildTime; // "HH:MM:SS" + +#define MSP_API_VERSION_STRING STR(API_VERSION_MAJOR) "." STR(API_VERSION_MINOR) \ No newline at end of file diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 36f3dbdd23..1bcf7eb88d 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -69,7 +69,7 @@ static char cmsx_BlackboxStatus[CMS_BLACKBOX_STRING_LENGTH]; static char cmsx_BlackboxDeviceStorageUsed[CMS_BLACKBOX_STRING_LENGTH]; static char cmsx_BlackboxDeviceStorageFree[CMS_BLACKBOX_STRING_LENGTH]; -static void cmsx_Blackbox_GetDeviceStatus() +static void cmsx_Blackbox_GetDeviceStatus(void) { char * unit = "B"; #if defined(USE_SDCARD) || defined(USE_FLASHFS) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 1144618b54..f364cbca12 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -391,6 +391,77 @@ static CMS_Menu cmsx_menuFilterPerProfile = { .entries = cmsx_menuFilterPerProfileEntries, }; +#ifdef USE_COPY_PROFILE_CMS_MENU + +static uint8_t cmsx_dstPidProfile; +static uint8_t cmsx_dstControlRateProfile; + +static const char * const cmsx_ProfileNames[] = { + "-", + "1", + "2", + "3" +}; + +static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames }; +static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames }; + +static long cmsx_menuCopyProfile_onEnter(void) +{ + cmsx_dstPidProfile = 0; + cmsx_dstControlRateProfile = 0; + + return 0; +} + +static long cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr) +{ + UNUSED(pDisplay); + UNUSED(ptr); + + if (cmsx_dstPidProfile > 0) { + pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex()); + } + + return 0; +} + +static long cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr) +{ + UNUSED(pDisplay); + UNUSED(ptr); + + if (cmsx_dstControlRateProfile > 0) { + copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex()); + } + + return 0; +} + +static OSD_Entry cmsx_menuCopyProfileEntries[] = +{ + { "-- COPY PROFILE --", OME_Label, NULL, NULL, 0}, + + { "CPY PID PROF TO", OME_TAB, NULL, &cmsx_PidProfileTable, 0 }, + { "COPY PP", OME_Funcall, cmsx_CopyPidProfile, NULL, 0 }, + { "CPY RATE PROF TO", OME_TAB, NULL, &cmsx_ControlRateProfileTable, 0 }, + { "COPY RP", OME_Funcall, cmsx_CopyControlRateProfile, NULL, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuCopyProfile = { + .GUARD_text = "XCPY", + .GUARD_type = OME_MENU, + .onEnter = cmsx_menuCopyProfile_onEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuCopyProfileEntries, +}; + +#endif + static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, @@ -404,6 +475,9 @@ static OSD_Entry cmsx_menuImuEntries[] = {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, +#ifdef USE_COPY_PROFILE_CMS_MENU + {"COPY PROF", OME_Submenu, cmsMenuChange, &cmsx_menuCopyProfile, 0}, +#endif {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} @@ -417,4 +491,5 @@ CMS_Menu cmsx_menuImu = { .onGlobalExit = NULL, .entries = cmsx_menuImuEntries, }; + #endif // CMS diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c index 34a8a713cc..6636e8b4ab 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ b/src/main/cms/cms_menu_vtx_smartaudio.c @@ -21,7 +21,7 @@ #include "platform.h" -#ifdef CMS +#if defined(CMS) && defined(VTX_SMARTAUDIO) #include "common/printf.h" #include "common/utils.h" diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index 5265c8dd22..ff0f9afaf2 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/src/main/cms/cms_menu_vtx_tramp.c @@ -21,7 +21,8 @@ #include "platform.h" -#ifdef CMS +#if defined(CMS) && defined(VTX_TRAMP) + #include "common/printf.h" #include "common/utils.h" @@ -155,7 +156,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) return MENU_CHAIN_BACK; } -static void trampCmsInitSettings() +static void trampCmsInitSettings(void) { if (trampBand > 0) trampCmsBand = trampBand; if (trampChannel > 0) trampCmsChan = trampChannel; @@ -173,7 +174,7 @@ static void trampCmsInitSettings() } } -static long trampCmsOnEnter() +static long trampCmsOnEnter(void) { trampCmsInitSettings(); return 0; diff --git a/src/main/common/crc.c b/src/main/common/crc.c index 1ccbd78e5e..afd77b79d1 100644 --- a/src/main/common/crc.c +++ b/src/main/common/crc.c @@ -77,3 +77,25 @@ void crc8_dvb_s2_sbuf_append(sbuf_t *dst, uint8_t *start) } sbufWriteU8(dst, crc); } + +uint8_t crc8_xor_update(uint8_t crc, const void *data, uint32_t length) +{ + const uint8_t *p = (const uint8_t *)data; + const uint8_t *pend = p + length; + + for (; p != pend; p++) { + crc ^= *p; + } + return crc; +} + +void crc8_xor_sbuf_append(sbuf_t *dst, uint8_t *start) +{ + uint8_t crc = 0; + const uint8_t *end = dst->ptr; + for (uint8_t *ptr = start; ptr < end; ++ptr) { + crc ^= *ptr; + } + sbufWriteU8(dst, crc); +} + diff --git a/src/main/common/crc.h b/src/main/common/crc.h index 8e8fbf5557..8aebc3c415 100644 --- a/src/main/common/crc.h +++ b/src/main/common/crc.h @@ -17,9 +17,12 @@ #pragma once +struct sbuf_s; + uint16_t crc16_ccitt(uint16_t crc, unsigned char a); uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length); uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a); uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length); -struct sbuf_s; void crc8_dvb_s2_sbuf_append(struct sbuf_s *dst, uint8_t *start); +uint8_t crc8_xor_update(uint8_t crc, const void *data, uint32_t length); +void crc8_xor_sbuf_append(struct sbuf_s *dst, uint8_t *start); diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 35bc54a794..c9585e857c 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -295,6 +295,7 @@ float firFilterLastInput(const firFilter_t *filter) void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { + memset(filter, 0, sizeof(firFilterDenoise_t)); filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE); } @@ -303,12 +304,14 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input) { filter->state[filter->index] = input; filter->movingSum += filter->state[filter->index++]; - if (filter->index == filter->targetCount) + if (filter->index == filter->targetCount) { filter->index = 0; + } filter->movingSum -= filter->state[filter->index]; - if (filter->targetCount >= filter->filledCount) + if (filter->targetCount >= filter->filledCount) { return filter->movingSum / filter->targetCount; - else + } else { return filter->movingSum / ++filter->filledCount + 1; + } } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 730298cdbf..5b282a91e2 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -44,7 +44,7 @@ typedef struct biquadFilter_s { float x1, x2, y1, y2; } biquadFilter_t; -typedef struct firFilterDenoise_s{ +typedef struct firFilterDenoise_s { int filledCount; int targetCount; int index; diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c index c055ad61ae..ad7db189d3 100644 --- a/src/main/common/streambuf.c +++ b/src/main/common/streambuf.c @@ -20,6 +20,13 @@ #include "streambuf.h" +sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end) +{ + sbuf->ptr = ptr; + sbuf->end = end; + return sbuf; +} + void sbufWriteU8(sbuf_t *dst, uint8_t val) { *dst->ptr++ = val; @@ -54,6 +61,12 @@ void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val) } +void sbufFill(sbuf_t *dst, uint8_t data, int len) +{ + memset(dst->ptr, data, len); + dst->ptr += len; +} + void sbufWriteData(sbuf_t *dst, const void *data, int len) { memcpy(dst->ptr, data, len); @@ -65,6 +78,11 @@ void sbufWriteString(sbuf_t *dst, const char *string) sbufWriteData(dst, string, strlen(string)); } +void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string) +{ + sbufWriteData(dst, string, strlen(string) + 1); +} + uint8_t sbufReadU8(sbuf_t *src) { return *src->ptr++; diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h index dad54adddb..21f5822fb6 100644 --- a/src/main/common/streambuf.h +++ b/src/main/common/streambuf.h @@ -20,20 +20,23 @@ #include // simple buffer-based serializer/deserializer without implicit size check -// little-endian encoding implemneted now typedef struct sbuf_s { - uint8_t *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **) + uint8_t *ptr; // data pointer must be first (sbuf_t* is equivalent to uint8_t **) uint8_t *end; } sbuf_t; +sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end); + void sbufWriteU8(sbuf_t *dst, uint8_t val); void sbufWriteU16(sbuf_t *dst, uint16_t val); void sbufWriteU32(sbuf_t *dst, uint32_t val); void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val); void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val); +void sbufFill(sbuf_t *dst, uint8_t data, int len); void sbufWriteData(sbuf_t *dst, const void *data, int len); void sbufWriteString(sbuf_t *dst, const char *string); +void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string); uint8_t sbufReadU8(sbuf_t *src); uint16_t sbufReadU16(sbuf_t *src); diff --git a/src/main/common/string_light.c b/src/main/common/string_light.c new file mode 100755 index 0000000000..1546f733c4 --- /dev/null +++ b/src/main/common/string_light.c @@ -0,0 +1,74 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 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 Cleanflight. If not, see . + */ + +#include + +#include "string_light.h" +#include "typeconversion.h" + +int sl_isalnum(int c) +{ + return sl_isdigit(c) || sl_isupper(c) || sl_islower(c); +} + +int sl_isdigit(int c) +{ + return (c >= '0' && c <= '9'); +} + +int sl_isupper(int c) +{ + return (c >= 'A' && c <= 'Z'); +} + +int sl_islower(int c) +{ + return (c >= 'a' && c <= 'z'); +} + +int sl_tolower(int c) +{ + return sl_isupper(c) ? (c) - 'A' + 'a' : c; +} + +int sl_toupper(int c) +{ + return sl_islower(c) ? (c) - 'a' + 'A' : c; +} + +int sl_strcasecmp(const char * s1, const char * s2) +{ + return sl_strncasecmp(s1, s2, INT_MAX); +} + +int sl_strncasecmp(const char * s1, const char * s2, int n) +{ + const unsigned char * ucs1 = (const unsigned char *) s1; + const unsigned char * ucs2 = (const unsigned char *) s2; + + int d = 0; + + for ( ; n != 0; n--) { + const int c1 = sl_tolower(*ucs1++); + const int c2 = sl_tolower(*ucs2++); + if (((d = c1 - c2) != 0) || (c2 == '\0')) { + break; + } + } + + return d; +} diff --git a/src/main/common/string_light.h b/src/main/common/string_light.h new file mode 100755 index 0000000000..005a6c6b12 --- /dev/null +++ b/src/main/common/string_light.h @@ -0,0 +1,28 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 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 Cleanflight. If not, see . + */ + +#pragma once + +int sl_isalnum(int c); +int sl_isdigit(int c); +int sl_isupper(int c); +int sl_islower(int c); +int sl_tolower(int c); +int sl_toupper(int c); + +int sl_strcasecmp(const char * s1, const char * s2); +int sl_strncasecmp(const char * s1, const char * s2, int n); diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index f784725e0d..702e15eeb6 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -33,11 +33,7 @@ #include "drivers/system.h" -#ifdef EEPROM_IN_RAM -extern uint8_t eepromData[EEPROM_SIZE]; -# define __config_start (*eepromData) -# define __config_end (*ARRAYEND(eepromData)) -#else +#ifndef EEPROM_IN_RAM extern uint8_t __config_start; // configured via linker script when building binaries. extern uint8_t __config_end; #endif diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 2994fb4ba4..382e7d14c2 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -20,7 +20,7 @@ #include #include -#define EEPROM_CONF_VERSION 161 +#define EEPROM_CONF_VERSION 163 bool isEEPROMContentValid(void); bool loadEEPROM(void); diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index 14c800d906..1fb05a0350 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -23,8 +23,10 @@ #include "config/config_streamer.h" +#ifndef EEPROM_IN_RAM extern uint8_t __config_start; // configured via linker script when building binaries. extern uint8_t __config_end; +#endif #if !defined(FLASH_PAGE_SIZE) // F1 diff --git a/src/main/config/feature.c b/src/main/config/feature.c index bb5e5b9a75..8ee9c4afef 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -43,7 +43,7 @@ void intFeatureClearAll(uint32_t *features) *features = 0; } -void latchActiveFeatures() +void latchActiveFeatures(void) { activeFeaturesLatch = featureConfig()->enabledFeatures; } @@ -68,7 +68,7 @@ void featureClear(uint32_t mask) intFeatureClear(mask, &featureConfigMutable()->enabledFeatures); } -void featureClearAll() +void featureClearAll(void) { intFeatureClearAll(&featureConfigMutable()->enabledFeatures); } diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c index a61b454820..a64d0eb973 100644 --- a/src/main/config/parameter_group.c +++ b/src/main/config/parameter_group.c @@ -39,7 +39,7 @@ static uint8_t *pgOffset(const pgRegistry_t* reg) return reg->address; } -static void pgResetInstance(const pgRegistry_t *reg, uint8_t *base) +void pgResetInstance(const pgRegistry_t *reg, uint8_t *base) { const uint16_t regSize = pgSize(reg); @@ -85,7 +85,7 @@ int pgStore(const pgRegistry_t* reg, void *to, int size) return take; } -void pgResetAll() +void pgResetAll(void) { PG_FOREACH(reg) { pgReset(reg); diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 16ab6c52fe..9bee7b925f 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -186,6 +186,7 @@ const pgRegistry_t* pgFind(pgn_t pgn); void pgLoad(const pgRegistry_t* reg, const void *from, int size, int version); int pgStore(const pgRegistry_t* reg, void *to, int size); -void pgResetAll(); +void pgResetAll(void); +void pgResetInstance(const pgRegistry_t *reg, uint8_t *base); bool pgResetCopy(void *copy, pgn_t pgn); void pgReset(const pgRegistry_t* reg); diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 20c7578003..0c4d3a2f2e 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -114,8 +114,9 @@ #define PG_ESCSERIAL_CONFIG 521 #define PG_CAMERA_CONTROL_CONFIG 522 #define PG_FRSKY_D_CONFIG 523 -#define PG_FLYSKY_CONFIG 524 -#define PG_BETAFLIGHT_END 524 +#define PG_MAX7456_CONFIG 524 +#define PG_FLYSKY_CONFIG 525 +#define PG_BETAFLIGHT_END 525 // OSD configuration (subject to change) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 4900638e67..f6df5875a0 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -42,8 +42,10 @@ typedef enum { GYRO_RATE_1_kHz, + GYRO_RATE_1100_Hz, GYRO_RATE_3200_Hz, GYRO_RATE_8_kHz, + GYRO_RATE_9_kHz, GYRO_RATE_32_kHz, } gyroRateKHz_e; @@ -58,6 +60,7 @@ typedef struct gyroDev_s { int32_t gyroZero[XYZ_AXIS_COUNT]; int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment int16_t gyroADCRaw[XYZ_AXIS_COUNT]; + int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT]; int16_t temperature; uint8_t lpf; gyroRateKHz_e gyroRateKHz; @@ -70,6 +73,7 @@ typedef struct gyroDev_s { mpuDetectionResult_t mpuDetectionResult; ioTag_t mpuIntExtiTag; mpuConfiguration_t mpuConfiguration; + bool gyro_high_fsr; } gyroDev_t; typedef struct accDev_s { @@ -86,6 +90,7 @@ typedef struct accDev_s { sensor_align_e accAlign; mpuDetectionResult_t mpuDetectionResult; mpuConfiguration_t mpuConfiguration; + bool acc_high_fsr; } accDev_t; static inline void accDevLock(accDev_t *acc) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index d15d4aa79a..ed7e59451f 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -39,16 +39,17 @@ #include "drivers/system.h" #include "drivers/time.h" -#include "accgyro.h" -#include "accgyro_mpu3050.h" -#include "accgyro_mpu6050.h" -#include "accgyro_mpu6500.h" -#include "accgyro_spi_bmi160.h" -#include "accgyro_spi_icm20689.h" -#include "accgyro_spi_mpu6000.h" -#include "accgyro_spi_mpu6500.h" -#include "accgyro_spi_mpu9250.h" -#include "accgyro_mpu.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_mpu3050.h" +#include "drivers/accgyro/accgyro_mpu6050.h" +#include "drivers/accgyro/accgyro_mpu6500.h" +#include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_icm20649.h" +#include "drivers/accgyro/accgyro_spi_icm20689.h" +#include "drivers/accgyro/accgyro_spi_mpu6000.h" +#include "drivers/accgyro/accgyro_spi_mpu6500.h" +#include "drivers/accgyro/accgyro_spi_mpu9250.h" +#include "drivers/accgyro/accgyro_mpu.h" mpuResetFnPtr mpuResetFn; @@ -289,6 +290,22 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) } #endif +#ifdef USE_GYRO_SPI_ICM20649 +#ifdef ICM20649_SPI_INSTANCE + spiBusSetInstance(&gyro->bus, ICM20649_SPI_INSTANCE); +#endif +#ifdef ICM20649_CS_PIN + gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20649_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; +#endif + sensor = icm20649SpiDetect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; + gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiBusWriteRegister; + return true; + } +#endif + #ifdef USE_GYRO_SPI_ICM20689 #ifdef ICM20689_SPI_INSTANCE spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE); diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 04d27c084e..ed2f7ca977 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -23,7 +23,8 @@ //#define DEBUG_MPU_DATA_READY_INTERRUPT -#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) +#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689) #define GYRO_USES_SPI #endif @@ -40,6 +41,7 @@ #define ICM20601_WHO_AM_I_CONST (0xAC) #define ICM20602_WHO_AM_I_CONST (0x12) #define ICM20608G_WHO_AM_I_CONST (0xAF) +#define ICM20649_WHO_AM_I_CONST (0xE1) #define ICM20689_WHO_AM_I_CONST (0x98) @@ -189,6 +191,7 @@ typedef enum { ICM_20601_SPI, ICM_20602_SPI, ICM_20608_SPI, + ICM_20649_SPI, ICM_20689_SPI, BMI_160_SPI, } mpuSensor_e; diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c new file mode 100644 index 0000000000..e6210c4bbe --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -0,0 +1,200 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 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 Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/axis.h" +#include "common/maths.h" + +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/gyro_sync.h" +#include "drivers/io.h" +#include "drivers/sensor.h" +#include "drivers/time.h" + +#include "accgyro.h" +#include "accgyro_mpu.h" +#include "accgyro_spi_icm20649.h" + +static void icm20649SpiInit(const busDevice_t *bus) +{ + static bool hardwareInitialised = false; + + if (hardwareInitialised) { + return; + } + + IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->busdev_u.spi.csnPin); + + // all registers can be read/written at full speed (7MHz +-10%) + // TODO verify that this works at 9MHz and 10MHz on non F7 + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + hardwareInitialised = true; +} + +uint8_t icm20649SpiDetect(const busDevice_t *bus) +{ + icm20649SpiInit(bus); + + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + spiBusWriteRegister(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + delay(15); + + spiBusWriteRegister(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + + uint8_t icmDetected = MPU_NONE; + uint8_t attemptsRemaining = 20; + do { + delay(150); + const uint8_t whoAmI = spiBusReadRegister(bus, ICM20649_RA_WHO_AM_I); + if (whoAmI == ICM20649_WHO_AM_I_CONST) { + icmDetected = ICM_20649_SPI; + } else { + icmDetected = MPU_NONE; + } + if (icmDetected != MPU_NONE) { + break; + } + if (!attemptsRemaining) { + return MPU_NONE; + } + } while (attemptsRemaining--); + + return icmDetected; + +} + +void icm20649AccInit(accDev_t *acc) +{ + // 2,048 LSB/g 16g + // 1,024 LSB/g 30g + acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048; + + spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + delay(15); + const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G; + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); + delay(15); + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 + delay(15); +} + +bool icm20649SpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != ICM_20649_SPI) { + return false; + } + + acc->initFn = icm20649AccInit; + acc->readFn = icm20649AccRead; + + return true; +} + + +void icm20649GyroInit(gyroDev_t *gyro) +{ + mpuGyroInit(gyro); + + spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); // ensure proper speed + + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + delay(100); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + delay(15); + const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS; + uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips) + raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3; + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + delay(100); + + // Data ready interrupt configuration + // back to bank 0 + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN + delay(15); + +#ifdef USE_MPU_DATA_READY_SIGNAL + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01); +#endif +} + +bool icm20649SpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != ICM_20649_SPI) + return false; + + gyro->initFn = icm20649GyroInit; + gyro->readFn = icm20649GyroReadSPI; + + // 16.4 dps/lsb 2kDps + // 8.2 dps/lsb 4kDps + gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f); + + return true; +} + +bool icm20649GyroReadSPI(gyroDev_t *gyro) +{ + static const uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + uint8_t data[7]; + + const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7); + if (!ack) { + return false; + } + + gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]); + gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]); + gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]); + + return true; +} + +bool icm20649AccRead(accDev_t *acc) +{ + uint8_t data[6]; + + const bool ack = acc->mpuConfiguration.readFn(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6); + if (!ack) { + return false; + } + + acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]); + acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]); + acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]); + + return true; +} diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.h b/src/main/drivers/accgyro/accgyro_spi_icm20649.h new file mode 100644 index 0000000000..66739a8b0a --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.h @@ -0,0 +1,64 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 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 Cleanflight. If not, see . + */ +#pragma once + +#include "drivers/bus.h" + +#define ICM20649_BIT_RESET (0x80) + +#define ICM20649_RA_REG_BANK_SEL 0x7F + +// BANK 0 +#define ICM20649_RA_WHO_AM_I 0x00 +#define ICM20649_RA_PWR_MGMT_1 0x06 +#define ICM20649_RA_PWR_MGMT_2 0x07 +#define ICM20649_RA_INT_PIN_CFG 0x0F +#define ICM20649_RA_INT_ENABLE_1 0x11 +#define ICM20649_RA_GYRO_XOUT_H 0x33 +#define ICM20649_RA_ACCEL_XOUT_H 0x2D + +// BANK 2 +#define ICM20649_RA_GYRO_SMPLRT_DIV 0x00 +#define ICM20649_RA_GYRO_CONFIG_1 0x01 +#define ICM20649_RA_ACCEL_CONFIG 0x14 + +enum icm20649_gyro_fsr_e { + ICM20649_FSR_500DPS = 0, + ICM20649_FSR_1000DPS, + ICM20649_FSR_2000DPS, + ICM20649_FSR_4000DPS, + NUM_ICM20649_GYRO_FSR +}; + +enum icm20649_accel_fsr_e { + ICM20649_FSR_4G = 0, + ICM20649_FSR_8G, + ICM20649_FSR_16G, + ICM20649_FSR_30G, + NUM_ICM20649_ACCEL_FSR +}; + +void icm20649AccInit(accDev_t *acc); +void icm20649GyroInit(gyroDev_t *gyro); + +uint8_t icm20649SpiDetect(const busDevice_t *bus); + +bool icm20649SpiAccDetect(accDev_t *acc); +bool icm20649SpiGyroDetect(gyroDev_t *gyro); + +bool icm20649GyroReadSPI(gyroDev_t *gyro); +bool icm20649AccRead(accDev_t *acc); diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 88507bc10f..a062c44ee4 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -87,6 +87,8 @@ typedef enum SPIDevice { #define SPI_DEV_TO_CFG(x) ((x) + 1) void spiPreInitCs(ioTag_t iotag); +void spiPreInitCsOutPU(ioTag_t iotag); + bool spiInit(SPIDevice device); void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data); diff --git a/src/main/drivers/bus_spi_config.c b/src/main/drivers/bus_spi_config.c index 68643797ff..897b78e254 100644 --- a/src/main/drivers/bus_spi_config.c +++ b/src/main/drivers/bus_spi_config.c @@ -25,7 +25,14 @@ // Bring a pin for possible CS line to pull-up state in preparation for // sequential initialization by relevant drivers. -// Note that the pin is set to input for safety at this point. + +// There are two versions: +// spiPreInitCs set the pin to input with pullup (IOCFG_IPU) for safety at this point. +// spiPreInitCsOutPU which actually drive the pin for digital hi. +// +// The later is required for SPI slave devices on some targets, interfaced through level shifters, such as Kakute F4. +// Note that with this handling, a pin declared as CS pin for MAX7456 needs special care when re-purposing the pin for other, especially, input uses. +// This will/should be fixed when we go fully reconfigurable. void spiPreInitCs(ioTag_t iotag) { @@ -35,3 +42,13 @@ void spiPreInitCs(ioTag_t iotag) IOConfigGPIO(io, IOCFG_IPU); } } + +void spiPreInitCsOutPU(ioTag_t iotag) +{ + IO_t io = IOGetByTag(iotag); + if (io) { + IOInit(io, OWNER_SPI_PREINIT, 0); + IOConfigGPIO(io, IOCFG_OUT_PP); + IOHi(io); + } +} diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c index a3abca17ed..50d68e818a 100644 --- a/src/main/drivers/camera_control.c +++ b/src/main/drivers/camera_control.c @@ -63,6 +63,7 @@ PG_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig, .mode = CAMERA_CONTROL_MODE_HARDWARE_PWM, .refVoltage = 330, .keyDelayMs = 180, + .internalResistance = 470, .ioTag = IO_TAG(CAMERA_CONTROL_PIN) ); @@ -76,14 +77,14 @@ static struct { static uint32_t endTimeMillis; #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE -void TIM6_DAC_IRQHandler() +void TIM6_DAC_IRQHandler(void) { IOHi(cameraControlRuntime.io); TIM6->SR = 0; } -void TIM7_IRQHandler() +void TIM7_IRQHandler(void) { IOLo(cameraControlRuntime.io); @@ -91,7 +92,7 @@ void TIM7_IRQHandler() } #endif -void cameraControlInit() +void cameraControlInit(void) { if (cameraControlConfig()->ioTag == IO_TAG_NONE) return; @@ -107,10 +108,10 @@ void cameraControlInit() return; } - #ifdef USE_HAL_DRIVER - IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction); + #ifdef STM32F1 + IOConfigGPIO(cameraControlRuntime.io, IOCFG_AF_PP); #else - IOConfigGPIO(cameraControlRuntime.io, IOCFG_AF_PP); + IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction); #endif pwmOutConfig(&cameraControlRuntime.channel, timerHardware, CAMERA_CONTROL_TIMER_HZ, CAMERA_CONTROL_PWM_RESOLUTION, 0, 0); @@ -158,13 +159,12 @@ void cameraControlProcess(uint32_t currentTimeUs) } } -static const int cameraPullUpResistance = 47000; static const int buttonResistanceValues[] = { 45000, 27000, 15000, 6810, 0 }; static float calculateKeyPressVoltage(const cameraControlKey_e key) { const int buttonResistance = buttonResistanceValues[key]; - return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (cameraPullUpResistance + buttonResistance); + return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance); } #if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE) diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control.h index 674aa9905e..031fb27aeb 100644 --- a/src/main/drivers/camera_control.h +++ b/src/main/drivers/camera_control.h @@ -38,15 +38,18 @@ typedef enum { typedef struct cameraControlConfig_s { cameraControlMode_e mode; + // measured in 10 mV steps uint16_t refVoltage; uint16_t keyDelayMs; + // measured 100 Ohm steps + uint16_t internalResistance; ioTag_t ioTag; } cameraControlConfig_t; PG_DECLARE(cameraControlConfig_t, cameraControlConfig); -void cameraControlInit(); +void cameraControlInit(void); void cameraControlProcess(uint32_t currentTimeUs); void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs); diff --git a/src/main/drivers/cc2500.c b/src/main/drivers/cc2500.c index 07aecd425d..b704244846 100644 --- a/src/main/drivers/cc2500.c +++ b/src/main/drivers/cc2500.c @@ -73,7 +73,7 @@ void cc2500SetPower(uint8_t power) cc2500WriteReg(CC2500_3E_PATABLE, patable[power]); } -uint8_t cc2500Reset() +uint8_t cc2500Reset(void) { cc2500Strobe(CC2500_SRES); delayMicroseconds(1000); // 1000us diff --git a/src/main/drivers/cc2500.h b/src/main/drivers/cc2500.h index 899ddb7247..a6cc96e1dc 100644 --- a/src/main/drivers/cc2500.h +++ b/src/main/drivers/cc2500.h @@ -150,4 +150,4 @@ uint8_t cc2500ReadReg(uint8_t reg); void cc2500Strobe(uint8_t address); uint8_t cc2500WriteReg(uint8_t address, uint8_t data); void cc2500SetPower(uint8_t power); -uint8_t cc2500Reset(); +uint8_t cc2500Reset(void); diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 99cedf6a16..40ca786940 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -152,7 +152,7 @@ static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) } #endif -static bool ak8963Init() +static bool ak8963Init(void) { uint8_t calibration[3]; uint8_t status; diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index f36596580e..52528f54e0 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/src/main/drivers/compass/compass_ak8975.c @@ -59,7 +59,7 @@ #define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value #define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value -static bool ak8975Init() +static bool ak8975Init(void) { uint8_t buffer[3]; uint8_t status; diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index f76f25d991..37de3d5780 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -207,9 +207,9 @@ void EXTI_IRQHandler(void) _EXTI_IRQ_HANDLER(EXTI0_IRQHandler); _EXTI_IRQ_HANDLER(EXTI1_IRQHandler); -#if defined(STM32F1) || defined(STM32F7) +#if defined(STM32F1) || defined(STM32F4) || defined(STM32F7) _EXTI_IRQ_HANDLER(EXTI2_IRQHandler); -#elif defined(STM32F3) || defined(STM32F4) +#elif defined(STM32F3) _EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler); #else # warning "Unknown CPU" diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index c89a1ba95e..e32754cec8 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -36,17 +36,32 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin gyro->gyroRateKHz = GYRO_RATE_32_kHz; gyroSamplePeriod = 31.5f; } else { -#ifdef USE_ACCGYRO_BMI160 - gyro->gyroRateKHz = GYRO_RATE_3200_Hz; - gyroSamplePeriod = 312.0f; -#else - gyro->gyroRateKHz = GYRO_RATE_8_kHz; - gyroSamplePeriod = 125.0f; -#endif + switch (gyro->mpuDetectionResult.sensor) { + case BMI_160_SPI: + gyro->gyroRateKHz = GYRO_RATE_3200_Hz; + gyroSamplePeriod = 312.0f; + break; + case ICM_20649_SPI: + gyro->gyroRateKHz = GYRO_RATE_9_kHz; + gyroSamplePeriod = 1000000.0f / 9000.0f; + break; + default: + gyro->gyroRateKHz = GYRO_RATE_8_kHz; + gyroSamplePeriod = 125.0f; + break; + } } } else { - gyro->gyroRateKHz = GYRO_RATE_1_kHz; - gyroSamplePeriod = 1000.0f; + switch (gyro->mpuDetectionResult.sensor) { + case ICM_20649_SPI: + gyro->gyroRateKHz = GYRO_RATE_1100_Hz; + gyroSamplePeriod = 1000000.0f / 1100.0f; + break; + default: + gyro->gyroRateKHz = GYRO_RATE_1_kHz; + gyroSamplePeriod = 1000.0f; + break; + } gyroSyncDenominator = 1; // Always full Sampling 1khz } diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index ceb5090d2a..4f70032ff2 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -86,21 +86,22 @@ ioRec_t* IO_Rec(IO_t io) GPIO_TypeDef* IO_GPIO(IO_t io) { - ioRec_t *ioRec = IO_Rec(io); + const ioRec_t *ioRec = IO_Rec(io); return ioRec->gpio; } uint16_t IO_Pin(IO_t io) { - ioRec_t *ioRec = IO_Rec(io); + const ioRec_t *ioRec = IO_Rec(io); return ioRec->pin; } // port index, GPIOA == 0 int IO_GPIOPortIdx(IO_t io) { - if (!io) + if (!io) { return -1; + } return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10); // ports are 0x400 apart } @@ -117,8 +118,9 @@ int IO_GPIO_PortSource(IO_t io) // zero based pin index int IO_GPIOPinIdx(IO_t io) { - if (!io) + if (!io) { return -1; + } return 31 - __builtin_clz(IO_Pin(io)); // CLZ is a bit faster than FFS } @@ -135,8 +137,9 @@ int IO_GPIO_PinSource(IO_t io) // mask on stm32f103, bit index on stm32f303 uint32_t IO_EXTI_Line(IO_t io) { - if (!io) + if (!io) { return 0; + } #if defined(STM32F1) return 1 << IO_GPIOPinIdx(io); #elif defined(STM32F3) @@ -154,8 +157,9 @@ uint32_t IO_EXTI_Line(IO_t io) bool IORead(IO_t io) { - if (!io) + if (!io) { return false; + } #if defined(USE_HAL_DRIVER) return (LL_GPIO_ReadInputPort(IO_GPIO(io)) & IO_Pin(io)); #else @@ -165,8 +169,9 @@ bool IORead(IO_t io) void IOWrite(IO_t io, bool hi) { - if (!io) + if (!io) { return; + } #if defined(USE_HAL_DRIVER) LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io) << (hi ? 0 : 16)); #elif defined(STM32F4) @@ -183,8 +188,9 @@ void IOWrite(IO_t io, bool hi) void IOHi(IO_t io) { - if (!io) + if (!io) { return; + } #if defined(USE_HAL_DRIVER) LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io)); #elif defined(STM32F4) @@ -196,8 +202,9 @@ void IOHi(IO_t io) void IOLo(IO_t io) { - if (!io) + if (!io) { return; + } #if defined(USE_HAL_DRIVER) LL_GPIO_ResetOutputPin(IO_GPIO(io), IO_Pin(io)); #elif defined(STM32F4) @@ -209,8 +216,9 @@ void IOLo(IO_t io) void IOToggle(IO_t io) { - if (!io) + if (!io) { return; + } uint32_t mask = IO_Pin(io); // Read pin state from ODR but write to BSRR because it only changes the pins @@ -238,8 +246,9 @@ void IOToggle(IO_t io) // claim IO pin, set owner and resources void IOInit(IO_t io, resourceOwner_e owner, uint8_t index) { - if (!io) + if (!io) { return; + } ioRec_t *ioRec = IO_Rec(io); ioRec->owner = owner; ioRec->index = index; @@ -247,17 +256,19 @@ void IOInit(IO_t io, resourceOwner_e owner, uint8_t index) void IORelease(IO_t io) { - if (!io) + if (!io) { return; + } ioRec_t *ioRec = IO_Rec(io); ioRec->owner = OWNER_FREE; } resourceOwner_e IOGetOwner(IO_t io) { - if (!io) + if (!io) { return OWNER_FREE; - ioRec_t *ioRec = IO_Rec(io); + } + const ioRec_t *ioRec = IO_Rec(io); return ioRec->owner; } @@ -265,9 +276,11 @@ resourceOwner_e IOGetOwner(IO_t io) void IOConfigGPIO(IO_t io, ioConfig_t cfg) { - if (!io) + if (!io) { return; - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; RCC_ClockCmd(rcc, ENABLE); GPIO_InitTypeDef init = { @@ -287,9 +300,11 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg) void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) { - if (!io) + if (!io) { return; - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; RCC_ClockCmd(rcc, ENABLE); LL_GPIO_InitTypeDef init = { @@ -308,9 +323,11 @@ void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) void IOConfigGPIO(IO_t io, ioConfig_t cfg) { - if (!io) + if (!io) { return; - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; RCC_ClockCmd(rcc, ENABLE); GPIO_InitTypeDef init = { @@ -325,10 +342,11 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg) void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) { - if (!io) + if (!io) { return; + } - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; RCC_ClockCmd(rcc, ENABLE); GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af); @@ -360,7 +378,8 @@ ioRec_t ioRecs[1]; // initialize all ioRec_t structures from ROM // currently only bitmask is used, this may change in future -void IOInitGlobal(void) { +void IOInitGlobal(void) +{ ioRec_t *ioRec = ioRecs; for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) { @@ -376,14 +395,16 @@ void IOInitGlobal(void) { IO_t IOGetByTag(ioTag_t tag) { - int portIdx = DEFIO_TAG_GPIOID(tag); - int pinIdx = DEFIO_TAG_PIN(tag); + const int portIdx = DEFIO_TAG_GPIOID(tag); + const int pinIdx = DEFIO_TAG_PIN(tag); - if (portIdx < 0 || portIdx >= DEFIO_PORT_USED_COUNT) + if (portIdx < 0 || portIdx >= DEFIO_PORT_USED_COUNT) { return NULL; + } // check if pin exists - if (!(ioDefUsedMask[portIdx] & (1 << pinIdx))) + if (!(ioDefUsedMask[portIdx] & (1 << pinIdx))) { return NULL; + } // count bits before this pin on single port int offset = __builtin_popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]); // and add port offset diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index 4ae9ac2b9e..b4337a41d6 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -38,17 +38,15 @@ static TIM_HandleTypeDef TimHandle; static uint16_t timerChannel = 0; static bool timerNChannel = false; -void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) -{ - if (htim->Instance == TimHandle.Instance) { - //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL); - ws2811LedDataTransferInProgress = 0; - } -} - void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) { HAL_DMA_IRQHandler(TimHandle.hdma[descriptor->userParam]); + if(timerNChannel) { + HAL_TIMEx_PWMN_Stop_DMA(&TimHandle,timerChannel); + } else { + HAL_TIM_PWM_Stop_DMA(&TimHandle,timerChannel); + } + ws2811LedDataTransferInProgress = 0; } void ws2811LedStripHardwareInit(ioTag_t ioTag) @@ -86,7 +84,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) ws2811IO = IOGetByTag(ioTag); IOInit(ws2811IO, OWNER_LED_STRIP, 0); - IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction); __DMA1_CLK_ENABLE(); diff --git a/src/main/drivers/light_ws2811strip_stdperiph.c b/src/main/drivers/light_ws2811strip_stdperiph.c index 342b3a3f46..695a4dcc07 100644 --- a/src/main/drivers/light_ws2811strip_stdperiph.c +++ b/src/main/drivers/light_ws2811strip_stdperiph.c @@ -22,6 +22,8 @@ #ifdef LED_STRIP +#include "build/debug.h" + #include "drivers/io.h" #include "drivers/nvic.h" @@ -99,14 +101,21 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) /* PWM1 Mode configuration */ TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; +#ifndef TEMPORARY_FIX_FOR_LED_ON_NCHAN_AND_HAVE_OUTPUT_INVERTED_FIX_ME_FOR_3_3 + TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; +#else + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; +#endif } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); @@ -115,7 +124,12 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) TIM_CtrlPWMOutputs(timer, ENABLE); TIM_ARRPreloadConfig(timer, ENABLE); - TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable); + } else { + TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); + } + TIM_Cmd(timer, ENABLE); dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index f2abc383fa..2e43433404 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -24,8 +24,13 @@ #ifdef USE_MAX7456 +#include "build/debug.h" + #include "common/printf.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/bus_spi.h" #include "drivers/dma.h" #include "drivers/io.h" @@ -36,6 +41,8 @@ #include "drivers/time.h" #include "drivers/vcd.h" +#include "fc/config.h" // For systemConfig() + // VM0 bits #define VIDEO_BUFFER_DISABLE 0x01 #define MAX7456_RESET 0x02 @@ -148,6 +155,10 @@ #define NVM_RAM_SIZE 54 #define WRITE_NVR 0xA0 +// Device type +#define MAX7456_DEVICE_TYPE_MAX 0 +#define MAX7456_DEVICE_TYPE_AT 1 + #define CHARS_PER_LINE 30 // XXX Should be related to VIDEO_BUFFER_CHARS_*? // On shared SPI buss we want to change clock for OSD chip and restore for other devices. @@ -164,6 +175,12 @@ #define DISABLE_MAX7456 IOHi(max7456CsPin) #endif +#ifndef MAX7456_SPI_CLK +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) +#endif + +static uint16_t max7456SpiClock = MAX7456_SPI_CLK; + uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL; // We write everything in screenBuffer and then compare @@ -193,6 +210,15 @@ static bool max7456Lock = false; static bool fontIsLoading = false; static IO_t max7456CsPin = IO_NONE; +static uint8_t max7456DeviceType; + + +PG_REGISTER_WITH_RESET_TEMPLATE(max7456Config_t, max7456Config, PG_MAX7456_CONFIG, 0); + +PG_RESET_TEMPLATE(max7456Config_t, max7456Config, + .clockConfig = MAX7456_CLOCK_CONFIG_OC, // SPI clock based on device type and overclock state +); + static uint8_t max7456Send(uint8_t add, uint8_t data) { @@ -387,6 +413,7 @@ void max7456ReInit(void) // Here we init only CS and try to init MAX for first time. +// Also detect device type (MAX v.s. AT) void max7456Init(const vcdProfile_t *pVcdProfile) { @@ -399,7 +426,44 @@ void max7456Init(const vcdProfile_t *pVcdProfile) IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); IOHi(max7456CsPin); - spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD); + // Detect device type by writing and reading CA[8] bit at CMAL[6]. + // Do this at half the speed for safety. + spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK * 2); + + max7456Send(MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit + + if (max7456Send(MAX7456ADD_CMAL|MAX7456ADD_READ, 0xff) & (1 << 6)) { + max7456DeviceType = MAX7456_DEVICE_TYPE_AT; + } else { + max7456DeviceType = MAX7456_DEVICE_TYPE_MAX; + } + +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + // Determine SPI clock divisor based on config and the device type. + + switch (max7456Config()->clockConfig) { + case MAX7456_CLOCK_CONFIG_HALF: + max7456SpiClock = MAX7456_SPI_CLK * 2; + break; + + case MAX7456_CLOCK_CONFIG_OC: + max7456SpiClock = (systemConfig()->cpu_overclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? MAX7456_SPI_CLK * 2 : MAX7456_SPI_CLK; + break; + + case MAX7456_CLOCK_CONFIG_FULL: + max7456SpiClock = MAX7456_SPI_CLK; + break; + } + +#ifdef DEBUG_MAX7456_SPI_CLOCK + debug[0] = systemConfig()->cpu_overclock; + debug[1] = max7456DeviceType; + debug[2] = max7456SpiClock; +#endif +#endif + + spiSetDivisor(MAX7456_SPI_INSTANCE, max7456SpiClock); + // force soft reset on Max7456 ENABLE_MAX7456; max7456Send(MAX7456ADD_VM0, MAX7456_RESET); @@ -484,8 +548,6 @@ bool max7456DmaInProgress(void) #endif } -#include "build/debug.h" - void max7456DrawScreen(void) { uint8_t stallCheck; diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 8e74d46fab..63cf0e15ff 100755 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -48,3 +48,13 @@ void max7456ClearScreen(void); void max7456RefreshAll(void); uint8_t* max7456GetScreenBuffer(void); bool max7456DmaInProgress(void); + +typedef struct max7456Config_s { + uint8_t clockConfig; // 0 = force half clock, 1 = half if OC, 2 = force full +} max7456Config_t; + +#define MAX7456_CLOCK_CONFIG_HALF 0 +#define MAX7456_CLOCK_CONFIG_OC 1 +#define MAX7456_CLOCK_CONFIG_FULL 2 + +PG_DECLARE(max7456Config_t, max7456Config); diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 6e3e8c9065..9767a13726 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -39,11 +39,11 @@ typedef enum { DSHOT_CMD_MOTOR_STOP = 0, - DSHOT_CMD_BEEP1, - DSHOT_CMD_BEEP2, - DSHOT_CMD_BEEP3, - DSHOT_CMD_BEEP4, - DSHOT_CMD_BEEP5, + DSHOT_CMD_BEACON1, + DSHOT_CMD_BEACON2, + DSHOT_CMD_BEACON3, + DSHOT_CMD_BEACON4, + DSHOT_CMD_BEACON5, DSHOT_CMD_ESC_INFO, // V2 includes settings DSHOT_CMD_SPIN_DIRECTION_1, DSHOT_CMD_SPIN_DIRECTION_2, @@ -53,6 +53,14 @@ typedef enum { DSHOT_CMD_SAVE_SETTINGS, DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, + DSHOT_CMD_LED0_ON, // BLHeli32 only + DSHOT_CMD_LED1_ON, // BLHeli32 only + DSHOT_CMD_LED2_ON, // BLHeli32 only + DSHOT_CMD_LED3_ON, // BLHeli32 only + DSHOT_CMD_LED0_OFF, // BLHeli32 only + DSHOT_CMD_LED1_OFF, // BLHeli32 only + DSHOT_CMD_LED2_OFF, // BLHeli32 only + DSHOT_CMD_LED3_OFF, // BLHeli32 only DSHOT_CMD_MAX = 47 } dshotCommands_e; diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index c4a65cfed4..bd3d6a9bb8 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -83,6 +83,11 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) { motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam]; HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]); + if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + HAL_TIMEx_PWMN_Stop_DMA(&motor->TimHandle,motor->timerHardware->channel); + } else { + HAL_TIM_PWM_Stop_DMA(&motor->TimHandle,motor->timerHardware->channel); + } } void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) @@ -96,7 +101,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m const uint8_t timerIndex = getTimerIndex(timer); IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); - IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction); + IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction); __DMA1_CLK_ENABLE(); diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index 044b8a24b2..f3c05084d7 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -385,12 +385,9 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) IOInit(io, OWNER_PWMINPUT, RESOURCE_INDEX(channel)); #ifdef STM32F1 IOConfigGPIO(io, IOCFG_IPD); -#elif defined(STM32F7) - IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); #else - IOConfigGPIO(io, IOCFG_AF_PP); + IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); #endif - timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ); timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback); timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback); @@ -442,10 +439,8 @@ void ppmRxInit(const ppmConfig_t *ppmConfig) IOInit(io, OWNER_PPMINPUT, 0); #ifdef STM32F1 IOConfigGPIO(io, IOCFG_IPD); -#elif defined(STM32F7) - IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); #else - IOConfigGPIO(io, IOCFG_AF_PP); + IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); #endif timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ); diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 3af9ee13a4..e2e8dd5f1c 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -632,7 +632,7 @@ static bool sdcard_setBlockLength(uint32_t blockLen) /* * Returns true if the card is ready to accept read/write commands. */ -static bool sdcard_isReady() +static bool sdcard_isReady(void) { return sdcard.state == SDCARD_STATE_READY || sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; } @@ -647,7 +647,7 @@ static bool sdcard_isReady() * the SDCARD_READY state. * */ -static sdcardOperationStatus_e sdcard_endWriteBlocks() +static sdcardOperationStatus_e sdcard_endWriteBlocks(void) { sdcard.multiWriteBlocksRemain = 0; diff --git a/src/main/drivers/sdcard.h b/src/main/drivers/sdcard.h index cf101dd156..1dca004440 100644 --- a/src/main/drivers/sdcard.h +++ b/src/main/drivers/sdcard.h @@ -66,11 +66,11 @@ sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, void sdcardInsertionDetectDeinit(void); void sdcardInsertionDetectInit(void); -bool sdcard_isInserted(); -bool sdcard_isInitialized(); -bool sdcard_isFunctional(); +bool sdcard_isInserted(void); +bool sdcard_isInitialized(void); +bool sdcard_isFunctional(void); -bool sdcard_poll(); -const sdcardMetadata_t* sdcard_getMetadata(); +bool sdcard_poll(void); +const sdcardMetadata_t* sdcard_getMetadata(void); void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback); diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 07bb0b81f7..25feddb389 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -660,6 +660,10 @@ static serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceive if (mode != PROTOCOL_KISSALL) { escSerial->rxTimerHardware = &(timerHardware[output]); + // N-Channels can't be used as RX. + if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + return NULL; + } #ifdef USE_HAL_DRIVER escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim); #endif @@ -944,7 +948,7 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin else { uint8_t first_output = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) { + if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { first_output = i; break; } diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index aaf0b2a212..d1396e89dd 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -131,18 +131,14 @@ static void serialInputPortActivate(softSerial_t *softSerial) if (softSerial->port.options & SERIAL_INVERTED) { #ifdef STM32F1 IOConfigGPIO(softSerial->rxIO, IOCFG_IPD); -#elif defined(STM32F7) - IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction); #else - IOConfigGPIO(softSerial->rxIO, IOCFG_AF_PP_PD); + IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction); #endif } else { #ifdef STM32F1 IOConfigGPIO(softSerial->rxIO, IOCFG_IPU); -#elif defined(STM32F7) - IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction); #else - IOConfigGPIO(softSerial->rxIO, IOCFG_AF_PP_UP); + IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction); #endif } @@ -165,35 +161,35 @@ static void serialInputPortDeActivate(softSerial_t *softSerial) TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Disable); #endif -#ifdef STM32F7 - IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->timerHardware->alternateFunction); -#else +#ifdef STM32F1 IOConfigGPIO(softSerial->rxIO, IOCFG_IN_FLOATING); +#else + IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->timerHardware->alternateFunction); #endif softSerial->rxActive = false; } static void serialOutputPortActivate(softSerial_t *softSerial) { -#ifdef STM32F7 +#ifdef STM32F1 + IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP); +#else if (softSerial->exTimerHardware) IOConfigGPIOAF(softSerial->txIO, IOCFG_OUT_PP, softSerial->exTimerHardware->alternateFunction); else IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP); -#else - IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP); #endif } static void serialOutputPortDeActivate(softSerial_t *softSerial) { -#ifdef STM32F7 +#ifdef STM32F1 + IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING); +#else if (softSerial->exTimerHardware) IOConfigGPIOAF(softSerial->txIO, IOCFG_IN_FLOATING, softSerial->exTimerHardware->alternateFunction); else IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING); -#else - IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING); #endif } @@ -254,9 +250,10 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb if (options & SERIAL_BIDIR) { // If RX and TX pins are both assigned, we CAN use either with a timer. // However, for consistency with hardware UARTs, we only use TX pin, - // and this pin must have a timer. - if (!timerTx) + // and this pin must have a timer, and it should not be N-Channel. + if (!timerTx || (timerTx->output & TIMER_OUTPUT_N_CHANNEL)) { return NULL; + } softSerial->timerHardware = timerTx; softSerial->txIO = txIO; @@ -264,9 +261,10 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(portIndex + RESOURCE_SOFT_OFFSET)); } else { if (mode & MODE_RX) { - // Need a pin & a timer on RX - if (!(tagRx && timerRx)) + // Need a pin & a timer on RX. Channel should not be N-Channel. + if (!timerRx || (timerRx->output & TIMER_OUTPUT_N_CHANNEL)) { return NULL; + } softSerial->rxIO = rxIO; softSerial->timerHardware = timerRx; diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index 39e7ad350e..0b485da4e2 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -176,7 +176,8 @@ void uartReconfigure(uartPort_t *uartPort) __HAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0); } else { - __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE); + /* Enable the UART Transmit Data Register Empty Interrupt */ + SET_BIT(uartPort->USARTx->CR1, USART_CR1_TXEIE); } } return; diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c index 8ab4aa7183..78c9ec8809 100644 --- a/src/main/drivers/serial_uart_stm32f7xx.c +++ b/src/main/drivers/serial_uart_stm32f7xx.c @@ -47,7 +47,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART1_RX_DMA .rxDMAStream = DMA2_Stream5, #endif +#ifdef USE_UART1_TX_DMA .txDMAStream = DMA2_Stream7, +#endif .rxPins = { DEFIO_TAG_E(PA10), DEFIO_TAG_E(PB7), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PB6), IO_TAG_NONE }, .af = GPIO_AF7_USART1, @@ -70,7 +72,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART2_RX_DMA .rxDMAStream = DMA1_Stream5, #endif +#ifdef USE_UART2_TX_DMA .txDMAStream = DMA1_Stream6, +#endif .rxPins = { DEFIO_TAG_E(PA3), DEFIO_TAG_E(PD6), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PA2), DEFIO_TAG_E(PD5), IO_TAG_NONE }, .af = GPIO_AF7_USART2, @@ -93,7 +97,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART3_RX_DMA .rxDMAStream = DMA1_Stream1, #endif +#ifdef USE_UART3_TX_DMA .txDMAStream = DMA1_Stream3, +#endif .rxPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PC11), DEFIO_TAG_E(PD9) }, .txPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PC10), DEFIO_TAG_E(PD8) }, .af = GPIO_AF7_USART3, @@ -116,7 +122,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART4_RX_DMA .rxDMAStream = DMA1_Stream2, #endif +#ifdef USE_UART4_TX_DMA .txDMAStream = DMA1_Stream4, +#endif .rxPins = { DEFIO_TAG_E(PA1), DEFIO_TAG_E(PC11), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PA0), DEFIO_TAG_E(PC10), IO_TAG_NONE }, .af = GPIO_AF8_UART4, @@ -139,7 +147,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART5_RX_DMA .rxDMAStream = DMA1_Stream0, #endif +#ifdef USE_UART5_TX_DMA .txDMAStream = DMA1_Stream7, +#endif .rxPins = { DEFIO_TAG_E(PD2), IO_TAG_NONE, IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PC12), IO_TAG_NONE, IO_TAG_NONE }, .af = GPIO_AF8_UART5, @@ -162,7 +172,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART6_RX_DMA .rxDMAStream = DMA2_Stream1, #endif +#ifdef USE_UART6_TX_DMA .txDMAStream = DMA2_Stream6, +#endif .rxPins = { DEFIO_TAG_E(PC7), DEFIO_TAG_E(PG9), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PC6), DEFIO_TAG_E(PG14), IO_TAG_NONE }, .af = GPIO_AF8_USART6, @@ -185,7 +197,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART7_RX_DMA .rxDMAStream = DMA1_Stream3, #endif +#ifdef USE_UART7_TX_DMA .txDMAStream = DMA1_Stream1, +#endif .rxPins = { DEFIO_TAG_E(PE7), DEFIO_TAG_E(PF6), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PE8), DEFIO_TAG_E(PF7), IO_TAG_NONE }, .af = GPIO_AF8_UART7, @@ -208,7 +222,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART8_RX_DMA .rxDMAStream = DMA1_Stream6, #endif +#ifdef USE_UART8_TX_DMA .txDMAStream = DMA1_Stream0, +#endif .rxPins = { DEFIO_TAG_E(PE0), IO_TAG_NONE, IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PE1), IO_TAG_NONE, IO_TAG_NONE }, .af = GPIO_AF8_UART8, @@ -228,9 +244,8 @@ void uartIrqHandler(uartPort_t *s) { UART_HandleTypeDef *huart = &s->Handle; /* UART in mode Receiver ---------------------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) - { - uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff); + if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) { + uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t) 0xff); if (s->port.rxCallback) { s->port.rxCallback(rbyte); @@ -247,42 +262,51 @@ void uartIrqHandler(uartPort_t *s) } /* UART parity error interrupt occurred -------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) - { - __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF); + if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) { + __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF); } /* UART frame error interrupt occurred --------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) - { - __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF); + if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) { + __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF); } /* UART noise error interrupt occurred --------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) - { - __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF); + if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) { + __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF); } /* UART Over-Run interrupt occurred -----------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) - { - __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF); + if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) { + __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF); } /* UART in mode Transmitter ------------------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) - { - HAL_UART_IRQHandler(huart); + if (!s->txDMAStream && (__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) { + /* Check that a Tx process is ongoing */ + if (huart->gState != HAL_UART_STATE_BUSY_TX) { + if (s->port.txBufferTail == s->port.txBufferHead) { + huart->TxXferCount = 0; + /* Disable the UART Transmit Data Register Empty Interrupt */ + CLEAR_BIT(huart->Instance->CR1, USART_CR1_TXEIE); + } else { + if ((huart->Init.WordLength == UART_WORDLENGTH_9B) && (huart->Init.Parity == UART_PARITY_NONE)) { + huart->Instance->TDR = (((uint16_t) s->port.txBuffer[s->port.txBufferTail]) & (uint16_t) 0x01FFU); + } else { + huart->Instance->TDR = (uint8_t)(s->port.txBuffer[s->port.txBufferTail]); + } + s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize; + } + } } /* UART in mode Transmitter (transmission end) -----------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) - { + if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) { HAL_UART_IRQHandler(huart); - handleUsartTxDma(s); + if (s->txDMAStream) { + handleUsartTxDma(s); + } } - } static void handleUsartTxDma(uartPort_t *s) @@ -329,8 +353,15 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, s->rxDMAChannel = hardware->DMAChannel; s->rxDMAStream = hardware->rxDMAStream; } - s->txDMAChannel = hardware->DMAChannel; - s->txDMAStream = hardware->txDMAStream; + + if (hardware->txDMAStream) { + s->txDMAChannel = hardware->DMAChannel; + s->txDMAStream = hardware->txDMAStream; + + // DMA TX Interrupt + dmaInit(hardware->txIrq, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); + dmaSetHandler(hardware->txIrq, dmaIRQHandler, hardware->txPriority, (uint32_t)uartdev); + } s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; @@ -362,16 +393,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, } } - // DMA TX Interrupt - dmaInit(hardware->txIrq, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); - dmaSetHandler(hardware->txIrq, dmaIRQHandler, hardware->txPriority, (uint32_t)uartdev); - - - //HAL_NVIC_SetPriority(hardware->txIrq, NVIC_PRIORITY_BASE(hardware->txPriority), NVIC_PRIORITY_SUB(hardware->txPriority)); - //HAL_NVIC_EnableIRQ(hardware->txIrq); - - if (!s->rxDMAChannel) - { + if (!s->rxDMAChannel) { HAL_NVIC_SetPriority(hardware->rxIrq, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority)); HAL_NVIC_EnableIRQ(hardware->rxIrq); } diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index dd70c8d028..fe9445dbdf 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -154,8 +154,9 @@ static void usbVcpBeginWrite(serialPort_t *instance) port->buffering = true; } -uint32_t usbTxBytesFree() +static uint32_t usbTxBytesFree(const serialPort_t *instance) { + UNUSED(instance); return CDC_Send_FreeBytes(); } diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index a53128bbdd..70e4b9c202 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -709,6 +709,10 @@ void timerInit(void) #if defined(STM32F3) || defined(STM32F4) for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; + if (timerHardwarePtr->usageFlags == TIM_USE_NONE) { + continue; + } + // XXX IOConfigGPIOAF in timerInit should eventually go away. IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 95653b4517..ddab0ad285 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -807,6 +807,10 @@ void timerInit(void) #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; + if (timerHardwarePtr->usageFlags == TIM_USE_NONE) { + continue; + } + // XXX IOConfigGPIOAF in timerInit should eventually go away. IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index fff7fcab6b..b58121487d 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -42,6 +42,11 @@ void vtxCommonRegisterDevice(vtxDevice_t *pDevice) vtxDevice = pDevice; } +bool vtxCommonDeviceRegistered(void) +{ + return vtxDevice; +} + void vtxCommonProcess(uint32_t currentTimeUs) { if (!vtxDevice) diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 000859ccab..fc74f130d8 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -78,6 +78,7 @@ typedef struct vtxVTable_s { void vtxCommonInit(void); void vtxCommonRegisterDevice(vtxDevice_t *pDevice); +bool vtxCommonDeviceRegistered(void); // VTable functions void vtxCommonProcess(uint32_t currentTimeUs); diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.c b/src/main/drivers/vtx_rtc6705_soft_spi.c index 2bffc9f6cc..bc66ae2001 100644 --- a/src/main/drivers/vtx_rtc6705_soft_spi.c +++ b/src/main/drivers/vtx_rtc6705_soft_spi.c @@ -65,11 +65,12 @@ void rtc6705IOInit(void) rtc6705LePin = IOGetByTag(IO_TAG(RTC6705_SPILE_PIN)); rtc6705ClkPin = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN)); - IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET); - IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP); - IOInit(rtc6705LePin, OWNER_SPI_CS, RESOURCE_SOFT_OFFSET); IOConfigGPIO(rtc6705LePin, IOCFG_OUT_PP); + RTC6705_SPILE_ON; + + IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET); + IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP); IOInit(rtc6705ClkPin, OWNER_SPI_SCK, RESOURCE_SOFT_OFFSET); IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5abefb6d16..853f6abcf1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -114,6 +114,8 @@ extern uint8_t __config_end; #include "io/vtx_rtc6705.h" #include "io/vtx_control.h" +#include "msp/msp_protocol.h" + #include "rx/rx.h" #include "rx/spektrum.h" #include "rx/frsky_d.h" @@ -205,7 +207,7 @@ static void cliPrint(const char *str) bufWriterFlush(cliWriter); } -static void cliPrintLinefeed() +static void cliPrintLinefeed(void) { cliPrint("\r\n"); } @@ -402,12 +404,18 @@ static uint16_t getValueOffset(const clivalue_t *value) return 0; } -STATIC_UNIT_TESTED void *getValuePointer(const clivalue_t *value) +void *cliGetValuePointer(const clivalue_t *value) { const pgRegistry_t* rec = pgFind(value->pgn); return CONST_CAST(void *, rec->address + getValueOffset(value)); } +const void *cliGetDefaultPointer(const clivalue_t *value) +{ + const pgRegistry_t* rec = pgFind(value->pgn); + return rec->address + getValueOffset(value); +} + static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) { const pgRegistry_t *pg = pgFind(value->pgn); @@ -448,7 +456,7 @@ static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) static void cliPrintVar(const clivalue_t *var, bool full) { - const void *ptr = getValuePointer(var); + const void *ptr = cliGetValuePointer(var); printValuePointer(var, ptr, full); } @@ -481,7 +489,7 @@ static void cliPrintVarRange(const clivalue_t *var) static void cliSetVar(const clivalue_t *var, const int16_t value) { - void *ptr = getValuePointer(var); + void *ptr = cliGetValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: @@ -888,18 +896,18 @@ static void cliSerialPassthrough(char *cmdline) while (tok != NULL) { switch (index) { - case 0: - id = atoi(tok); - break; - case 1: - baud = atoi(tok); - break; - case 2: - if (strstr(tok, "rx") || strstr(tok, "RX")) - mode |= MODE_RX; - if (strstr(tok, "tx") || strstr(tok, "TX")) - mode |= MODE_TX; - break; + case 0: + id = atoi(tok); + break; + case 1: + baud = atoi(tok); + break; + case 2: + if (strstr(tok, "rx") || strstr(tok, "RX")) + mode |= MODE_RX; + if (strstr(tok, "tx") || strstr(tok, "TX")) + mode |= MODE_TX; + break; } index++; tok = strtok_r(NULL, " ", &saveptr); @@ -1696,28 +1704,28 @@ static void cliSdInfo(char *cmdline) cliPrint("'\r\n" "Filesystem: "); switch (afatfs_getFilesystemState()) { - case AFATFS_FILESYSTEM_STATE_READY: - cliPrint("Ready"); + case AFATFS_FILESYSTEM_STATE_READY: + cliPrint("Ready"); break; - case AFATFS_FILESYSTEM_STATE_INITIALIZATION: - cliPrint("Initializing"); + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: + cliPrint("Initializing"); break; - case AFATFS_FILESYSTEM_STATE_UNKNOWN: - case AFATFS_FILESYSTEM_STATE_FATAL: - cliPrint("Fatal"); + case AFATFS_FILESYSTEM_STATE_UNKNOWN: + case AFATFS_FILESYSTEM_STATE_FATAL: + cliPrint("Fatal"); - switch (afatfs_getLastError()) { - case AFATFS_ERROR_BAD_MBR: - cliPrint(" - no FAT MBR partitions"); - break; - case AFATFS_ERROR_BAD_FILESYSTEM_HEADER: - cliPrint(" - bad FAT header"); - break; - case AFATFS_ERROR_GENERIC: - case AFATFS_ERROR_NONE: - ; // Nothing more detailed to print - break; - } + switch (afatfs_getLastError()) { + case AFATFS_ERROR_BAD_MBR: + cliPrint(" - no FAT MBR partitions"); + break; + case AFATFS_ERROR_BAD_FILESYSTEM_HEADER: + cliPrint(" - bad FAT header"); + break; + case AFATFS_ERROR_GENERIC: + case AFATFS_ERROR_NONE: + ; // Nothing more detailed to print + break; + } break; } cliPrintLinefeed(); @@ -2253,64 +2261,98 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) { #ifdef USE_DSHOT -#define ESC_INFO_V1_EXPECTED_FRAME_SIZE 15 -#define ESC_INFO_V2_EXPECTED_FRAME_SIZE 21 +#define ESC_INFO_KISS_V1_EXPECTED_FRAME_SIZE 15 +#define ESC_INFO_KISS_V2_EXPECTED_FRAME_SIZE 21 +#define ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE 64 + +enum { + ESC_INFO_KISS_V1, + ESC_INFO_KISS_V2, + ESC_INFO_BLHELI32 +}; #define ESC_INFO_VERSION_POSITION 12 -void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead) +void printEscInfo(const uint8_t *escInfoBuffer, uint8_t bytesRead) { bool escInfoReceived = false; if (bytesRead > ESC_INFO_VERSION_POSITION) { - uint8_t escInfoVersion = 0; - uint8_t frameLength = 0; - if (escInfoBytes[ESC_INFO_VERSION_POSITION] == 255) { - escInfoVersion = 2; - frameLength = ESC_INFO_V2_EXPECTED_FRAME_SIZE; + uint8_t escInfoVersion; + uint8_t frameLength; + if (escInfoBuffer[ESC_INFO_VERSION_POSITION] == 254) { + escInfoVersion = ESC_INFO_BLHELI32; + frameLength = ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE; + } else if (escInfoBuffer[ESC_INFO_VERSION_POSITION] == 255) { + escInfoVersion = ESC_INFO_KISS_V2; + frameLength = ESC_INFO_KISS_V2_EXPECTED_FRAME_SIZE; } else { - escInfoVersion = 1; - frameLength = ESC_INFO_V1_EXPECTED_FRAME_SIZE; + escInfoVersion = ESC_INFO_KISS_V1; + frameLength = ESC_INFO_KISS_V1_EXPECTED_FRAME_SIZE; } - if (((escInfoVersion == 1) && (bytesRead == ESC_INFO_V1_EXPECTED_FRAME_SIZE)) - || ((escInfoVersion == 2) && (bytesRead == ESC_INFO_V2_EXPECTED_FRAME_SIZE))) { + if (bytesRead == frameLength) { escInfoReceived = true; - if (calculateCrc8(escInfoBytes, frameLength - 1) == escInfoBytes[frameLength - 1]) { + if (calculateCrc8(escInfoBuffer, frameLength - 1) == escInfoBuffer[frameLength - 1]) { uint8_t firmwareVersion = 0; - char firmwareSubVersion = 0; + uint8_t firmwareSubVersion = 0; uint8_t escType = 0; switch (escInfoVersion) { - case 1: - firmwareVersion = escInfoBytes[12]; - firmwareSubVersion = (char)((escInfoBytes[13] & 0x1f) + 97); - escType = (escInfoBytes[13] & 0xe0) >> 5; + case ESC_INFO_KISS_V1: + firmwareVersion = escInfoBuffer[12]; + firmwareSubVersion = (escInfoBuffer[13] & 0x1f) + 97; + escType = (escInfoBuffer[13] & 0xe0) >> 5; break; - case 2: - firmwareVersion = escInfoBytes[13]; - firmwareSubVersion = (char)escInfoBytes[14]; - escType = escInfoBytes[15]; + case ESC_INFO_KISS_V2: + firmwareVersion = escInfoBuffer[13]; + firmwareSubVersion = escInfoBuffer[14]; + escType = escInfoBuffer[15]; + + break; + case ESC_INFO_BLHELI32: + firmwareVersion = escInfoBuffer[13]; + firmwareSubVersion = escInfoBuffer[14]; + escType = escInfoBuffer[15]; break; } cliPrint("ESC: "); - switch (escType) { - case 1: - cliPrintLine("KISS8A"); + switch (escInfoVersion) { + case ESC_INFO_KISS_V1: + case ESC_INFO_KISS_V2: + switch (escType) { + case 1: + cliPrintLine("KISS8A"); + + break; + case 2: + cliPrintLine("KISS16A"); + + break; + case 3: + cliPrintLine("KISS24A"); + + break; + case 5: + cliPrintLine("KISS Ultralite"); + + break; + default: + cliPrintLine("unknown"); + + break; + } + break; - case 2: - cliPrintLine("KISS16A"); - break; - case 3: - cliPrintLine("KISS24A"); - break; - case 5: - cliPrintLine("KISS Ultralite"); - break; - default: - cliPrintLine("unknown"); + case ESC_INFO_BLHELI32: + { + char *escType = (char *)(escInfoBuffer + 31); + escType[32] = 0; + cliPrintLine(escType); + } + break; } @@ -2319,14 +2361,64 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead) if (i && (i % 3 == 0)) { cliPrint("-"); } - cliPrintf("%02x", escInfoBytes[i]); + cliPrintf("%02x", escInfoBuffer[i]); } cliPrintLinefeed(); - cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, firmwareSubVersion); - if (escInfoVersion == 2) { - cliPrintLinef("Rotation: %s", escInfoBytes[16] ? "reversed" : "normal"); - cliPrintLinef("3D: %s", escInfoBytes[17] ? "on" : "off"); + switch (escInfoVersion) { + case ESC_INFO_KISS_V1: + case ESC_INFO_KISS_V2: + cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, (char)firmwareSubVersion); + + break; + case ESC_INFO_BLHELI32: + cliPrintLinef("Firmware: %d.%02d%", firmwareVersion, firmwareSubVersion); + + break; + } + if (escInfoVersion == ESC_INFO_KISS_V2 || escInfoVersion == ESC_INFO_BLHELI32) { + cliPrintLinef("Rotation: %s", escInfoBuffer[16] ? "reversed" : "normal"); + cliPrintLinef("3D: %s", escInfoBuffer[17] ? "on" : "off"); + if (escInfoVersion == ESC_INFO_BLHELI32) { + uint8_t setting = escInfoBuffer[18]; + cliPrint("Low voltage limit: "); + switch (setting) { + case 0: + cliPrintLine("off"); + + break; + case 255: + cliPrintLine("unsupported"); + + break; + default: + cliPrintLinef("%d.%01d", setting / 10, setting % 10); + + break; + } + + setting = escInfoBuffer[19]; + cliPrint("Current limit: "); + switch (setting) { + case 0: + cliPrintLine("off"); + + break; + case 255: + cliPrintLine("unsupported"); + + break; + default: + cliPrintLinef("%d", setting); + + break; + } + + for (int i = 0; i < 4; i++) { + setting = escInfoBuffer[i + 20]; + cliPrintLinef("LED %d: %s", i, setting ? (setting == 255) ? "unsupported" : "on" : "off"); + } + } } } else { cliPrintLine("Checksum error."); @@ -2341,14 +2433,15 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead) static void executeEscInfoCommand(uint8_t escIndex) { - uint8_t escInfoBuffer[ESC_INFO_V2_EXPECTED_FRAME_SIZE]; cliPrintLinef("Info for ESC %d:", escIndex); - startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE); + uint8_t escInfoBuffer[ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE]; + + startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE); pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO); - delay(5); + delay(10); printEscInfo(escInfoBuffer, getNumberEscBytesRead()); } @@ -2365,22 +2458,30 @@ static void cliDshotProg(char *cmdline) char *pch = strtok_r(cmdline, " ", &saveptr); int pos = 0; int escIndex = 0; + bool firstCommand = true; while (pch != NULL) { switch (pos) { - case 0: - escIndex = parseOutputIndex(pch, true); - if (escIndex == -1) { - return; - } - - break; - default: - pwmDisableMotors(); + case 0: + escIndex = parseOutputIndex(pch, true); + if (escIndex == -1) { + return; + } + break; + default: + { int command = atoi(pch); if (command >= 0 && command < DSHOT_MIN_THROTTLE) { - if (command == DSHOT_CMD_ESC_INFO) { - delay(5); // Wait for potential ESC telemetry transmission to finish + if (firstCommand) { + pwmDisableMotors(); + + if (command == DSHOT_CMD_ESC_INFO) { + delay(5); // Wait for potential ESC telemetry transmission to finish + } else { + delay(1); + } + + firstCommand = false; } if (command != DSHOT_CMD_ESC_INFO) { @@ -2403,8 +2504,9 @@ static void cliDshotProg(char *cmdline) } else { cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1); } + } - break; + break; } pos++; @@ -2431,34 +2533,34 @@ static void cliEscPassthrough(char *cmdline) int escIndex = 0; while (pch != NULL) { switch (pos) { - case 0: - if (strncasecmp(pch, "sk", strlen(pch)) == 0) { - mode = PROTOCOL_SIMONK; - } else if (strncasecmp(pch, "bl", strlen(pch)) == 0) { - mode = PROTOCOL_BLHELI; - } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) { - mode = PROTOCOL_KISS; - } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) { - mode = PROTOCOL_KISSALL; - } else { - cliShowParseError(); - - return; - } - break; - case 1: - escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS); - if (escIndex == -1) { - return; - } - - break; - default: + case 0: + if (strncasecmp(pch, "sk", strlen(pch)) == 0) { + mode = PROTOCOL_SIMONK; + } else if (strncasecmp(pch, "bl", strlen(pch)) == 0) { + mode = PROTOCOL_BLHELI; + } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) { + mode = PROTOCOL_KISS; + } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) { + mode = PROTOCOL_KISSALL; + } else { cliShowParseError(); return; + } + break; + case 1: + escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS); + if (escIndex == -1) { + return; + } - break; + break; + default: + cliShowParseError(); + + return; + + break; } pos++; @@ -2660,11 +2762,21 @@ static void cliSave(char *cmdline) static void cliDefaults(char *cmdline) { - UNUSED(cmdline); + bool saveConfigs; + + if (isEmpty(cmdline)) { + saveConfigs = true; + } else if (strncasecmp(cmdline, "nosave", 6) == 0) { + saveConfigs = false; + } else { + return; + } cliPrintHashLine("resetting to defaults"); - resetEEPROM(); - cliReboot(); + resetConfigs(); + if (saveConfigs) { + cliSave(NULL); + } } STATIC_UNIT_TESTED void cliGet(char *cmdline) @@ -2743,7 +2855,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) bool valueChanged = false; int16_t value = 0; switch (val->type & VALUE_MODE_MASK) { - case MODE_DIRECT: { + case MODE_DIRECT: { int16_t value = atoi(eqptr); if (value >= val->config.minmax.min && value <= val->config.minmax.max) { @@ -2753,7 +2865,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) } break; - case MODE_LOOKUP: { + case MODE_LOOKUP: { const lookupTableEntry_t *tableEntry = &lookupTables[val->config.lookup.tableIndex]; bool matched = false; for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { @@ -2769,7 +2881,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) } break; - case MODE_ARRAY: { + case MODE_ARRAY: { const uint8_t arrayLength = val->config.array.length; char *valPtr = eqptr; @@ -2788,7 +2900,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) default: case VAR_UINT8: { // fetch data pointer - uint8_t *data = (uint8_t *)getValuePointer(val) + i; + uint8_t *data = (uint8_t *)cliGetValuePointer(val) + i; // store value *data = (uint8_t)atoi((const char*) valPtr); } @@ -2796,7 +2908,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) case VAR_INT8: { // fetch data pointer - int8_t *data = (int8_t *)getValuePointer(val) + i; + int8_t *data = (int8_t *)cliGetValuePointer(val) + i; // store value *data = (int8_t)atoi((const char*) valPtr); } @@ -2804,7 +2916,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) case VAR_UINT16: { // fetch data pointer - uint16_t *data = (uint16_t *)getValuePointer(val) + i; + uint16_t *data = (uint16_t *)cliGetValuePointer(val) + i; // store value *data = (uint16_t)atoi((const char*) valPtr); } @@ -2812,7 +2924,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) case VAR_INT16: { // fetch data pointer - int16_t *data = (int16_t *)getValuePointer(val) + i; + int16_t *data = (int16_t *)cliGetValuePointer(val) + i; // store value *data = (int16_t)atoi((const char*) valPtr); } @@ -2979,14 +3091,15 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - cliPrintLinef("# %s / %s (%s) %s %s / %s (%s)", + cliPrintLinef("# %s / %s (%s) %s %s / %s (%s) MSP API: %s", FC_FIRMWARE_NAME, targetName, systemConfig()->boardIdentifier, FC_VERSION_STRING, buildDate, buildTime, - shortGitRevision + shortGitRevision, + MSP_API_VERSION_STRING ); } @@ -3332,7 +3445,7 @@ static void printConfig(char *cmdline, bool doDiff) if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { cliPrintHashLine("reset configuration to default settings"); - cliPrint("defaults"); + cliPrint("defaults nosave"); cliPrintLinefeed(); } @@ -3493,14 +3606,11 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif -#ifdef USE_RX_FRSKY_D - CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind), -#endif + CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), #endif - CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), - CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), + CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", "[nosave]", cliDefaults), CLI_COMMAND_DEF("diff", "list configuration changes from default", "[master|profile|rates|all] {defaults}", cliDiff), #ifdef USE_DSHOT @@ -3522,6 +3632,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("flash_read", NULL, "
", cliFlashRead), CLI_COMMAND_DEF("flash_write", NULL, "
", cliFlashWrite), #endif +#ifdef USE_RX_FRSKY_D + CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind), +#endif #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), #ifdef GPS diff --git a/src/main/fc/cli.h b/src/main/fc/cli.h index a0ad8bee53..d1624476c3 100644 --- a/src/main/fc/cli.h +++ b/src/main/fc/cli.h @@ -19,6 +19,10 @@ extern uint8_t cliMode; +struct clivalue_s; +void *cliGetValuePointer(const struct clivalue_s *value); +const void *cliGetDefaultPointer(const struct clivalue_s *value); + struct serialConfig_s; void cliInit(const struct serialConfig_s *serialConfig); void cliProcess(void); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 007596ab27..2e67640b51 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -124,7 +124,7 @@ PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig, .name = { 0 } ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 1); #ifndef USE_OSD_SLAVE #if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) @@ -156,12 +156,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, ); #endif -#ifdef BEEPER -PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); -PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, - .dshotForward = true -); -#endif #ifdef USE_ADC PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0); #endif @@ -273,7 +267,6 @@ static void setPidProfile(uint8_t pidProfileIndex) if (pidProfileIndex < MAX_PROFILE_COUNT) { systemConfigMutable()->pidProfileIndex = pidProfileIndex; currentPidProfile = pidProfilesMutable(pidProfileIndex); - pidInit(currentPidProfile); // re-initialise pid controller to re-initialise filters and config } } @@ -313,6 +306,7 @@ void activateConfig(void) resetAdjustmentStates(); + pidInit(currentPidProfile); useRcControlsConfig(currentPidProfile); useAdjustmentConfig(currentPidProfile); @@ -348,6 +342,13 @@ void validateAndFixConfig(void) #endif #ifndef USE_OSD_SLAVE + if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) { + systemConfigMutable()->activeRateProfile = 0; + } + if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) { + systemConfigMutable()->pidProfileIndex = 0; + } + if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { motorConfigMutable()->mincommand = 1000; } @@ -376,7 +377,7 @@ void validateAndFixConfig(void) if (featureConfigured(FEATURE_RX_SPI)) { featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP); } -#endif +#endif // USE_RX_SPI if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); @@ -387,7 +388,7 @@ void validateAndFixConfig(void) if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; } -#endif +#endif // STM32F10X // software serial needs free PWM ports featureClear(FEATURE_SOFTSERIAL); } @@ -404,9 +405,9 @@ void validateAndFixConfig(void) if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; } -#endif +#endif // STM32F10X } -#endif +#endif // USE_SOFTSPI // Prevent invalid notch cutoff if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) { @@ -414,7 +415,7 @@ void validateAndFixConfig(void) } validateAndFixGyroConfig(); -#endif +#endif // USE_OSD_SLAVE if (!isSerialConfigValid(serialConfig())) { pgResetFn_serialConfig(serialConfigMutable()); @@ -507,17 +508,13 @@ void validateAndFixGyroConfig(void) gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } - float samplingTime = 0.000125f; - if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed gyroConfigMutable()->gyro_sync_denom = 1; gyroConfigMutable()->gyro_use_32khz = false; - samplingTime = 0.001f; } if (gyroConfig()->gyro_use_32khz) { - samplingTime = 0.00003125; // F1 and F3 can't handle high sample speed. #if defined(STM32F1) gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16); @@ -530,44 +527,69 @@ void validateAndFixGyroConfig(void) #endif } + float samplingTime; + switch (gyroMpuDetectionResult()->sensor) { + case ICM_20649_SPI: + samplingTime = 1.0f / 9000.0f; + break; + case BMI_160_SPI: + samplingTime = 0.0003125f; + break; + default: + samplingTime = 0.000125f; + break; + } + if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { + switch (gyroMpuDetectionResult()->sensor) { + case ICM_20649_SPI: + samplingTime = 1.0f / 1100.0f; + break; + default: + samplingTime = 0.001f; + break; + } + } + if (gyroConfig()->gyro_use_32khz) { + samplingTime = 0.00003125; + } + // check for looptime restrictions based on motor protocol. Motor times have safety margin - const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom; float motorUpdateRestriction; switch (motorConfig()->dev.motorPwmProtocol) { - case (PWM_TYPE_STANDARD): + case PWM_TYPE_STANDARD: motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE; break; - case (PWM_TYPE_ONESHOT125): + case PWM_TYPE_ONESHOT125: motorUpdateRestriction = 0.0005f; break; - case (PWM_TYPE_ONESHOT42): + case PWM_TYPE_ONESHOT42: motorUpdateRestriction = 0.0001f; break; #ifdef USE_DSHOT - case (PWM_TYPE_DSHOT150): + case PWM_TYPE_DSHOT150: motorUpdateRestriction = 0.000250f; break; - case (PWM_TYPE_DSHOT300): + case PWM_TYPE_DSHOT300: motorUpdateRestriction = 0.0001f; break; #endif - default: - motorUpdateRestriction = 0.00003125f; + default: + motorUpdateRestriction = 0.00003125f; + break; } - if (!motorConfig()->dev.useUnsyncedPwm) { - if (pidLooptime < motorUpdateRestriction) { - const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM); - - pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom); - } - } else { + if (motorConfig()->dev.useUnsyncedPwm) { // Prevent overriding the max rate of motors if ((motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && (motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD)) { const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); - motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate); } + } else { + const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom; + if (pidLooptime < motorUpdateRestriction) { + const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM); + pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom); + } } } #endif @@ -582,19 +604,12 @@ void readEEPROM(void) if (!loadEEPROM()) { failureMode(FAILURE_INVALID_EEPROM_CONTENTS); } -#ifndef USE_OSD_SLAVE - if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {// sanity check - systemConfigMutable()->activeRateProfile = 0; - } - setControlRateProfile(systemConfig()->activeRateProfile); - - if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) {// sanity check - systemConfigMutable()->pidProfileIndex = 0; - } - setPidProfile(systemConfig()->pidProfileIndex); -#endif validateAndFixConfig(); +#ifndef USE_OSD_SLAVE + setControlRateProfile(systemConfig()->activeRateProfile); + setPidProfile(systemConfig()->pidProfileIndex); +#endif activateConfig(); #ifndef USE_OSD_SLAVE diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 40d7fe1a13..d4460900fb 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -110,7 +110,7 @@ void setPreferredBeeperOffMask(uint32_t mask); void initEEPROM(void); void resetEEPROM(void); void readEEPROM(void); -void writeEEPROM(); +void writeEEPROM(void); void ensureEEPROMContainsValidData(void); void saveConfigAndNotify(void); diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index c89c9ea8e2..b8a4c65d52 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -71,3 +71,11 @@ void changeControlRateProfile(uint8_t controlRateProfileIndex) setControlRateProfile(controlRateProfileIndex); generateThrottleCurve(); } + +void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex) { + if ((dstControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1 && srcControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1) + && dstControlRateProfileIndex != srcControlRateProfileIndex + ) { + memcpy(controlRateProfilesMutable(dstControlRateProfileIndex), controlRateProfilesMutable(srcControlRateProfileIndex), sizeof(controlRateConfig_t)); + } +} diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 2911907077..2978d9110a 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -41,3 +41,5 @@ extern controlRateConfig_t *currentControlRateProfile; void setControlRateProfile(uint8_t controlRateProfileIndex); void changeControlRateProfile(uint8_t controlRateProfileIndex); + +void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0cd9933e9a..8326a434d8 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -109,6 +109,8 @@ int16_t magHold; int16_t headFreeModeHold; static bool reverseMotors = false; +static bool flipOverAfterCrashMode = false; + static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; @@ -129,7 +131,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD saveConfigAndNotify(); } -static bool isCalibrating() +static bool isCalibrating(void) { #ifdef BARO if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) { @@ -188,7 +190,7 @@ void updateArmingStatus(void) unsetArmingDisabled(ARMING_DISABLED_THROTTLE); } - if (!STATE(SMALL_ANGLE)) { + if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { setArmingDisabled(ARMING_DISABLED_ANGLE); } else { unsetArmingDisabled(ARMING_DISABLED_ANGLE); @@ -261,14 +263,19 @@ void tryArm(void) return; } #ifdef USE_DSHOT - if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXDSHOTREVERSE)) { - if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) { - reverseMotors = false; + if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { + pwmDisableMotors(); + delay(1); + + if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { + flipOverAfterCrashMode = false; pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL); } else { - reverseMotors = true; + flipOverAfterCrashMode = true; pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED); } + + pwmEnableMotors(); } #endif @@ -361,7 +368,9 @@ void updateMagHold(void) } #endif - +/* + * processRx called from taskUpdateRxMain + */ void processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; @@ -719,7 +728,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs) } } -bool isMotorsReversed() +bool isMotorsReversed(void) { return reverseMotors; } +bool isFlipOverAfterCrashMode(void) +{ + return flipOverAfterCrashMode; +} diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 085ce3d758..e42957633c 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -36,7 +36,7 @@ PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig); union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); -void handleInflightCalibrationStickPosition(); +void handleInflightCalibrationStickPosition(void); void resetArmingDisabled(void); @@ -49,3 +49,4 @@ void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); bool isMotorsReversed(void); +bool isFlipOverAfterCrashMode(void); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 2b687885ce..80ef8dc2e5 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -200,6 +200,9 @@ void spiPreInit(void) #ifdef USE_GYRO_SPI_MPU9250 spiPreInitCs(IO_TAG(MPU9250_CS_PIN)); #endif +#ifdef USE_GYRO_SPI_ICM20649 + spiPreInitCs(IO_TAG(ICM20649_CS_PIN)); +#endif #ifdef USE_GYRO_SPI_ICM20689 spiPreInitCs(IO_TAG(ICM20689_CS_PIN)); #endif @@ -210,7 +213,7 @@ void spiPreInit(void) spiPreInitCs(IO_TAG(L3GD20_CS_PIN)); #endif #ifdef USE_MAX7456 - spiPreInitCs(IO_TAG(MAX7456_SPI_CS_PIN)); + spiPreInitCsOutPU(IO_TAG(MAX7456_SPI_CS_PIN)); // XXX 3.2 workaround for Kakute F4. See comment for spiPreInitCSOutPU. #endif #ifdef USE_SDCARD spiPreInitCs(IO_TAG(SDCARD_SPI_CS_PIN)); @@ -270,20 +273,6 @@ void init(void) resetEEPROM(); } -#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) - // If F4 Overclocking is set and System core clock is not correct a reset is forced - if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { - *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); - } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { - *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); - } - -#endif - systemState |= SYSTEM_STATE_CONFIG_LOADED; //i2cSetOverclock(masterConfig.i2c_overclock); @@ -348,6 +337,20 @@ void init(void) } #endif +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + // If F4 Overclocking is set and System core clock is not correct a reset is forced + if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } + +#endif + delay(100); timerInit(); // timer must be initialized before any channel is allocated @@ -373,32 +376,6 @@ void init(void) serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif - mixerInit(mixerConfig()->mixerMode); -#ifdef USE_SERVOS - servosInit(); -#endif - - uint16_t idlePulse = motorConfig()->mincommand; - if (feature(FEATURE_3D)) { - idlePulse = flight3DConfig()->neutral3d; - } - - if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { - featureClear(FEATURE_3D); - idlePulse = 0; // brushed motors - } - - mixerConfigureOutput(); - motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount()); - -#ifdef USE_SERVOS - servoConfigureOutput(); - if (isMixerUsingServos()) { - //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); - servoDevInit(&servoConfig()->dev); - } -#endif - if (0) {} #if defined(USE_PPM) else if (feature(FEATURE_RX_PPM)) { @@ -411,8 +388,6 @@ void init(void) } #endif - systemState |= SYSTEM_STATE_MOTORS_READY; - #ifdef BEEPER beeperInit(beeperDevConfig()); #endif @@ -525,10 +500,33 @@ void init(void) LED0_OFF; LED1_OFF; - // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit() + // gyro.targetLooptime set in sensorsAutodetect(), + // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter() + validateAndFixGyroConfig(); pidInit(currentPidProfile); + setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); + + mixerInit(mixerConfig()->mixerMode); + + uint16_t idlePulse = motorConfig()->mincommand; + if (feature(FEATURE_3D)) { + idlePulse = flight3DConfig()->neutral3d; + } + if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { + featureClear(FEATURE_3D); + idlePulse = 0; // brushed motors + } + mixerConfigureOutput(); + motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount()); + systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef USE_SERVOS + servosInit(); + servoConfigureOutput(); + if (isMixerUsingServos()) { + //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); + servoDevInit(&servoConfig()->dev); + } servosFilterInit(); #endif @@ -674,7 +672,7 @@ void init(void) #endif #ifdef VTX_RTC6705 -#ifdef VTX_RTC6705OPTIONAL +#ifdef VTX_RTC6705_OPTIONAL if (!vtxCommonDeviceRegistered()) // external VTX takes precedence when configured. #endif { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8281750ecc..8869842cb8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -108,6 +108,7 @@ #include "sensors/gyro.h" #include "sensors/sensors.h" #include "sensors/sonar.h" +#include "sensors/esc_sensor.h" #include "telemetry/telemetry.h" @@ -310,7 +311,12 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin sbufWriteU32(dst, address); // legacy format does not support compression +#ifdef USE_HUFFMAN const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN; +#else + const uint8_t compressionMethod = NO_COMPRESSION; + UNUSED(allowCompression); +#endif if (compressionMethod == NO_COMPRESSION) { if (!useLegacyFormat) { @@ -750,8 +756,10 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) if (acc.dev.acc_1G > 512*4) { scale = 8; - } else if (acc.dev.acc_1G >= 512) { + } else if (acc.dev.acc_1G > 512*2) { scale = 4; + } else if (acc.dev.acc_1G >= 512) { + scale = 2; } else { scale = 1; } @@ -923,6 +931,16 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) break; #endif +#ifdef USE_DSHOT + case MSP_ESC_SENSOR_DATA: + sbufWriteU8(dst, getMotorCount()); + for (int i = 0; i < getMotorCount(); i++) { + sbufWriteU8(dst, getEscSensorData(i)->temperature); + sbufWriteU16(dst, getEscSensorData(i)->rpm); + } + break; +#endif + #ifdef GPS case MSP_GPS_CONFIG: sbufWriteU8(dst, gpsConfig()->provider); @@ -1162,7 +1180,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, currentPidProfile->rateAccelLimit); sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit); sbufWriteU8(dst, currentPidProfile->levelAngleLimit); - sbufWriteU8(dst, currentPidProfile->levelSensitivity); + sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold); sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain); break; @@ -1314,6 +1332,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; + case MSP_COPY_PROFILE: + value = sbufReadU8(src); // 0 = pid profile, 1 = control rate profile + uint8_t dstProfileIndex = sbufReadU8(src); + uint8_t srcProfileIndex = sbufReadU8(src); + if (value == 0) { + pidCopyProfile(dstProfileIndex, srcProfileIndex); + } + else if (value == 1) { + copyControlRateProfile(dstProfileIndex, srcProfileIndex); + } + break; + #if defined(GPS) || defined(MAG) case MSP_SET_HEADING: magHold = sbufReadU16(src); @@ -1571,7 +1601,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentPidProfile->yawRateAccelLimit = sbufReadU16(src); if (sbufBytesRemaining(src) >= 2) { currentPidProfile->levelAngleLimit = sbufReadU8(src); - currentPidProfile->levelSensitivity = sbufReadU8(src); + sbufReadU8(src); // was pidProfile.levelSensitivity } if (sbufBytesRemaining(src) >= 4) { currentPidProfile->itermThrottleThreshold = sbufReadU16(src); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 769d1c7e13..c35ad51ae6 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -76,7 +76,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXCAMERA1, "CAMERA CONTROL 1", 32}, { BOXCAMERA2, "CAMERA CONTROL 2", 33}, { BOXCAMERA3, "CAMERA CONTROL 3", 34 }, - { BOXDSHOTREVERSE, "DSHOT REVERSE MOTORS", 35 }, + { BOXFLIPOVERAFTERCRASH, "FLIP OVER AFTER CRASH", 35 }, { BOXPREARM, "PREARM", 36 }, }; @@ -217,7 +217,7 @@ void initActiveBoxIds(void) } if (isMotorProtocolDshot()) { - BME(BOXDSHOTREVERSE); + BME(BOXFLIPOVERAFTERCRASH); } if (feature(FEATURE_SERVO_TILT)) { @@ -290,7 +290,7 @@ int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) const uint64_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON) | BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD) | BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE) - | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXDSHOTREVERSE) | BM(BOX3DDISABLE); + | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXFLIPOVERAFTERCRASH) | BM(BOX3DDISABLE); STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes); for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) { if ((rcModeCopyMask & BM(i)) // mode copy is enabled diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index b620aefd16..3d4439be2b 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -23,14 +23,11 @@ #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/maths.h" #include "common/utils.h" -#include "common/filter.h" #include "config/feature.h" -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -40,33 +37,36 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/pid.h" #include "rx/rx.h" #include "scheduler/scheduler.h" #include "sensors/battery.h" -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/pid.h" -#include "flight/mixer.h" static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; -float getSetpointRate(int axis) { +float getSetpointRate(int axis) +{ return setpointRate[axis]; } -float getRcDeflection(int axis) { +float getRcDeflection(int axis) +{ return rcDeflection[axis]; } -float getRcDeflectionAbs(int axis) { +float getRcDeflectionAbs(int axis) +{ return rcDeflectionAbs[axis]; } -float getThrottlePIDAttenuation(void) { +float getThrottlePIDAttenuation(void) +{ return throttlePIDAttenuation; } @@ -75,10 +75,8 @@ static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for void generateThrottleCurve(void) { - uint8_t i; - - for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - int16_t tmp = 10 * i - currentControlRateProfile->thrMid8; + for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { + const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8; uint8_t y = 1; if (tmp > 0) y = 100 - currentControlRateProfile->thrMid8; @@ -89,7 +87,7 @@ void generateThrottleCurve(void) } } -int16_t rcLookupThrottle(int32_t tmp) +static int16_t rcLookupThrottle(int32_t tmp) { const int32_t tmp2 = tmp / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] @@ -114,6 +112,7 @@ static void calculateSetpointRate(int axis) rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f); } + // scale rcCommandf to range [-1.0, 1.0] float rcCommandf = rcCommand[axis] / 500.0f; rcDeflection[axis] = rcCommandf; const float rcCommandfAbs = ABS(rcCommandf); @@ -135,7 +134,8 @@ static void calculateSetpointRate(int axis) setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) } -static void scaleRcCommandToFpvCamAngle(void) { +static void scaleRcCommandToFpvCamAngle(void) +{ //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed static uint8_t lastFpvCamAngleDegrees = 0; static float cosFactor = 1.0; @@ -156,23 +156,27 @@ static void scaleRcCommandToFpvCamAngle(void) { #define THROTTLE_BUFFER_MAX 20 #define THROTTLE_DELTA_MS 100 - static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) { +static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) +{ static int index; static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX]; + const int rxRefreshRateMs = rxRefreshRate / 1000; const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX); const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; - if (index >= indexMax) + if (index >= indexMax) { index = 0; + } const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; - if (ABS(rcCommandSpeed) > throttleVelocityThreshold) + if (ABS(rcCommandSpeed) > throttleVelocityThreshold) { pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); - else + } else { pidSetItermAccelerator(1.0f); + } } void processRcCommand(void) @@ -181,10 +185,6 @@ void processRcCommand(void) static float rcStepSize[4] = { 0, 0, 0, 0 }; static int16_t rcInterpolationStepCount; static uint16_t currentRxRefreshRate; - const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT" - uint16_t rxRefreshRate; - bool readyToCalculateRate = false; - uint8_t readyToCalculateRateAxisCnt = 0; if (isRXDataNew) { currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); @@ -193,19 +193,24 @@ void processRcCommand(void) } } + const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT" + uint16_t rxRefreshRate; + bool readyToCalculateRate = false; + uint8_t readyToCalculateRateAxisCnt = 0; + if (rxConfig()->rcInterpolation) { // Set RC refresh rate for sampling and channels to filter switch (rxConfig()->rcInterpolation) { - case(RC_SMOOTHING_AUTO): - rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps - break; - case(RC_SMOOTHING_MANUAL): - rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval; - break; - case(RC_SMOOTHING_OFF): - case(RC_SMOOTHING_DEFAULT): - default: - rxRefreshRate = rxGetRefreshRate(); + case RC_SMOOTHING_AUTO: + rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps + break; + case RC_SMOOTHING_MANUAL: + rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval; + break; + case RC_SMOOTHING_OFF: + case RC_SMOOTHING_DEFAULT: + default: + rxRefreshRate = rxGetRefreshRate(); } if (isRXDataNew && rxRefreshRate > 0) { @@ -239,19 +244,22 @@ void processRcCommand(void) } if (readyToCalculateRate || isRXDataNew) { - if (isRXDataNew) + if (isRXDataNew) { readyToCalculateRateAxisCnt = FD_YAW; + } - for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) + for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) { calculateSetpointRate(axis); + } if (debugMode == DEBUG_RC_INTERPOLATION) { debug[2] = rcInterpolationStepCount; debug[3] = setpointRate[0]; } // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) - if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) + if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { scaleRcCommandToFpvCamAngle(); + } isRXDataNew = false; } @@ -329,7 +337,8 @@ void updateRcCommands(void) } } -void resetYawAxis(void) { +void resetYawAxis(void) +{ rcCommand[YAW] = 0; setpointRate[YAW] = 0; } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index b2dcf762ad..475de617c4 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -57,6 +57,7 @@ #include "sensors/acceleration.h" #include "rx/rx.h" +#include "scheduler/scheduler.h" #include "flight/pid.h" #include "flight/navigation.h" @@ -118,11 +119,22 @@ throttleStatus_e calculateThrottleStatus(void) return THROTTLE_HIGH; } +#define ARM_DELAY_MS 500 +#define STICK_DELAY_MS 50 +#define STICK_AUTOREPEAT_MS 250 +#define repeatAfter(t) { \ + rcDelayMs -= (t); \ + doNotRepeat = false; \ +} void processRcStickPositions(throttleStatus_e throttleStatus) { - static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors - static uint8_t rcSticks; // this hold sticks position for command combos - static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it + // time the sticks are maintained + static int16_t rcDelayMs; + // hold sticks position for command combos + static uint8_t rcSticks; + // an extra guard for disarming through switch to prevent that one frame can disarm it + static uint8_t rcDisarmTicks; + static bool doNotRepeat; uint8_t stTmp = 0; int i; @@ -132,7 +144,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } #endif - // ------------------ STICKS COMMAND HANDLER -------------------- // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; @@ -142,15 +153,16 @@ void processRcStickPositions(throttleStatus_e throttleStatus) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { - if (rcDelayCommand < 250) - rcDelayCommand++; - } else - rcDelayCommand = 0; + if (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000)) + rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000; + } else { + rcDelayMs = 0; + doNotRepeat = false; + } rcSticks = stTmp; // perform actions if (!isUsingSticksToArm) { - if (IS_RC_MODE_ACTIVE(BOXARM)) { rcDisarmTicks = 0; // Arming via ARM BOX @@ -158,7 +170,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } else { // Disarming via ARM BOX resetArmingDisabled(); - if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { rcDisarmTicks++; if (rcDisarmTicks > 3) { @@ -170,31 +181,37 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } } } - } - - if (rcDelayCommand != 20) { - return; - } - - if (isUsingSticksToArm) { - // Disarm on throttle down + yaw - if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { + } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { + if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) { + doNotRepeat = true; + // Disarm on throttle down + yaw if (ARMING_FLAG(ARMED)) disarm(); else { - beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held - rcDelayCommand = 0; // reset so disarm tone will repeat + beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held + repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat + } + } + return; + } else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { + if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) { + doNotRepeat = true; + if (!ARMING_FLAG(ARMED)) { + // Arm via YAW + tryArm(); + } else { + resetArmingDisabled(); } } - } - - if (ARMING_FLAG(ARMED)) { - // actions during armed return; } + if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS) { + return; + } + doNotRepeat = true; + // actions during not armed - i = 0; if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration @@ -221,6 +238,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } // Multiple configuration profiles + i = 0; if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 i = 1; else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 @@ -236,18 +254,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) saveConfigAndNotify(); } - if (isUsingSticksToArm) { - - if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { - // Arm via YAW - tryArm(); - - return; - } else { - resetArmingDisabled(); - } - } - if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { // Calibrating Acc accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); @@ -283,7 +289,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } if (shouldApplyRollAndPitchTrimDelta) { applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta); - rcDelayCommand = 0; // allow autorepetition + repeatAfter(STICK_AUTOREPEAT_MS); return; } @@ -325,7 +331,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0); } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) { cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000); - } + } #endif } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 25658e9fdb..500103c4f2 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -57,7 +57,7 @@ typedef enum { BOXCAMERA1, BOXCAMERA2, BOXCAMERA3, - BOXDSHOTREVERSE, + BOXFLIPOVERAFTERCRASH, BOXPREARM, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 66444de380..0094679329 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -31,8 +31,8 @@ static uint32_t enabledSensors = 0; #if defined(OSD) || !defined(MINIMAL_CLI) const char *armingDisableFlagNames[]= { - "NOGYRO", "FAILSAFE", "RX LOSS", "BAD RX", "BOXFAILSAFE", - "THROTTLE", "ANGLE", "BOOT GRACE", "NO PREARM", "ARM SWITCH", + "NOGYRO", "FAILSAFE", "RXLOSS", "BADRX", "BOXFAILSAFE", + "THROTTLE", "ANGLE", "BOOTGRACE", "NOPREARM", "ARMSWITCH", "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST" }; #endif diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 5fa30ab16b..f2abb232a1 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -38,6 +38,7 @@ #include "drivers/light_led.h" #include "drivers/camera_control.h" +#include "drivers/max7456.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -60,6 +61,7 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" +#include "io/vtx_control.h" #include "io/vtx_rtc6705.h" #include "rx/rx.h" @@ -81,13 +83,15 @@ // sync with accelerationSensor_e const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20689", "BMI160", "FAKE" + "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20649", "ICM20689", + "BMI160", "FAKE" }; // sync with gyroSensor_e const char * const lookupTableGyroHardware[] = { "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE" + "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", + "BMI160", "FAKE" }; #if defined(USE_SENSOR_NAMES) || defined(BARO) @@ -248,6 +252,12 @@ static const char * const lookupTableBusType[] = { "NONE", "I2C", "SPI" }; +#ifdef USE_MAX7456 +static const char * const lookupTableMax7456Clock[] = { + "HALF", "DEFAULT", "FULL" +}; +#endif + const lookupTableEntry_t lookupTables[] = { { lookupTableOffOn, sizeof(lookupTableOffOn) / sizeof(char *) }, { lookupTableUnit, sizeof(lookupTableUnit) / sizeof(char *) }, @@ -294,19 +304,25 @@ const lookupTableEntry_t lookupTables[] = { { lookupTableCameraControlMode, sizeof(lookupTableCameraControlMode) / sizeof(char *) }, #endif { lookupTableBusType, sizeof(lookupTableBusType) / sizeof(char *) }, +#ifdef USE_MAX7456 + { lookupTableMax7456Clock, sizeof(lookupTableMax7456Clock) / sizeof(char *) }, +#endif }; const clivalue_t valueTable[] = { // PG_GYRO_CONFIG { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf) }, +#if defined(USE_GYRO_SPI_ICM20649) + { "gyro_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) }, +#endif { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) }, { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_type) }, { "gyro_lowpass_hz", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz) }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) }, - { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) }, + { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) }, { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) }, - { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, + { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, #if defined(GYRO_USES_SPI) #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) @@ -320,6 +336,9 @@ const clivalue_t valueTable[] = { // PG_ACCELEROMETER_CONFIG { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) }, +#if defined(USE_GYRO_SPI_ICM20649) + { "acc_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_high_fsr) }, +#endif { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) }, { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) }, { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) }, @@ -366,7 +385,7 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) }, - { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, + { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, #endif { "airmode_start_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) }, @@ -385,7 +404,7 @@ const clivalue_t valueTable[] = { // PG_BLACKBOX_CONFIG #ifdef BLACKBOX - { "blackbox_p_denom", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, INT16_MAX }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, p_denom) }, + { "blackbox_p_ratio", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, INT16_MAX }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, p_denom) }, { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_DEVICE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, device) }, { "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, on_motor_test) }, { "blackbox_record_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, record_acc) }, @@ -460,7 +479,7 @@ const clivalue_t valueTable[] = { // PG_BEEPER_CONFIG #ifdef USE_DSHOT - { "beeper_dshot", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BEEPER_CONFIG, offsetof(beeperConfig_t, dshotForward) }, + { "beeper_dshot_beacon_tone", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, DSHOT_CMD_BEACON5 }, PG_BEEPER_CONFIG, offsetof(beeperConfig_t, dshotBeaconTone) }, #endif #endif @@ -549,7 +568,7 @@ const clivalue_t valueTable[] = { { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz) }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) }, - { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) }, + { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) }, { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) }, { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, @@ -596,8 +615,7 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) }, - { "level_sensitivity", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelSensitivity) }, - { "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 120 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) }, + { "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) }, { "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) }, { "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) }, @@ -716,13 +734,18 @@ const clivalue_t valueTable[] = { #endif { "pwr_on_arm_grace", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, powerOnArmingGraceTime) }, -// PG_VTX_CONFIG +// PG_VTX_RTC6705_CONFIG #ifdef VTX_RTC6705 { "vtx_band", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, band) }, { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 8 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, channel) }, { "vtx_power", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, RTC6705_POWER_COUNT - 1 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, power) }, #endif +// PG_VTX_CONFIG +#if defined(VTX_CONTROL) && defined(VTX_COMMON) + { "vtx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, halfDuplex) }, +#endif + // PG_VCD_CONFIG #ifdef USE_MAX7456 { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 2 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, video_system) }, @@ -730,6 +753,11 @@ const clivalue_t valueTable[] = { { "vcd_v_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -15, 16 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, v_offset) }, #endif +// PG_MAX7456_CONFIG +#ifdef USE_MAX7456 + { "max7456_clock", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MAX7456_CLOCK }, PG_MAX7456_CONFIG, offsetof(max7456Config_t, clockConfig) }, +#endif + // PG_DISPLAY_PORT_MSP_CONFIG #ifdef USE_MSP_DISPLAYPORT { "displayport_msp_col_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) }, @@ -766,6 +794,7 @@ const clivalue_t valueTable[] = { { "camera_control_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CAMERA_CONTROL_MODE }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, mode) }, { "camera_control_ref_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 200, 400 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, refVoltage) }, { "camera_control_key_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, keyDelayMs) }, + { "camera_control_internal_resistance", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 1000 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, internalResistance) }, #endif }; diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 6dcf5d37f0..7283bb4f27 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -17,6 +17,10 @@ #pragma once +#include +#include +#include "config/parameter_group.h" + typedef enum { TABLE_OFF_ON = 0, @@ -64,6 +68,9 @@ typedef enum { TABLE_CAMERA_CONTROL_MODE, #endif TABLE_BUS_TYPE, +#ifdef USE_MAX7456 + TABLE_MAX7456_CLOCK, +#endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; @@ -119,7 +126,7 @@ typedef union { cliArrayLengthConfig_t array; } cliValueConfig_t; -typedef struct { +typedef struct clivalue_s { const char *name; const uint8_t type; // see cliValueFlag_e const cliValueConfig_t config; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 581fe287ac..0d179fbcba 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -80,7 +80,7 @@ void failsafeReset(void); void failsafeStartMonitoring(void); void failsafeUpdateState(void); -failsafePhase_e failsafePhase(); +failsafePhase_e failsafePhase(void); bool failsafeIsMonitoring(void); bool failsafeIsActive(void); bool failsafeIsReceivingRxData(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 0cf6465d42..b6edc62c69 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -23,6 +23,7 @@ #include "platform.h" #include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" #include "common/filter.h" @@ -43,6 +44,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/fc_core.h" +#include "fc/fc_rc.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -102,12 +104,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); -#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 -// (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) -#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 -#define EXTERNAL_CONVERSION_MIN_VALUE 1000 -#define EXTERNAL_CONVERSION_MAX_VALUE 2000 -#define EXTERNAL_CONVERSION_3D_MID_VALUE 1500 +#define PWM_RANGE_MID 1500 #define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight @@ -312,8 +309,11 @@ const mixer_t mixers[] = { }; #endif // !USE_QUAD_MIXER_ONLY -static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; float motorOutputHigh, motorOutputLow; + +static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; +static uint16_t rcCommand3dDeadBandLow; +static uint16_t rcCommand3dDeadBandHigh; static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; uint8_t getMotorCount(void) @@ -356,7 +356,9 @@ bool mixerIsOutputSaturated(int axis, float errorRate) // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // DSHOT scaling is done to the actual dshot range -void initEscEndpoints(void) { +void initEscEndpoints(void) +{ + // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet switch (motorConfig()->dev.motorPwmProtocol) { #ifdef USE_DSHOT case PWM_TYPE_PROSHOT1000: @@ -365,10 +367,11 @@ void initEscEndpoints(void) { case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: disarmMotorOutput = DSHOT_DISARM_COMMAND; - if (feature(FEATURE_3D)) + if (feature(FEATURE_3D)) { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); - else + } else { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); + } motorOutputHigh = DSHOT_MAX_THROTTLE; deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; @@ -376,8 +379,13 @@ void initEscEndpoints(void) { break; #endif default: - disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand; - motorOutputLow = motorConfig()->minthrottle; + if (feature(FEATURE_3D)) { + disarmMotorOutput = flight3DConfig()->neutral3d; + motorOutputLow = PWM_RANGE_MIN; + } else { + disarmMotorOutput = motorConfig()->mincommand; + motorOutputLow = motorConfig()->minthrottle; + } motorOutputHigh = motorConfig()->maxthrottle; deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; deadbandMotor3dLow = flight3DConfig()->deadband3d_low; @@ -385,9 +393,13 @@ void initEscEndpoints(void) { break; } - rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck); - rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle; - rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck; + + rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle; + + rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; + rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; } void mixerInit(mixerMode_e mixerMode) @@ -514,53 +526,112 @@ void stopPwmAllMotors(void) delayMicroseconds(1500); } -float throttle = 0; -float motorOutputMin, motorOutputMax; -bool mixerInversion = false; -float motorOutputRange; +static float throttle = 0; +static float motorOutputMin; +static float motorRangeMin; +static float motorRangeMax; +static float motorOutputRange; +static int8_t motorOutputMixSign; void calculateThrottleAndCurrentMotorEndpoints(void) { - static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions + static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions float currentThrottleInputRange = 0; if(feature(FEATURE_3D)) { - if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. + if (!ARMING_FLAG(ARMED)) { + rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. + } - if((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { - motorOutputMax = deadbandMotor3dLow; - motorOutputMin = motorOutputLow; - throttlePrevious = rcCommand[THROTTLE]; //3D Mode Throttle Fix #3696 - throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; //3D Mode Throttle Fix #3696 + if(rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) { + // INVERTED + motorRangeMin = motorOutputLow; + motorRangeMax = deadbandMotor3dLow; + if(isMotorProtocolDshot()) { + motorOutputMin = motorOutputLow; + motorOutputRange = deadbandMotor3dLow - motorOutputLow; + } else { + motorOutputMin = deadbandMotor3dLow; + motorOutputRange = motorOutputLow - deadbandMotor3dLow; + } + motorOutputMixSign = -1; + rcThrottlePrevious = rcCommand[THROTTLE]; + throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; currentThrottleInputRange = rcCommandThrottleRange3dLow; - if(isMotorProtocolDshot()) mixerInversion = true; - } else if(rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { - motorOutputMax = motorOutputHigh; + } else if(rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { + // NORMAL + motorRangeMin = deadbandMotor3dHigh; + motorRangeMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; - throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; + motorOutputMixSign = 1; + rcThrottlePrevious = rcCommand[THROTTLE]; + throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { - motorOutputMax = deadbandMotor3dLow; - motorOutputMin = motorOutputLow; - throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow)) { + // INVERTED_TO_DEADBAND + motorRangeMin = motorOutputLow; + motorRangeMax = deadbandMotor3dLow; + if(isMotorProtocolDshot()) { + motorOutputMin = motorOutputLow; + motorOutputRange = deadbandMotor3dLow - motorOutputLow; + } else { + motorOutputMin = deadbandMotor3dLow; + motorOutputRange = motorOutputLow - deadbandMotor3dLow; + } + motorOutputMixSign = -1; + throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dLow; - if(isMotorProtocolDshot()) mixerInversion = true; } else { - motorOutputMax = motorOutputHigh; + // NORMAL_TO_DEADBAND + motorRangeMin = deadbandMotor3dHigh; + motorRangeMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; + motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; + motorOutputMixSign = 1; throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dHigh; } } else { throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange; + motorRangeMin = motorOutputLow; + motorRangeMax = motorOutputHigh; motorOutputMin = motorOutputLow; - motorOutputMax = motorOutputHigh; + motorOutputRange = motorOutputHigh - motorOutputLow; + motorOutputMixSign = 1; } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); - motorOutputRange = motorOutputMax - motorOutputMin; +} + +#define CRASH_FLIP_DEADBAND 20 + +static void applyFlipOverAfterCrashModeToMotors(void) +{ + float motorMix[MAX_SUPPORTED_MOTORS]; + + if (ARMING_FLAG(ARMED)) { + for (int i = 0; i < motorCount; i++) { + if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) { + motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * -1; + } else { + motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * -1; + } + + // Apply the mix to motor endpoints + float motorOutput = motorOutputMin + motorOutputRange * motorMix[i]; + //Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered + motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND ) ? disarmMotorOutput : motorOutput - CRASH_FLIP_DEADBAND; + + motor[i] = motorOutput; + } + } else { + // Disarmed mode + for (int i = 0; i < motorCount; i++) { + motor[i] = motor_disarmed[i]; + } + } } static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) @@ -568,21 +639,16 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (uint32_t i = 0; i < motorCount; i++) { - float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)); - - // Dshot works exactly opposite in lower 3D section. - if (mixerInversion) { - motorOutput = motorOutputMin + (motorOutputMax - motorOutput); - } + float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle)); if (failsafeIsActive()) { if (isMotorProtocolDshot()) { - motorOutput = (motorOutput < motorOutputMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range + motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range } - motorOutput = constrain(motorOutput, disarmMotorOutput, motorOutputMax); + motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax); } else { - motorOutput = constrain(motorOutput, motorOutputMin, motorOutputMax); + motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } // Motor stop handling @@ -604,6 +670,11 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) void mixTable(uint8_t vbatPidCompensation) { + if (isFlipOverAfterCrashMode()) { + applyFlipOverAfterCrashModeToMotors(); + return; + } + // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(); @@ -668,44 +739,62 @@ void mixTable(uint8_t vbatPidCompensation) float convertExternalToMotor(uint16_t externalValue) { - uint16_t motorValue = externalValue; + uint16_t motorValue; + switch ((int)isMotorProtocolDshot()) { #ifdef USE_DSHOT - if (isMotorProtocolDshot()) { - // Add 1 to the value, otherwise throttle tops out at 2046 - motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR + 1, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); + case true: + externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX); if (feature(FEATURE_3D)) { - if (externalValue == EXTERNAL_CONVERSION_3D_MID_VALUE) { + if (externalValue == PWM_RANGE_MID) { motorValue = DSHOT_DISARM_COMMAND; - } else if (motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) { - // Add 1 to the value, otherwise throttle tops out at 2046 - motorValue = DSHOT_MIN_THROTTLE + (DSHOT_3D_DEADBAND_LOW - motorValue) + 1; + } else if (externalValue < PWM_RANGE_MID) { + motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE); + } else { + motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); } + } else { + motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_DISARM_COMMAND : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); } - } + + break; + case false: #endif + default: + motorValue = externalValue; + + break; + } return (float)motorValue; } uint16_t convertMotorToExternal(float motorValue) { - uint16_t externalValue = lrintf(motorValue); + uint16_t externalValue; + switch ((int)isMotorProtocolDshot()) { #ifdef USE_DSHOT - if (isMotorProtocolDshot()) { - if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) { - // Subtract 1 to compensate for imbalance introduced in convertExternalToMotor() - motorValue = DSHOT_MIN_THROTTLE + (DSHOT_3D_DEADBAND_LOW - motorValue) - 1; + case true: + if (feature(FEATURE_3D)) { + if (motorValue == DSHOT_DISARM_COMMAND || motorValue < DSHOT_MIN_THROTTLE) { + externalValue = PWM_RANGE_MID; + } else if (motorValue <= DSHOT_3D_DEADBAND_LOW) { + externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW, PWM_RANGE_MID - 1, PWM_RANGE_MIN); + } else { + externalValue = scaleRange(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX); + } + } else { + externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX); } - // Subtract 1 to compensate for imbalance introduced in convertExternalToMotor() - externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain(((motorValue - 1)/ EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE); - - if (feature(FEATURE_3D) && motorValue == DSHOT_DISARM_COMMAND) { - externalValue = EXTERNAL_CONVERSION_3D_MID_VALUE; - } - } + break; + case false: #endif + default: + externalValue = motorValue; + + break; + } return externalValue; } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index fd3ef0ceec..e4803a6548 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -105,6 +105,7 @@ PG_DECLARE(motorConfig_t, motorConfig); extern const mixer_t mixers[]; extern float motor[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; +extern float motorOutputHigh, motorOutputLow; struct rxConfig_s; uint8_t getMotorCount(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 064a2c95ac..a64d20dd9c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -69,7 +69,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, .pid_process_denom = PID_PROCESS_DENOM_DEFAULT ); -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 1); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 2); void resetPidProfile(pidProfile_t *pidProfile) { @@ -98,9 +98,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, - .levelSensitivity = 55, .setpointRelaxRatio = 100, - .dtermSetpointWeight = 60, + .dtermSetpointWeight = 0, .yawRateAccelLimit = 100, .rateAccelLimit = 0, .itermThrottleThreshold = 350, @@ -127,7 +126,7 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) } } -void pidSetTargetLooptime(uint32_t pidLooptime) +static void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; dT = (float)targetPidLooptime * 0.000001f; @@ -171,9 +170,17 @@ void pidInitFilters(const pidProfile_t *pidProfile) { BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 + if (targetPidLooptime == 0) { + // no looptime set, so set all the filters to null + dtermNotchFilterApplyFn = nullFilterApply; + dtermLpfApplyFn = nullFilterApply; + ptermYawFilterApplyFn = nullFilterApply; + return; + } + const uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed - float dTermNotchHz; + uint16_t dTermNotchHz; if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) { dTermNotchHz = pidProfile->dterm_notch_hz; } else { @@ -184,8 +191,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) } } - static biquadFilter_t biquadFilterNotch[2]; - if (dTermNotchHz) { + if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) { + static biquadFilter_t biquadFilterNotch[2]; dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { @@ -200,8 +207,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) { dtermLpfApplyFn = nullFilterApply; } else { - memset(&dtermFilterLpfUnion, 0, sizeof(dtermFilterLpfUnion)); - switch (pidProfile->dterm_filter_type) { default: dtermLpfApplyFn = nullFilterApply; @@ -295,6 +300,16 @@ void pidInit(const pidProfile_t *pidProfile) pidInitMixer(pidProfile); } + +void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex) +{ + if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1) + && dstPidProfileIndex != srcPidProfileIndex + ) { + memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t)); + } +} + // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling static float calcHorizonLevelStrength(void) { @@ -353,25 +368,27 @@ static float calcHorizonLevelStrength(void) static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination - float angle = pidProfile->levelSensitivity * getRcDeflection(axis); + // rcDeflection is in range [-1.0, 1.0] + float angle = pidProfile->levelAngleLimit * getRcDeflection(axis); #ifdef GPS angle += GPS_angle[axis]; #endif angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit); const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f); if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed + // ANGLE mode - control is angle based currentPidSetpoint = errorAngle * levelGain; } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel + // HORIZON mode - mix of ANGLE and ACRO modes + // mix in errorAngle to currentPidSetpoint to add a little auto-level feel const float horizonLevelStrength = calcHorizonLevelStrength(); currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength); } return currentPidSetpoint; } -static float accelerationLimit(int axis, float currentPidSetpoint) { +static float accelerationLimit(int axis, float currentPidSetpoint) +{ static float previousSetpoint[3]; const float currentVelocity = currentPidSetpoint- previousSetpoint[axis]; @@ -488,7 +505,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered); float dynC = 0; - if ( (pidProfile->setpointRelaxRatio < 100) && (!flightModeFlags) ) { + if ( (pidProfile->dtermSetpointWeight > 0) && (!flightModeFlags) ) { dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); } const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e487e9e8cb..c48a5eecff 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -85,7 +85,6 @@ typedef struct pidProfile_s { uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. uint8_t levelAngleLimit; // Max angle in degrees in level mode - uint8_t levelSensitivity; // Angle mode sensitivity reflected in degrees assuming user using full stick uint8_t horizon_tilt_effect; // inclination factor for Horizon mode uint8_t horizon_tilt_expert_mode; // OFF or ON @@ -129,10 +128,10 @@ extern uint8_t PIDweight[3]; void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); -void pidSetTargetLooptime(uint32_t pidLooptime); void pidSetItermAccelerator(float newItermAccelerator); void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); void pidInit(const pidProfile_t *pidProfile); +void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex); #endif diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index d8d5c3cf2c..4400f4dc48 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -539,7 +539,7 @@ static bool afatfs_fileIsBusy(afatfsFilePtr_t file) * * Note that this is the same as the number of clusters in an AFATFS supercluster. */ -static uint32_t afatfs_fatEntriesPerSector() +static uint32_t afatfs_fatEntriesPerSector(void) { return afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT32 ? AFATFS_FAT32_FAT_ENTRIES_PER_SECTOR : AFATFS_FAT16_FAT_ENTRIES_PER_SECTOR; } @@ -548,7 +548,7 @@ static uint32_t afatfs_fatEntriesPerSector() * Size of a FAT cluster in bytes */ ONLY_EXPOSE_FOR_TESTING -uint32_t afatfs_clusterSize() +uint32_t afatfs_clusterSize(void) { return afatfs.sectorsPerCluster * AFATFS_SECTOR_SIZE; } @@ -806,7 +806,7 @@ static int afatfs_allocateCacheSector(uint32_t sectorIndex) /** * Attempt to flush dirty cache pages out to the sdcard, returning true if all flushable data has been flushed. */ -bool afatfs_flush() +bool afatfs_flush(void) { if (afatfs.cacheDirtyEntries > 0) { // Flush the oldest flushable sector @@ -836,7 +836,7 @@ bool afatfs_flush() /** * Returns true if either the freefile or the regular cluster pool has been exhausted during a previous write operation. */ -bool afatfs_isFull() +bool afatfs_isFull(void) { return afatfs.filesystemFull; } @@ -1618,7 +1618,7 @@ static afatfsOperationStatus_e afatfs_appendRegularFreeCluster(afatfsFilePtr_t f * Size of a AFATFS supercluster in bytes */ ONLY_EXPOSE_FOR_TESTING -uint32_t afatfs_superClusterSize() +uint32_t afatfs_superClusterSize(void) { return afatfs_fatEntriesPerSector() * afatfs_clusterSize(); } @@ -2364,7 +2364,7 @@ static afatfsOperationStatus_e afatfs_allocateDirectoryEntry(afatfsFilePtr_t dir * Return a pointer to a free entry in the open files table (a file whose type is "NONE"). You should initialize * the file afterwards with afatfs_initFileHandle(). */ -static afatfsFilePtr_t afatfs_allocateFileHandle() +static afatfsFilePtr_t afatfs_allocateFileHandle(void) { for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) { if (afatfs.openFiles[i].type == AFATFS_FILE_TYPE_NONE) { @@ -3211,7 +3211,7 @@ static void afatfs_fileOperationContinue(afatfsFile_t *file) /** * Check files for pending operations and execute them. */ -static void afatfs_fileOperationsPoll() +static void afatfs_fileOperationsPoll(void) { afatfs_fileOperationContinue(&afatfs.currentDirectory); @@ -3229,7 +3229,7 @@ static void afatfs_fileOperationsPoll() /** * Return the available size of the freefile (used for files in contiguous append mode) */ -uint32_t afatfs_getContiguousFreeSpace() +uint32_t afatfs_getContiguousFreeSpace(void) { return afatfs.freeFile.logicalSize; } @@ -3238,7 +3238,7 @@ uint32_t afatfs_getContiguousFreeSpace() * Call to set up the initial state for finding the largest block of free space on the device whose corresponding FAT * sectors are themselves entirely free space (so the free space has dedicated FAT sectors of its own). */ -static void afatfs_findLargestContiguousFreeBlockBegin() +static void afatfs_findLargestContiguousFreeBlockBegin(void) { // The first FAT sector has two reserved entries, so it isn't eligible for this search. Start at the next FAT sector. afatfs.initState.freeSpaceSearch.candidateStart = afatfs_fatEntriesPerSector(); @@ -3256,7 +3256,7 @@ static void afatfs_findLargestContiguousFreeBlockBegin() * AFATFS_OPERATION_SUCCESS - When the search has finished and afatfs.initState.freeSpaceSearch has been updated with the details of the best gap. * AFATFS_OPERATION_FAILURE - When a read error occured */ -static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue() +static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue(void) { afatfsFreeSpaceSearch_t *opState = &afatfs.initState.freeSpaceSearch; uint32_t fatEntriesPerSector = afatfs_fatEntriesPerSector(); @@ -3361,7 +3361,7 @@ static void afatfs_introspecLogCreated(afatfsFile_t *file) #endif -static void afatfs_initContinue() +static void afatfs_initContinue(void) { #ifdef AFATFS_USE_FREEFILE afatfsOperationStatus_e status; @@ -3491,7 +3491,7 @@ static void afatfs_initContinue() * Check to see if there are any pending operations on the filesystem and perform a little work (without waiting on the * sdcard). You must call this periodically. */ -void afatfs_poll() +void afatfs_poll(void) { // Only attempt to continue FS operations if the card is present & ready, otherwise we would just be wasting time if (sdcard_poll()) { @@ -3554,17 +3554,17 @@ void afatfs_sdcardProfilerCallback(sdcardBlockOperation_e operation, uint32_t bl #endif -afatfsFilesystemState_e afatfs_getFilesystemState() +afatfsFilesystemState_e afatfs_getFilesystemState(void) { return afatfs.filesystemState; } -afatfsError_e afatfs_getLastError() +afatfsError_e afatfs_getLastError(void) { return afatfs.lastError; } -void afatfs_init() +void afatfs_init(void) { afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_INITIALIZATION; afatfs.initPhase = AFATFS_INITIALIZATION_READ_MBR; @@ -3646,7 +3646,7 @@ bool afatfs_destroy(bool dirty) /** * Get a pessimistic estimate of the amount of buffer space that we have available to write to immediately. */ -uint32_t afatfs_getFreeBufferSpace() +uint32_t afatfs_getFreeBufferSpace(void) { uint32_t result = 0; for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { diff --git a/src/main/io/asyncfatfs/asyncfatfs.h b/src/main/io/asyncfatfs/asyncfatfs.h index faeb7ea510..40cd7f27d7 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.h +++ b/src/main/io/asyncfatfs/asyncfatfs.h @@ -58,7 +58,7 @@ typedef enum { } afatfsSeek_e; typedef void (*afatfsFileCallback_t)(afatfsFilePtr_t file); -typedef void (*afatfsCallback_t)(); +typedef void (*afatfsCallback_t)(void); bool afatfs_fopen(const char *filename, const char *mode, afatfsFileCallback_t complete); bool afatfs_ftruncate(afatfsFilePtr_t file, afatfsFileCallback_t callback); @@ -79,14 +79,14 @@ void afatfs_findFirst(afatfsFilePtr_t directory, afatfsFinder_t *finder); afatfsOperationStatus_e afatfs_findNext(afatfsFilePtr_t directory, afatfsFinder_t *finder, fatDirectoryEntry_t **dirEntry); void afatfs_findLast(afatfsFilePtr_t directory); -bool afatfs_flush(); -void afatfs_init(); +bool afatfs_flush(void); +void afatfs_init(void); bool afatfs_destroy(bool dirty); -void afatfs_poll(); +void afatfs_poll(void); -uint32_t afatfs_getFreeBufferSpace(); -uint32_t afatfs_getContiguousFreeSpace(); -bool afatfs_isFull(); +uint32_t afatfs_getFreeBufferSpace(void); +uint32_t afatfs_getContiguousFreeSpace(void); +bool afatfs_isFull(void); -afatfsFilesystemState_e afatfs_getFilesystemState(); -afatfsError_e afatfs_getLastError(); +afatfsFilesystemState_e afatfs_getFilesystemState(void); +afatfsError_e afatfs_getLastError(void); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index cb2a9430bb..9c03769420 100755 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -84,6 +84,11 @@ PG_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig, #define BEEPER_COMMAND_STOP 0xFF #ifdef BEEPER +PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); +PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, + .dshotBeaconTone = 0 +); + /* Beeper Sound Sequences: (Square wave generation) * Sequence must end with 0xFF or 0xFE. 0xFE repeats the sequence from * start when 0xFF stops the sound when it's completed. @@ -224,7 +229,7 @@ void beeper(beeperMode_e mode) if ( mode == BEEPER_SILENCE || ( (getBeeperOffMask() & (1 << (BEEPER_USB - 1))) - && (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && (getBatteryCellCount() == 0)) + && getBatteryState() == BATTERY_NOT_PRESENT ) ) { beeperSilence(); @@ -363,8 +368,13 @@ void beeperUpdate(timeUs_t currentTimeUs) } #ifdef USE_DSHOT - if (!areMotorsRunning() && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) { - pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3); + if (!areMotorsRunning() && beeperConfig()->dshotBeaconTone && (beeperConfig()->dshotBeaconTone <= DSHOT_CMD_BEACON5) && currentBeeperEntry->mode == BEEPER_RX_SET) { + pwmDisableMotors(); + delay(1); + + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone); + + pwmEnableMotors(); } #endif diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index b4d8de0211..997539b80e 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -51,7 +51,7 @@ typedef enum { typedef struct beeperConfig_s { uint32_t beeper_off_flags; uint32_t preferred_beeper_off_flags; - bool dshotForward; + uint8_t dshotBeaconTone; } beeperConfig_t; #ifdef BEEPER diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index c12b240eca..4d34a025a2 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -181,7 +181,7 @@ static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) } #if 0 -static void fillScreenWithCharacters() +static void fillScreenWithCharacters(void) { for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) { for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) { @@ -241,7 +241,7 @@ static void updateFailsafeStatus(void) i2c_OLED_send_char(bus, failsafeIndicator); } -static void showTitle() +static void showTitle(void) { i2c_OLED_set_line(bus, 0); i2c_OLED_send_string(bus, pageState.page->title); @@ -346,7 +346,7 @@ static void showProfilePage(void) #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2) #ifdef GPS -static void showGpsPage() +static void showGpsPage(void) { if (!feature(FEATURE_GPS)) { pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c index 5e346d1b82..3d0b882bef 100644 --- a/src/main/io/flashfs.c +++ b/src/main/io/flashfs.c @@ -52,12 +52,12 @@ static uint8_t bufferHead = 0, bufferTail = 0; // The position of the buffer's tail in the overall flash address space: static uint32_t tailAddress = 0; -static void flashfsClearBuffer() +static void flashfsClearBuffer(void) { bufferTail = bufferHead = 0; } -static bool flashfsBufferIsEmpty() +static bool flashfsBufferIsEmpty(void) { return bufferTail == bufferHead; } @@ -67,7 +67,7 @@ static void flashfsSetTailAddress(uint32_t address) tailAddress = address; } -void flashfsEraseCompletely() +void flashfsEraseCompletely(void) { m25p16_eraseCompletely(); @@ -106,17 +106,17 @@ void flashfsEraseRange(uint32_t start, uint32_t end) /** * Return true if the flash is not currently occupied with an operation. */ -bool flashfsIsReady() +bool flashfsIsReady(void) { return m25p16_isReady(); } -uint32_t flashfsGetSize() +uint32_t flashfsGetSize(void) { return m25p16_getGeometry()->totalSize; } -static uint32_t flashfsTransmitBufferUsed() +static uint32_t flashfsTransmitBufferUsed(void) { if (bufferHead >= bufferTail) return bufferHead - bufferTail; @@ -127,7 +127,7 @@ static uint32_t flashfsTransmitBufferUsed() /** * Get the size of the largest single write that flashfs could ever accept without blocking or data loss. */ -uint32_t flashfsGetWriteBufferSize() +uint32_t flashfsGetWriteBufferSize(void) { return FLASHFS_WRITE_BUFFER_USABLE; } @@ -135,12 +135,12 @@ uint32_t flashfsGetWriteBufferSize() /** * Get the number of bytes that can currently be written to flashfs without any blocking or data loss. */ -uint32_t flashfsGetWriteBufferFreeSpace() +uint32_t flashfsGetWriteBufferFreeSpace(void) { return flashfsGetWriteBufferSize() - flashfsTransmitBufferUsed(); } -const flashGeometry_t* flashfsGetGeometry() +const flashGeometry_t* flashfsGetGeometry(void) { return m25p16_getGeometry(); } @@ -270,7 +270,7 @@ static void flashfsGetDirtyDataBuffers(uint8_t const *buffers[], uint32_t buffer /** * Get the current offset of the file pointer within the volume. */ -uint32_t flashfsGetOffset() +uint32_t flashfsGetOffset(void) { uint8_t const * buffers[2]; uint32_t bufferSizes[2]; @@ -305,7 +305,7 @@ static void flashfsAdvanceTailInBuffer(uint32_t delta) * Returns true if all data in the buffer has been flushed to the device, or false if * there is still data to be written (call flush again later). */ -bool flashfsFlushAsync() +bool flashfsFlushAsync(void) { if (flashfsBufferIsEmpty()) { return true; // Nothing to flush @@ -328,7 +328,7 @@ bool flashfsFlushAsync() * The flash will still be busy some time after this sync completes, but space will * be freed up to accept more writes in the write buffer. */ -void flashfsFlushSync() +void flashfsFlushSync(void) { if (flashfsBufferIsEmpty()) { return; // Nothing to flush @@ -484,7 +484,7 @@ int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len) /** * Find the offset of the start of the free space on the device (or the size of the device if it is full). */ -int flashfsIdentifyStartOfFreeSpace() +int flashfsIdentifyStartOfFreeSpace(void) { /* Find the start of the free space on the device by examining the beginning of blocks with a binary search, * looking for ones that appear to be erased. We can achieve this with good accuracy because an erased block @@ -553,14 +553,15 @@ int flashfsIdentifyStartOfFreeSpace() /** * Returns true if the file pointer is at the end of the device. */ -bool flashfsIsEOF() { +bool flashfsIsEOF(void) +{ return tailAddress >= flashfsGetSize(); } /** * Call after initializing the flash chip in order to set up the filesystem. */ -void flashfsInit() +void flashfsInit(void) { // If we have a flash chip present at all if (flashfsGetSize() > 0) { diff --git a/src/main/io/flashfs.h b/src/main/io/flashfs.h index 90f1ca9e57..216c8c22e0 100644 --- a/src/main/io/flashfs.h +++ b/src/main/io/flashfs.h @@ -23,16 +23,16 @@ // Automatically trigger a flush when this much data is in the buffer #define FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN 64 -void flashfsEraseCompletely(); +void flashfsEraseCompletely(void); void flashfsEraseRange(uint32_t start, uint32_t end); -uint32_t flashfsGetSize(); -uint32_t flashfsGetOffset(); -uint32_t flashfsGetWriteBufferFreeSpace(); -uint32_t flashfsGetWriteBufferSize(); -int flashfsIdentifyStartOfFreeSpace(); +uint32_t flashfsGetSize(void); +uint32_t flashfsGetOffset(void); +uint32_t flashfsGetWriteBufferFreeSpace(void); +uint32_t flashfsGetWriteBufferSize(void); +int flashfsIdentifyStartOfFreeSpace(void); struct flashGeometry_s; -const struct flashGeometry_s* flashfsGetGeometry(); +const struct flashGeometry_s* flashfsGetGeometry(void); void flashfsSeekAbs(uint32_t offset); void flashfsSeekRel(int32_t offset); @@ -42,10 +42,10 @@ void flashfsWrite(const uint8_t *data, unsigned int len, bool sync); int flashfsReadAbs(uint32_t offset, uint8_t *data, unsigned int len); -bool flashfsFlushAsync(); -void flashfsFlushSync(); +bool flashfsFlushAsync(void); +void flashfsFlushSync(void); -void flashfsInit(); +void flashfsInit(void); -bool flashfsIsReady(); -bool flashfsIsEOF(); +bool flashfsIsReady(void); +bool flashfsIsEOF(void); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index eb5d1840f6..607cd5dd9f 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -441,22 +441,35 @@ static const struct { {0, LED_MODE_ORIENTATION}, }; -static void applyLedFixedLayers() +static void applyLedFixedLayers(void) { for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex]; hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); - hsvColor_t nextColor = *getSC(LED_SCOLOR_BACKGROUND); //next color above the one selected, or color 0 if your are at the maximum - hsvColor_t previousColor = *getSC(LED_SCOLOR_BACKGROUND); //Previous color to the one selected, modulo color count int fn = ledGetFunction(ledConfig); - int hOffset = HSV_HUE_MAX; + int hOffset = HSV_HUE_MAX + 1; switch (fn) { case LED_FUNCTION_COLOR: color = ledStripConfig()->colors[ledGetColor(ledConfig)]; - nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; - previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; + + hsvColor_t nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; + hsvColor_t previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor + int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2; + if (auxInput < centerPWM) { + color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h); + color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s); + color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v); + } else { + color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h); + color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s); + color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v); + } + } + break; case LED_FUNCTION_FLIGHT_MODE: @@ -489,22 +502,10 @@ static void applyLedFixedLayers() break; } - if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor - { - int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2; - if (auxInput < centerPWM) - { - color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h); - color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s); - color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v); - } - else - { - color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h); - color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s); - color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v); - } - } + if ((fn != LED_FUNCTION_COLOR) && ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { + hOffset += scaleRange(auxInput, PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1); + } + color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); setLedHsv(ledIndex, &color); } @@ -1145,7 +1146,7 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) return true; } -void ledStripInit() +void ledStripInit(void) { colors = ledStripConfigMutable()->colors; modeColors = ledStripConfig()->modeColors; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a73d1ae7c3..8be67ee857 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -163,7 +163,7 @@ static escSensorData_t *escData; /** * Gets the correct altitude symbol for the current unit system */ -static char osdGetMetersToSelectedUnitSymbol() +static char osdGetMetersToSelectedUnitSymbol(void) { switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: @@ -572,7 +572,7 @@ static void osdDrawSingleElement(uint8_t item) } case OSD_POWER: - tfp_sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000); + tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000); break; case OSD_PIDRATE_PROFILE: diff --git a/src/main/io/rcsplit.c b/src/main/io/rcsplit.c index d567b2afd6..aada0181ab 100644 --- a/src/main/io/rcsplit.c +++ b/src/main/io/rcsplit.c @@ -83,7 +83,7 @@ static void sendCtrlCommand(rcsplit_ctrl_argument_e argument) serialWriteBuf(rcSplitSerialPort, uart_buffer, 5); } -static void rcSplitProcessMode() +static void rcSplitProcessMode(void) { // if the device not ready, do not handle any mode change event if (RCSPLIT_STATE_IS_READY != cameraState) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 1cc30d1d42..388318ecad 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -119,15 +119,6 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) serialConfig->portConfigs[0].functionMask = FUNCTION_MSP; -#if defined(USE_VCP) && defined(USE_MSP_UART) - if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) { - serialPortConfig_t * uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1); - if (uart1Config) { - uart1Config->functionMask = FUNCTION_MSP; - } - } -#endif - #ifdef SERIALRX_UART serialPortConfig_t *serialRxUartConfig = serialFindPortConfiguration(SERIALRX_UART); if (serialRxUartConfig) { @@ -142,6 +133,15 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) } #endif +#if defined(USE_VCP) && defined(USE_MSP_UART) + if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) { + serialPortConfig_t * uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1); + if (uart1Config) { + uart1Config->functionMask = FUNCTION_MSP; + } + } +#endif + serialConfig->reboot_character = 'R'; serialConfig->serial_update_rate_hz = 100; } diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 622f656e70..17de43bb16 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -16,9 +16,13 @@ */ -// Get target build configuration +#include +#include + #include "platform.h" +#if defined(VTX_CONTROL) && defined(VTX_COMMON) + #include "common/maths.h" #include "config/config_eeprom.h" @@ -38,9 +42,13 @@ #include "io/vtx_control.h" -#if defined(VTX_CONTROL) && defined(VTX_COMMON) -PG_REGISTER(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); + +PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, +// .vtxChannelActivationConditions = { 0 }, + .halfDuplex = true +); static uint8_t locked = 0; @@ -171,7 +179,6 @@ void vtxCyclePower(const uint8_t powerStep) void handleVTXControlButton(void) { #if defined(VTX_RTC6705) && defined(BUTTON_A_PIN) - bool buttonHeld; bool buttonWasPressed = false; uint32_t start = millis(); uint32_t ledToggleAt = start; @@ -179,6 +186,7 @@ void handleVTXControlButton(void) uint8_t flashesDone = 0; uint8_t actionCounter = 0; + bool buttonHeld; while ((buttonHeld = buttonAPressed())) { uint32_t end = millis(); @@ -227,21 +235,20 @@ void handleVTXControlButton(void) LED1_OFF; switch (actionCounter) { - case 4: - vtxCycleBandOrChannel(0, +1); - break; - case 3: - vtxCycleBandOrChannel(+1, 0); - break; - case 2: - vtxCyclePower(+1); - break; - case 1: - saveConfigAndNotify(); - break; + case 4: + vtxCycleBandOrChannel(0, +1); + break; + case 3: + vtxCycleBandOrChannel(+1, 0); + break; + case 2: + vtxCyclePower(+1); + break; + case 1: + saveConfigAndNotify(); + break; } #endif } #endif - diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 3715b60e00..66c202d446 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -17,6 +17,12 @@ #pragma once +#include +#include + +#include "platform.h" + +#include "config/parameter_group.h" #include "fc/rc_modes.h" #define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10 @@ -30,6 +36,7 @@ typedef struct vtxChannelActivationCondition_s { typedef struct vtxConfig_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; + uint8_t halfDuplex; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); diff --git a/src/main/io/vtx_rtc6705.c b/src/main/io/vtx_rtc6705.c index f79e164e13..1b387fff4f 100644 --- a/src/main/io/vtx_rtc6705.c +++ b/src/main/io/vtx_rtc6705.c @@ -25,7 +25,9 @@ #include "platform.h" #if defined(VTX_RTC6705) && defined(VTX_CONTROL) + #include "build/build_config.h" +#include "build/debug.h" #include "cms/cms.h" #include "cms/cms_types.h" @@ -46,8 +48,6 @@ #include "io/vtx_rtc6705.h" #include "io/vtx_string.h" -#include "build/debug.h" - bool canUpdateVTX(void); PG_REGISTER_WITH_RESET_TEMPLATE(vtxRTC6705Config_t, vtxRTC6705Config, PG_VTX_RTC6705_CONFIG, 0); diff --git a/src/main/io/vtx_rtc6705.h b/src/main/io/vtx_rtc6705.h index 9a48a8aa03..002800fea8 100644 --- a/src/main/io/vtx_rtc6705.h +++ b/src/main/io/vtx_rtc6705.h @@ -20,6 +20,8 @@ #include #include +#include "platform.h" + #include "config/parameter_group.h" typedef struct vtxRTC6705Config_s { @@ -41,4 +43,4 @@ PG_DECLARE(vtxRTC6705Config_t, vtxRTC6705Config); extern const char * const rtc6705PowerNames[RTC6705_POWER_COUNT]; void vtxRTC6705Configure(void); -bool vtxRTC6705Init(); +bool vtxRTC6705Init(void); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index ea293f39c4..61b1fef5fa 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -25,10 +25,10 @@ #include "platform.h" #if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) + #include "build/build_config.h" #include "build/debug.h" - #include "cms/cms.h" #include "cms/cms_types.h" #include "cms/cms_menu_vtx_smartaudio.h" @@ -49,6 +49,7 @@ #include "flight/pid.h" #include "io/serial.h" +#include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_string.h" @@ -193,8 +194,9 @@ static void saPrintSettings(void) int saDacToPowerIndex(int dac) { for (int idx = 3 ; idx >= 0 ; idx--) { - if (saPowerTable[idx].valueV1 <= dac) + if (saPowerTable[idx].valueV1 <= dac) { return idx; + } } return 0; } @@ -213,9 +215,10 @@ static int sa_baudstep = 50; static void saAutobaud(void) { - if (saStat.pktsent < 10) + if (saStat.pktsent < 10) { // Not enough samples collected return; + } #if 0 dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", @@ -276,8 +279,9 @@ static void saProcessResponse(uint8_t *buf, int len) switch (resp) { case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings - if (len < 7) + if (len < 7) { break; + } saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2; saDevice.channel = buf[2]; @@ -300,10 +304,11 @@ static void saProcessResponse(uint8_t *buf, int len) break; case SA_CMD_SET_FREQ: // Set Frequency - if (len < 5) + if (len < 5) { break; + } - uint16_t freq = (buf[2] << 8)|buf[3]; + const uint16_t freq = (buf[2] << 8)|buf[3]; if (freq & SA_FREQ_GETPIT) { saDevice.orfreq = freq & SA_FREQ_MASK; @@ -327,8 +332,9 @@ static void saProcessResponse(uint8_t *buf, int len) } // Debug - if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) + if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) { saPrintSettings(); + } saDevicePrev = saDevice; #ifdef VTX_COMMON @@ -426,12 +432,11 @@ static void saReceiveFramer(uint8_t c) static void saSendFrame(uint8_t *buf, int len) { - int i; - serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit - for (i = 0 ; i < len ; i++) + for (int i = 0 ; i < len ; i++) { serialWrite(smartAudioSerialPort, buf[i]); + } serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this @@ -466,10 +471,9 @@ static void saResendCmd(void) static void saSendCmd(uint8_t *buf, int len) { - int i; - - for (i = 0 ; i < len ; i++) + for (int i = 0 ; i < len ; i++) { sa_osbuf[i] = buf[i]; + } sa_oslen = len; sa_outstanding = (buf[2] >> 1); @@ -490,7 +494,7 @@ static uint8_t sa_qhead = 0; static uint8_t sa_qtail = 0; #ifdef DPRINTF_SMARTAUDIO -static int saQueueLength() +static int saQueueLength(void) { if (sa_qhead >= sa_qtail) { return sa_qhead - sa_qtail; @@ -500,20 +504,21 @@ static int saQueueLength() } #endif -static bool saQueueEmpty() +static bool saQueueEmpty(void) { return sa_qhead == sa_qtail; } -static bool saQueueFull() +static bool saQueueFull(void) { return ((sa_qhead + 1) % SA_QSIZE) == sa_qtail; } static void saQueueCmd(uint8_t *buf, int len) { - if (saQueueFull()) + if (saQueueFull()) { return; + } sa_queue[sa_qhead].buf = buf; sa_queue[sa_qhead].len = len; @@ -522,8 +527,9 @@ static void saQueueCmd(uint8_t *buf, int len) static void saSendQueue(void) { - if (saQueueEmpty()) + if (saQueueEmpty()) { return; + } saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len); sa_qtail = (sa_qtail + 1) % SA_QSIZE; @@ -600,15 +606,16 @@ void saSetPowerByIndex(uint8_t index) return; } - if (index > 3) + if (index > 3) { return; + } buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); } -bool vtxSmartAudioInit() +bool vtxSmartAudioInit(void) { #ifdef SMARTAUDIO_DPRINTF // Setup debugSerialPort @@ -622,7 +629,14 @@ bool vtxSmartAudioInit() serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + portOptions_e portOptions = 0; +#if defined(VTX_COMMON) + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR); +#else + portOptions = SERIAL_BIDIR; +#endif + + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, portOptions); } if (!smartAudioSerialPort) { @@ -638,8 +652,9 @@ void vtxSAProcess(uint32_t now) { static char initPhase = 0; - if (smartAudioSerialPort == NULL) + if (smartAudioSerialPort == NULL) { return; + } while (serialRxBytesWaiting(smartAudioSerialPort) > 0) { uint8_t c = serialRead(smartAudioSerialPort); @@ -665,8 +680,7 @@ void vtxSAProcess(uint32_t now) break; } - if ((sa_outstanding != SA_CMD_NONE) - && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { + if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { // Last command timed out // dprintf(("process: resending 0x%x\r\n", sa_outstanding)); // XXX Todo: Resend termination and possible offline transition @@ -686,8 +700,9 @@ void vtxSAProcess(uint32_t now) // Testing VTX_COMMON API { static uint32_t lastMonitorUs = 0; - if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) + if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) { return; + } static uint8_t monBand; static uint8_t monChan; @@ -717,8 +732,9 @@ bool vtxSAIsReady(void) void vtxSASetBandAndChannel(uint8_t band, uint8_t channel) { - if (band && channel) + if (band && channel) { saSetBandAndChannel(band - 1, channel - 1); + } } void vtxSASetPowerByIndex(uint8_t index) @@ -733,8 +749,9 @@ void vtxSASetPowerByIndex(uint8_t index) void vtxSASetPitMode(uint8_t onoff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) + if (!(vtxSAIsReady() && (saDevice.version == 2))) { return; + } if (onoff) { // SmartAudio can not turn pit mode on by software. @@ -743,11 +760,13 @@ void vtxSASetPitMode(uint8_t onoff) uint8_t newmode = SA_MODE_CLR_PITMODE; - if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) + if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { newmode |= SA_MODE_SET_IN_RANGE_PITMODE; + } - if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) { newmode |= SA_MODE_SET_OUT_RANGE_PITMODE; + } saSetMode(newmode); @@ -756,8 +775,9 @@ void vtxSASetPitMode(uint8_t onoff) bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) { - if (!vtxSAIsReady()) + if (!vtxSAIsReady()) { return false; + } *pBand = (saDevice.channel / 8) + 1; *pChannel = (saDevice.channel % 8) + 1; @@ -766,8 +786,9 @@ bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) bool vtxSAGetPowerIndex(uint8_t *pIndex) { - if (!vtxSAIsReady()) + if (!vtxSAIsReady()) { return false; + } *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1; return true; @@ -775,8 +796,9 @@ bool vtxSAGetPowerIndex(uint8_t *pIndex) bool vtxSAGetPitMode(uint8_t *pOnOff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) + if (!(vtxSAIsReady() && (saDevice.version == 2))) { return false; + } *pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0; return true; diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 0fd31dad80..a4a0cf820d 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -1,5 +1,10 @@ #pragma once +#include +#include + +#include "platform.h" + // opmode flags, GET side #define SA_MODE_GET_FREQ_BY_FREQ 1 #define SA_MODE_GET_PITMODE 2 @@ -60,7 +65,7 @@ void saSetBandAndChannel(uint8_t band, uint8_t channel); void saSetMode(int mode); void saSetPowerByIndex(uint8_t index); void saSetFreq(uint16_t freq); -bool vtxSmartAudioInit(); +bool vtxSmartAudioInit(void); #ifdef SMARTAUDIO_DPRINTF #ifdef OMNIBUSF4 diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index 71f0e4047a..c341a1699f 100644 --- a/src/main/io/vtx_string.c +++ b/src/main/io/vtx_string.c @@ -19,10 +19,9 @@ #include #include -#include -#include #include "platform.h" + #include "build/debug.h" #if defined(VTX_COMMON) @@ -53,13 +52,10 @@ const char * const vtx58ChannelNames[] = { bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel) { - int8_t band; - uint8_t channel; - // Use reverse lookup order so that 5880Mhz // get Raceband 7 instead of Fatshark 8. - for (band = 4 ; band >= 0 ; band--) { - for (channel = 0 ; channel < 8 ; channel++) { + for (int band = 4 ; band >= 0 ; band--) { + for (int channel = 0 ; channel < 8 ; channel++) { if (vtx58frequencyTable[band][channel] == freq) { *pBand = band + 1; *pChannel = channel + 1; diff --git a/src/main/io/vtx_string.h b/src/main/io/vtx_string.h index a3074a8966..736ac8daa3 100644 --- a/src/main/io/vtx_string.h +++ b/src/main/io/vtx_string.h @@ -2,6 +2,8 @@ #include +#include "platform.h" + #if defined(VTX_COMMON) extern const uint16_t vtx58frequencyTable[5][8]; diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 281eb003d1..09e9fb4e9e 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -35,11 +35,10 @@ #include "drivers/vtx_common.h" #include "io/serial.h" +#include "io/vtx_control.h" #include "io/vtx_string.h" #include "io/vtx_tramp.h" -#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) - #if defined(CMS) || defined(VTX_COMMON) const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = { 25, 100, 200, 400, 600 @@ -108,16 +107,18 @@ static uint8_t trampChecksum(uint8_t *trampBuf) { uint8_t cksum = 0; - for (int i = 1 ; i < 14 ; i++) + for (int i = 1 ; i < 14 ; i++) { cksum += trampBuf[i]; + } return cksum; } void trampCmdU16(uint8_t cmd, uint16_t param) { - if (!trampSerialPort) + if (!trampSerialPort) { return; + } memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer)); trampReqBuffer[0] = 15; @@ -131,8 +132,9 @@ void trampCmdU16(uint8_t cmd, uint16_t param) void trampSetFreq(uint16_t freq) { trampConfFreq = freq; - if (trampConfFreq != trampCurFreq) + if (trampConfFreq != trampCurFreq) { trampFreqRetries = TRAMP_MAX_RETRIES; + } } void trampSendFreq(uint16_t freq) @@ -148,8 +150,9 @@ void trampSetBandAndChannel(uint8_t band, uint8_t channel) void trampSetRFPower(uint16_t level) { trampConfPower = level; - if (trampConfPower != trampPower) + if (trampConfPower != trampPower) { trampPowerRetries = TRAMP_MAX_RETRIES; + } } void trampSendRFPower(uint16_t level) @@ -158,10 +161,11 @@ void trampSendRFPower(uint16_t level) } // return false if error -bool trampCommitChanges() +bool trampCommitChanges(void) { - if (trampStatus != TRAMP_STATUS_ONLINE) + if (trampStatus != TRAMP_STATUS_ONLINE) { return false; + } trampStatus = TRAMP_STATUS_SET_FREQ_PW; return true; @@ -175,12 +179,12 @@ void trampSetPitMode(uint8_t onoff) // returns completed response code char trampHandleResponse(void) { - uint8_t respCode = trampRespBuffer[1]; + const uint8_t respCode = trampRespBuffer[1]; switch (respCode) { case 'r': { - uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); if (min_freq != 0) { trampRFFreqMin = min_freq; trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); @@ -194,7 +198,7 @@ char trampHandleResponse(void) case 'v': { - uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); if (freq != 0) { trampCurFreq = freq; trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); @@ -213,7 +217,7 @@ char trampHandleResponse(void) case 's': { - uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); + const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); if (temp != 0) { trampTemperature = temp; return 's'; @@ -234,7 +238,7 @@ typedef enum { static trampReceiveState_e trampReceiveState = S_WAIT_LEN; static int trampReceivePos = 0; -static void trampResetReceiver() +static void trampResetReceiver(void) { trampReceiveState = S_WAIT_LEN; trampReceivePos = 0; @@ -242,10 +246,11 @@ static void trampResetReceiver() static bool trampIsValidResponseCode(uint8_t code) { - if (code == 'r' || code == 'v' || code == 's') + if (code == 'r' || code == 'v' || code == 's') { return true; - else + } else { return false; + } } // returns completed response code or 0 @@ -253,11 +258,12 @@ static char trampReceive(uint32_t currentTimeUs) { UNUSED(currentTimeUs); - if (!trampSerialPort) + if (!trampSerialPort) { return 0; + } while (serialRxBytesWaiting(trampSerialPort)) { - uint8_t c = serialRead(trampSerialPort); + const uint8_t c = serialRead(trampSerialPort); trampRespBuffer[trampReceivePos++] = c; switch (trampReceiveState) { @@ -291,6 +297,7 @@ static char trampReceive(uint32_t currentTimeUs) default: trampResetReceiver(); + break; } } @@ -327,10 +334,11 @@ void vtxTrampProcess(uint32_t currentTimeUs) static uint16_t debugPowReqCounter = 0; #endif - if (trampStatus == TRAMP_STATUS_BAD_DEVICE) + if (trampStatus == TRAMP_STATUS_BAD_DEVICE) { return; + } - char replyCode = trampReceive(currentTimeUs); + const char replyCode = trampReceive(currentTimeUs); #ifdef TRAMP_DEBUG debug[0] = trampStatus; @@ -338,13 +346,15 @@ void vtxTrampProcess(uint32_t currentTimeUs) switch (replyCode) { case 'r': - if (trampStatus <= TRAMP_STATUS_OFFLINE) + if (trampStatus <= TRAMP_STATUS_OFFLINE) { trampStatus = TRAMP_STATUS_ONLINE; + } break; case 'v': - if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) + if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) { trampStatus = TRAMP_STATUS_SET_FREQ_PW; + } break; } @@ -354,14 +364,15 @@ void vtxTrampProcess(uint32_t currentTimeUs) case TRAMP_STATUS_ONLINE: if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s - if (trampStatus == TRAMP_STATUS_OFFLINE) + if (trampStatus == TRAMP_STATUS_OFFLINE) { trampQueryR(); - else { + } else { static unsigned int cnt = 0; - if (((cnt++) & 1) == 0) + if (((cnt++) & 1) == 0) { trampQueryV(); - else + } else { trampQueryS(); + } } lastQueryTimeUs = currentTimeUs; @@ -378,8 +389,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) debugFreqReqCounter++; #endif done = false; - } - else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) { + } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) { trampSendRFPower(trampConfPower); trampPowerRetries--; #ifdef TRAMP_DEBUG @@ -393,8 +403,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) // delay next status query by 300ms lastQueryTimeUs = currentTimeUs + 300 * 1000; - } - else { + } else { // everything has been done, let's return to original state trampStatus = TRAMP_STATUS_ONLINE; // reset configuration value in case it failed (no more retries) @@ -465,8 +474,9 @@ void vtxTrampSetPitMode(uint8_t onoff) bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady()) { return false; + } *pBand = trampBand; *pChannel = trampChannel; @@ -475,8 +485,9 @@ bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) bool vtxTrampGetPowerIndex(uint8_t *pIndex) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady()) { return false; + } if (trampConfiguredPower > 0) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { @@ -492,8 +503,9 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex) bool vtxTrampGetPitMode(uint8_t *pOnOff) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady()) { return false; + } *pOnOff = trampPitMode; return true; @@ -518,7 +530,14 @@ bool vtxTrampInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); + portOptions_e portOptions = 0; +#if defined(VTX_COMMON) + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); +#else + portOptions = SERIAL_BIDIR; +#endif + + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, portOptions); } if (!trampSerialPort) { diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 094f2b0d75..edb420cd1b 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -1,5 +1,7 @@ #pragma once +#include + #define VTX_TRAMP_POWER_COUNT 5 extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT]; extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1]; @@ -12,8 +14,8 @@ extern uint32_t trampCurFreq; extern uint16_t trampConfiguredPower; // Configured transmitting power extern int16_t trampTemperature; -bool vtxTrampInit(); -bool trampCommitChanges(); +bool vtxTrampInit(void); +bool trampCommitChanges(void); void trampSetPitMode(uint8_t onoff); void trampSetBandAndChannel(uint8_t band, uint8_t channel); void trampSetRFPower(uint16_t level); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 4deda31f51..e74709330d 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -223,6 +223,8 @@ // External OSD displayport mode messages #define MSP_DISPLAYPORT 182 +#define MSP_COPY_PROFILE 183 + #define MSP_BEEPER_CONFIG 184 #define MSP_SET_BEEPER_CONFIG 185 @@ -269,6 +271,7 @@ #define MSP_MOTOR_CONFIG 131 //out message Motor configuration (min/max throttle, etc) #define MSP_GPS_CONFIG 132 //out message GPS configuration #define MSP_COMPASS_CONFIG 133 //out message Compass configuration +#define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM) #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -316,3 +319,4 @@ #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration #define MSP_SET_4WAY_IF 245 //in message Sets 4way interface +#define MSP_SET_RTC 246 //in message Sets the RTC clock diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 7e05410cfc..9500296b40 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -301,7 +301,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direct } -uint32_t mspSerialTxBytesFree() +uint32_t mspSerialTxBytesFree(void) { uint32_t ret = UINT32_MAX; diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 7ef216f3f4..1e82f0a8d7 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -33,30 +33,36 @@ #include "drivers/serial.h" #include "drivers/serial_uart.h" +#include "drivers/system.h" #include "drivers/time.h" #include "io/serial.h" +#include "msp/msp.h" + #include "rx/rx.h" #include "rx/crsf.h" +#include "telemetry/crsf.h" +#include "telemetry/msp_shared.h" + #define CRSF_TIME_NEEDED_PER_FRAME_US 1000 #define CRSF_TIME_BETWEEN_FRAMES_US 4000 // a frame is sent by the transmitter every 4 milliseconds #define CRSF_DIGITAL_CHANNEL_MIN 172 #define CRSF_DIGITAL_CHANNEL_MAX 1811 +#define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type) + STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED crsfFrame_t crsfFrame; - STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; static serialPort_t *serialPort; -static uint32_t crsfFrameStartAt = 0; +static uint32_t crsfFrameStartAtUs = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; - /* * CRSF protocol * @@ -69,13 +75,13 @@ static uint8_t telemetryBufLen = 0; * 1 Stop bit * Big endian * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte - * Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes - * A 36 byte frame can be transmitted in 771 microseconds. + * Max frame size is 64 bytes + * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds. * - * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1000 microseconds + * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds * * Every frame has the structure: - * < Type> < CRC> + * * * Device address: (uint8_t) * Frame length: length in bytes including Type (uint8_t) @@ -111,20 +117,20 @@ typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t; STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c) { static uint8_t crsfFramePosition = 0; - const uint32_t now = micros(); + const uint32_t currentTimeUs = micros(); #ifdef DEBUG_CRSF_PACKETS - debug[2] = now - crsfFrameStartAt; + debug[2] = currentTimeUs - crsfFrameStartAtUs; #endif - if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) { + if (currentTimeUs > crsfFrameStartAtUs + CRSF_TIME_NEEDED_PER_FRAME_US) { // We've received a character after max time needed to complete a frame, // so this must be the start of a new frame. crsfFramePosition = 0; } if (crsfFramePosition == 0) { - crsfFrameStartAt = now; + crsfFrameStartAtUs = currentTimeUs; } // assume frame is 5 bytes long until we have received the frame length // full frame length includes the length of the address and framelength fields @@ -133,6 +139,9 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c) if (crsfFramePosition < fullFrameLength) { crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true; + if (crsfFrameDone) { + crsfFramePosition = 0; + } } } @@ -158,7 +167,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) } crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; // unpack the RC channels - const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; + const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; crsfChannelData[0] = rcChannels->chan0; crsfChannelData[1] = rcChannels->chan1; crsfChannelData[2] = rcChannels->chan2; @@ -176,6 +185,22 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) crsfChannelData[14] = rcChannels->chan14; crsfChannelData[15] = rcChannels->chan15; return RX_FRAME_COMPLETE; + } else { + if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) { + // TODO: CRC CHECK + crsfScheduleDeviceInfoResponse(); + return RX_FRAME_COMPLETE; +#if defined(USE_MSP_OVER_TELEMETRY) + } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) { + // TODO: CRC CHECK + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; + if (handleMspFrame(frameStart, frameEnd)) { + crsfScheduleMspResponse(); + } + return RX_FRAME_COMPLETE; +#endif + } } } return RX_FRAME_PENDING; @@ -208,10 +233,10 @@ void crsfRxSendTelemetryData(void) if (telemetryBufLen > 0) { // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame) // and that there is time to send the telemetry frame before the next RX frame arrives - if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) { - const uint32_t timeSinceStartOfFrame = micros() - crsfFrameStartAt; - if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) || - (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { + if (rxConfig()->halfDuplex) { + const uint32_t timeSinceStartOfFrameUs = micros() - crsfFrameStartAtUs; + if ((timeSinceStartOfFrameUs < CRSF_TIME_NEEDED_PER_FRAME_US) || + (timeSinceStartOfFrameUs > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { return; } } diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 7b93bf8df8..8342a29917 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -22,6 +22,8 @@ #define CRSF_PORT_MODE MODE_RXTX #define CRSF_MAX_CHANNEL 16 +#define CRSF_MSP_RX_BUF_SIZE 128 +#define CRSF_MSP_TX_BUF_SIZE 128 typedef enum { CRSF_FRAMETYPE_GPS = 0x02, @@ -29,8 +31,13 @@ typedef enum { CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, - CRSF_FRAMETYPE_FLIGHT_MODE = 0x21 -} crsfFrameTypes_e; + CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, + CRSF_FRAMETYPE_DEVICE_PING = 0x28, + CRSF_FRAMETYPE_DEVICE_INFO = 0x29, + CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command + CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary + CRSF_FRAMETYPE_MSP_WRITE = 0x7C // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit) +} crsfFrameType_e; enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, @@ -38,11 +45,14 @@ enum { CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, + CRSF_FRAME_TX_MSP_PAYLOAD_SIZE = 58, + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE = 8, CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field - CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined + CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined + CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 4 // length of Extended Dest/Origin, TYPE and CRC fields combined }; enum { @@ -51,7 +61,7 @@ enum { CRSF_ADDRESS_RESERVED1 = 0x8A, CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, - CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8, + CRSF_ADDRESS_BETAFLIGHT = 0xC8, CRSF_ADDRESS_RESERVED2 = 0xCA, CRSF_ADDRESS_RACE_TAG = 0xCC, CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, @@ -59,7 +69,7 @@ enum { CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE }; -#define CRSF_PAYLOAD_SIZE_MAX 32 // !!TODO needs checking +#define CRSF_PAYLOAD_SIZE_MAX 60 #define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4) typedef struct crsfFrameDef_s { @@ -74,7 +84,6 @@ typedef union crsfFrame_u { crsfFrameDef_t frame; } crsfFrame_t; - void crsfRxWriteTelemetryData(const void *data, int len); void crsfRxSendTelemetryData(void); diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c index f93c33a503..4308440d61 100644 --- a/src/main/rx/frsky_d.c +++ b/src/main/rx/frsky_d.c @@ -193,25 +193,25 @@ static void telemetry_build_frame(uint8_t *packet) #endif #if defined(USE_FRSKY_RX_PA_LNA) -static void RX_enable() +static void RX_enable(void) { IOLo(txEnPin); IOHi(rxEnPin); } -static void TX_enable() +static void TX_enable(void) { IOLo(rxEnPin); IOHi(txEnPin); } #endif -void frSkyDBind() +void frSkyDBind(void) { bindRequested = true; } -static void initialize() +static void initialize(void) { cc2500Reset(); cc2500WriteReg(CC2500_02_IOCFG0, 0x01); diff --git a/src/main/rx/frsky_d.h b/src/main/rx/frsky_d.h index d7c661b130..c565f8943f 100644 --- a/src/main/rx/frsky_d.h +++ b/src/main/rx/frsky_d.h @@ -36,4 +36,4 @@ struct rxRuntimeConfig_s; void frSkyDInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); void frSkyDSetRcData(uint16_t *rcData, const uint8_t *payload); rx_spi_received_e frSkyDDataReceived(uint8_t *payload); -void frSkyDBind(); +void frSkyDBind(void); diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 4f13e61fbe..f83070792e 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -281,7 +281,7 @@ void jetiExBusDecodeChannelFrame(uint8_t *exBusFrame) } -void jetiExBusFrameReset() +void jetiExBusFrameReset(void) { jetiExBusFramePosition = 0; jetiExBusFrameLength = EXBUS_MAX_CHANNEL_FRAME_SIZE; @@ -376,7 +376,7 @@ static void jetiExBusDataReceive(uint16_t c) // Check if it is time to read a frame from the data... -static uint8_t jetiExBusFrameStatus() +static uint8_t jetiExBusFrameStatus(void) { if (jetiExBusFrameState != EXBUS_STATE_RECEIVED) return RX_FRAME_PENDING; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d82d8d55e2..bd33f8674a 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -45,6 +45,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -112,7 +113,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) RESET_CONFIG_2(accelerometerConfig_t, instance, .acc_lpf_hz = 10, .acc_align = ALIGN_DEFAULT, - .acc_hardware = ACC_DEFAULT + .acc_hardware = ACC_DEFAULT, + .acc_high_fsr = false, ); resetRollAndPitchTrims(&instance->accelerometerTrims); resetFlightDynamicsTrims(&instance->accZero); @@ -243,6 +245,17 @@ retry: } break; } +#endif + ; // fallthrough + case ACC_ICM20649: +#ifdef USE_ACC_SPI_ICM20649 + if (icm20649SpiAccDetect(dev)) { + accHardware = ACC_ICM20649; +#ifdef ACC_ICM20649_ALIGN + dev->accAlign = ACC_ICM20649_ALIGN; +#endif + break; + } #endif ; // fallthrough case ACC_ICM20689: @@ -305,6 +318,7 @@ bool accInit(uint32_t gyroSamplingInverval) acc.dev.bus = *gyroSensorBus(); acc.dev.mpuConfiguration = *gyroMpuConfiguration(); acc.dev.mpuDetectionResult = *gyroMpuDetectionResult(); + acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr; if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { return false; } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b211abbb65..57ee0fc82d 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -36,6 +36,7 @@ typedef enum { ACC_ICM20601, ACC_ICM20602, ACC_ICM20608G, + ACC_ICM20649, ACC_ICM20689, ACC_BMI160, ACC_FAKE @@ -65,6 +66,7 @@ typedef struct accelerometerConfig_s { uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz sensor_align_e acc_align; // acc alignment uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device + bool acc_high_fsr; flightDynamicsTrims_t accZero; rollAndPitchTrims_t accelerometerTrims; } accelerometerConfig_t; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 97254497a6..ed479b0baf 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -87,7 +87,7 @@ static batteryState_e consumptionState; #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_NONE #endif -PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 2); PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig, // voltage @@ -154,6 +154,7 @@ static void updateBatteryBeeperAlert(void) break; case BATTERY_OK: case BATTERY_NOT_PRESENT: + case BATTERY_INIT: break; } } @@ -168,7 +169,7 @@ void batteryUpdatePresence(void) if ( - voltageState == BATTERY_NOT_PRESENT + (voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBat && isVoltageStable ) { @@ -265,7 +266,7 @@ void batteryUpdateStates(timeUs_t currentTimeUs) batteryState = MAX(voltageState, consumptionState); } -lowVoltageCutoff_t *getLowVoltageCutoff(void) +const lowVoltageCutoff_t *getLowVoltageCutoff(void) { return &lowVoltageCutoff; } @@ -275,7 +276,7 @@ batteryState_e getBatteryState(void) return batteryState; } -const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT"}; +const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT", "INIT"}; const char * getBatteryStateString(void) { @@ -287,13 +288,13 @@ void batteryInit(void) // // presence // - batteryState = BATTERY_NOT_PRESENT; + batteryState = BATTERY_INIT; batteryCellCount = 0; // // voltage // - voltageState = BATTERY_NOT_PRESENT; + voltageState = BATTERY_INIT; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; lowVoltageCutoff.enabled = false; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 770c6b3759..27744df495 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -59,7 +59,8 @@ typedef enum { BATTERY_OK = 0, BATTERY_WARNING, BATTERY_CRITICAL, - BATTERY_NOT_PRESENT + BATTERY_NOT_PRESENT, + BATTERY_INIT } batteryState_e; void batteryInit(void); @@ -87,4 +88,4 @@ int32_t getMAhDrawn(void); void batteryUpdateCurrentMeter(timeUs_t currentTimeUs); -lowVoltageCutoff_t *getLowVoltageCutoff(void); +const lowVoltageCutoff_t *getLowVoltageCutoff(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4261c2307a..ccbf38abd1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -19,6 +19,7 @@ #include #include #include +#include #include "platform.h" @@ -44,6 +45,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -68,6 +70,10 @@ #include "hardware_revision.h" #endif +#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689))) +#define USE_GYRO_SLEW_LIMITER +#endif + gyro_t gyro; static uint8_t gyroDebugMode; @@ -110,13 +116,14 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); #ifdef STM32F10X #define GYRO_SYNC_DENOM_DEFAULT 8 -#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689) #define GYRO_SYNC_DENOM_DEFAULT 1 #else #define GYRO_SYNC_DENOM_DEFAULT 4 #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_align = ALIGN_DEFAULT, @@ -125,6 +132,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = GYRO_LPF_256HZ, .gyro_soft_lpf_type = FILTER_PT1, .gyro_soft_lpf_hz = 90, + .gyro_high_fsr = false, .gyro_use_32khz = false, .gyro_to_use = 0, .gyro_soft_notch_hz_1 = 400, @@ -258,6 +266,17 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) } #endif +#ifdef USE_GYRO_SPI_ICM20649 + case GYRO_ICM20649: + if (icm20649SpiGyroDetect(dev)) { + gyroHardware = GYRO_ICM20649; +#ifdef GYRO_ICM20649_ALIGN + dev->gyroAlign = GYRO_ICM20649_ALIGN; +#endif + break; + } +#endif + #ifdef USE_GYRO_SPI_ICM20689 case GYRO_ICM20689: if (icm20689SpiGyroDetect(dev)) { @@ -302,7 +321,8 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) static bool gyroInitSensor(gyroSensor_t *gyroSensor) { -#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) +#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) #if defined(MPU_INT_EXTI) gyroSensor->gyroDev.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI); @@ -321,6 +341,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) mpuDetect(&gyroSensor->gyroDev); mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect #endif + gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); if (gyroHardware == GYRO_NONE) { @@ -349,6 +370,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align; } + gyroInitSensorFilters(gyroSensor); #ifdef USE_GYRO_DATA_ANALYSE gyroDataAnalyseInit(gyro.targetLooptime); @@ -362,7 +384,9 @@ bool gyroInit(void) case DEBUG_FFT: case DEBUG_GYRO_NOTCH: case DEBUG_GYRO: + case DEBUG_GYRO_RAW: gyroDebugMode = debugMode; + break; default: // debugMode is not gyro-related gyroDebugMode = DEBUG_NONE; @@ -419,6 +443,15 @@ static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notch return notchHz; } +#if defined(USE_GYRO_SLEW_LIMITER) +void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) { + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0; + } +} +#endif + static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) { gyroSensor->notchFilter1ApplyFn = nullFilterApply; @@ -466,6 +499,9 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { +#if defined(USE_GYRO_SLEW_LIMITER) + gyroInitSlewLimiter(gyroSensor); +#endif gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); @@ -556,7 +592,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) { schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics - if (!firstArmingCalibrationWasStarted || !isArmingDisabled()) { + if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) { beeper(BEEPER_GYRO_CALIBRATED); } } @@ -564,6 +600,19 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t } +#if defined(USE_GYRO_SLEW_LIMITER) +int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) +{ + int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis]; + if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) { + newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; + } else { + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; + } + return newRawGyro; +} +#endif + void gyroUpdateSensor(gyroSensor_t *gyroSensor) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { @@ -572,10 +621,15 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyroSensor->gyroDev.dataReady = false; if (isGyroSensorCalibrationComplete(gyroSensor)) { - // move gyro data into 32-bit variables to avoid overflows in calculations - gyroSensor->gyroDev.gyroADC[X] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[X] - (int32_t)gyroSensor->gyroDev.gyroZero[X]; - gyroSensor->gyroDev.gyroADC[Y] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Y] - (int32_t)gyroSensor->gyroDev.gyroZero[Y]; - gyroSensor->gyroDev.gyroADC[Z] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Z] - (int32_t)gyroSensor->gyroDev.gyroZero[Z]; + // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations + + gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X]; + gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; +#if defined(USE_GYRO_SLEW_LIMITER) + gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z]; +#else + gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; +#endif alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); } else { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index fd055ceddb..801fe6bc9d 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -35,6 +35,7 @@ typedef enum { GYRO_ICM20601, GYRO_ICM20602, GYRO_ICM20608G, + GYRO_ICM20649, GYRO_ICM20689, GYRO_BMI160, GYRO_FAKE @@ -54,6 +55,7 @@ typedef struct gyroConfig_s { uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_soft_lpf_type; uint8_t gyro_soft_lpf_hz; + bool gyro_high_fsr; bool gyro_use_32khz; uint8_t gyro_to_use; uint16_t gyro_soft_notch_hz_1; diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index b680a131b3..dd741bc973 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -86,14 +86,14 @@ static biquadFilter_t fftFreqFilter[3]; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window static float hanningWindow[FFT_WINDOW_SIZE]; -void initHanning() +void initHanning(void) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) { hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); } } -void initGyroData() +void initGyroData(void) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) { diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index ac4290738a..a4a64cd431 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -31,4 +31,4 @@ const gyroFftData_t *gyroFftData(int axis); struct gyroDev_s; void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn); void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn); -bool isDynamicFilterActive(); +bool isDynamicFilterActive(void); diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 90d1be9599..f06309a309 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -49,12 +49,16 @@ void targetConfiguration(void) motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; } - pidProfilesMutable(0)->pid[PID_ROLL].P = 90; - pidProfilesMutable(0)->pid[PID_ROLL].I = 44; - pidProfilesMutable(0)->pid[PID_ROLL].D = 60; - pidProfilesMutable(0)->pid[PID_PITCH].P = 90; - pidProfilesMutable(0)->pid[PID_PITCH].I = 44; - pidProfilesMutable(0)->pid[PID_PITCH].D = 60; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 90; + pidProfile->pid[PID_ROLL].I = 44; + pidProfile->pid[PID_ROLL].D = 60; + pidProfile->pid[PID_PITCH].P = 90; + pidProfile->pid[PID_PITCH].I = 44; + pidProfile->pid[PID_PITCH].D = 60; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 8431e680f4..a6341c2eec 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -107,12 +107,16 @@ void targetConfiguration(void) pidConfigMutable()->pid_process_denom = 1; } - pidProfilesMutable(0)->pid[PID_ROLL].P = 90; - pidProfilesMutable(0)->pid[PID_ROLL].I = 44; - pidProfilesMutable(0)->pid[PID_ROLL].D = 60; - pidProfilesMutable(0)->pid[PID_PITCH].P = 90; - pidProfilesMutable(0)->pid[PID_PITCH].I = 44; - pidProfilesMutable(0)->pid[PID_PITCH].D = 60; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 90; + pidProfile->pid[PID_ROLL].I = 44; + pidProfile->pid[PID_ROLL].D = 60; + pidProfile->pid[PID_PITCH].P = 90; + pidProfile->pid[PID_PITCH].I = 44; + pidProfile->pid[PID_PITCH].D = 60; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index cabae587c0..b13c52b3d5 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -71,14 +71,18 @@ void targetConfiguration(void) featureSet(FEATURE_TELEMETRY); } - pidProfilesMutable(0)->pid[PID_ROLL].P = 53; - pidProfilesMutable(0)->pid[PID_ROLL].I = 45; - pidProfilesMutable(0)->pid[PID_ROLL].D = 52; - pidProfilesMutable(0)->pid[PID_PITCH].P = 53; - pidProfilesMutable(0)->pid[PID_PITCH].I = 45; - pidProfilesMutable(0)->pid[PID_PITCH].D = 52; - pidProfilesMutable(0)->pid[PID_YAW].P = 64; - pidProfilesMutable(0)->pid[PID_YAW].D = 18; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 53; + pidProfile->pid[PID_ROLL].I = 45; + pidProfile->pid[PID_ROLL].D = 52; + pidProfile->pid[PID_PITCH].P = 53; + pidProfile->pid[PID_PITCH].I = 45; + pidProfile->pid[PID_PITCH].D = 52; + pidProfile->pid[PID_YAW].P = 64; + pidProfile->pid[PID_YAW].D = 18; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index 4fa4e376a7..a5feb582d9 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -70,14 +70,18 @@ void targetConfiguration(void) featureSet(FEATURE_TELEMETRY); } - pidProfilesMutable(0)->pid[FD_ROLL].P = 53; - pidProfilesMutable(0)->pid[FD_ROLL].I = 45; - pidProfilesMutable(0)->pid[FD_ROLL].D = 52; - pidProfilesMutable(0)->pid[FD_PITCH].P = 53; - pidProfilesMutable(0)->pid[FD_PITCH].I = 45; - pidProfilesMutable(0)->pid[FD_PITCH].D = 52; - pidProfilesMutable(0)->pid[FD_YAW].P = 64; - pidProfilesMutable(0)->pid[FD_YAW].D = 18; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[FD_ROLL].P = 53; + pidProfile->pid[FD_ROLL].I = 45; + pidProfile->pid[FD_ROLL].D = 52; + pidProfile->pid[FD_PITCH].P = 53; + pidProfile->pid[FD_PITCH].I = 45; + pidProfile->pid[FD_PITCH].D = 52; + pidProfile->pid[FD_YAW].P = 64; + pidProfile->pid[FD_YAW].D = 18; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index 905b61c268..b26d223626 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -39,16 +39,22 @@ #ifdef TARGET_CONFIG +#include "fc/rc_modes.h" #include "common/axis.h" #include "config/feature.h" #include "drivers/pwm_esc_detect.h" +#include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/rc_controls.h" #include "flight/mixer.h" #include "flight/pid.h" +#include "io/beeper.h" #include "io/serial.h" #include "rx/rx.h" #include "sensors/barometer.h" #include "sensors/boardalignment.h" #include "sensors/compass.h" +#include "sensors/gyro.h" #ifdef BRUSHED_MOTORS_PWM_RATE #undef BRUSHED_MOTORS_PWM_RATE @@ -61,25 +67,83 @@ void targetConfiguration(void) { if (hardwareMotorType == MOTOR_BRUSHED) { motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - motorConfigMutable()->minthrottle = 1080; - motorConfigMutable()->maxthrottle = 2000; - pidConfigMutable()->pid_process_denom = 1; + motorConfigMutable()->minthrottle = 1080; + motorConfigMutable()->maxthrottle = 2000; } - rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; + /* Default to Spektrum */ + rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048; + rxConfigMutable()->spektrum_sat_bind = 5; + rxConfigMutable()->spektrum_sat_bind_autoreset = 1; + parseRcChannels("TAER1234", rxConfigMutable()); #if defined(ALIENWHOOPF4) rxConfigMutable()->sbus_inversion = 0; // TODO: what to do about F4 inversion? #else rxConfigMutable()->sbus_inversion = 1; // invert on F7 #endif -/* Breadboard-specific settings for development purposes only - */ + beeperOffSet((BEEPER_BAT_CRIT_LOW | BEEPER_BAT_LOW | BEEPER_RX_SET) ^ BEEPER_GYRO_CALIBRATED); + + /* Breadboard-specific settings for development purposes only + */ #if defined(BREADBOARD) boardAlignmentMutable()->pitchDegrees = 90; // vertical breakout board barometerConfigMutable()->baro_hardware = BARO_DEFAULT; // still testing not on V1 or V2 pcb +#else + barometerConfigMutable()->baro_hardware = BARO_NONE; #endif compassConfigMutable()->mag_hardware = MAG_DEFAULT; + + /* F4 (especially overclocked) and F7 ALIENWHOOP perform splendidly with 32kHz gyro enabled */ + gyroConfigMutable()->gyro_use_32khz = 1; + gyroConfigMutable()->gyro_sync_denom = 2; // 16kHz gyro + pidConfigMutable()->pid_process_denom = 1; // 16kHz PID + + featureSet((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM); + + /* AlienWhoop PIDs based on Ole Gravy Leg (aka Matt Williamson's) PIDs + */ + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */ + pidProfile->pid[PID_PITCH].P = 75; + pidProfile->pid[PID_PITCH].I = 36; + pidProfile->pid[PID_PITCH].D = 25; + pidProfile->pid[PID_ROLL].P = 75; + pidProfile->pid[PID_ROLL].I = 36; + pidProfile->pid[PID_ROLL].D = 25; + pidProfile->pid[PID_YAW].P = 70; + pidProfile->pid[PID_YAW].I = 36; + + pidProfile->pid[PID_LEVEL].P = 30; + pidProfile->pid[PID_LEVEL].D = 30; + + /* Setpoints */ + pidProfile->dtermSetpointWeight = 100; + pidProfile->setpointRelaxRatio = 100; // default to snappy for racers + + /* Throttle PID Attenuation (TPA) */ + pidProfile->itermThrottleThreshold = 400; + } + + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + /* RC Rates */ + controlRateConfig->rcRate8 = 100; + controlRateConfig->rcYawRate8 = 100; + controlRateConfig->rcExpo8 = 0; + + /* Super Expo Rates */ + controlRateConfig->rates[FD_ROLL] = 80; + controlRateConfig->rates[FD_PITCH] = 80; + controlRateConfig->rates[FD_YAW] = 85; + + /* Throttle PID Attenuation (TPA) */ + controlRateConfig->dynThrPID = 0; // tpa_rate off + controlRateConfig->tpa_breakpoint = 1600; + } } #endif diff --git a/src/main/target/ALIENWHOOP/target.h b/src/main/target/ALIENWHOOP/target.h index eebdb91099..0777d9fab2 100644 --- a/src/main/target/ALIENWHOOP/target.h +++ b/src/main/target/ALIENWHOOP/target.h @@ -104,6 +104,15 @@ //#define SPI5_MOSI_PIN #endif +/* OSD MAX7456E */ +#define OSD + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + /* Motion Processing Unit (MPU) - Invensense 6-axis MPU-6500 or 9-axis MPU-9250 */ // Interrupt @@ -132,11 +141,13 @@ /* Optional Digital Pressure Sensor (barometer) - Bosch BMP280 * TODO: not implemented on V1 or V2 pcb */ +#if defined(BREADBOARD) #define BARO #define USE_BARO_BMP280 #define USE_BARO_SPI_BMP280 #define BMP280_SPI_INSTANCE SPI3 #define BMP280_CS_PIN SPI3_NSS_PIN +#endif /* Serial ports etc. */ @@ -185,15 +196,17 @@ /* Defaults - What do we want out of the box? */ #if defined(BREADBOARD) -#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP | FEATURE_LED_STRIP ) +#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP | FEATURE_LED_STRIP | FEATURE_OSD ) #else -#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP) // FEATURE_TELEMETRY changes bind pin from rx to tx +#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP ) // TODO FEATURE_OSD for V3 board ... FEATURE_TELEMETRY changes bind pin from rx to tx #endif +/* OSD currently dependent upon CMS, SMARTAUDIO, TRAMP #undef VTX_COMMON #undef VTX_CONTROL #undef VTX_SMARTAUDIO #undef VTX_TRAMP +*/ /* OLED Support */ @@ -206,7 +219,7 @@ #define I2C1_SCL PB6 #define I2C1_SDA PB7 #else -#undef CMS +//#undef CMS // TODO: OSD depends upon CMS #undef USE_I2C #endif diff --git a/src/main/target/ALIENWHOOP/target.mk b/src/main/target/ALIENWHOOP/target.mk index 62476322ab..f1f9cd8a7c 100644 --- a/src/main/target/ALIENWHOOP/target.mk +++ b/src/main/target/ALIENWHOOP/target.mk @@ -21,4 +21,6 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ - drivers/compass/compass_ak8963.c + drivers/compass/compass_ak8963.c \ + drivers/max7456.c \ + io/osd.c diff --git a/src/main/target/BEEBRAIN_V2/config.c b/src/main/target/BEEBRAIN_V2/config.c index 9c79a8e9f9..ada367f353 100755 --- a/src/main/target/BEEBRAIN_V2/config.c +++ b/src/main/target/BEEBRAIN_V2/config.c @@ -63,12 +63,16 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; #endif - pidProfilesMutable(0)->pid[PID_ROLL].P = 90; - pidProfilesMutable(0)->pid[PID_ROLL].I = 44; - pidProfilesMutable(0)->pid[PID_ROLL].D = 60; - pidProfilesMutable(0)->pid[PID_PITCH].P = 90; - pidProfilesMutable(0)->pid[PID_PITCH].I = 44; - pidProfilesMutable(0)->pid[PID_PITCH].D = 60; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 90; + pidProfile->pid[PID_ROLL].I = 44; + pidProfile->pid[PID_ROLL].D = 60; + pidProfile->pid[PID_PITCH].P = 90; + pidProfile->pid[PID_PITCH].I = 44; + pidProfile->pid[PID_PITCH].D = 60; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 6501394508..bb8389396b 100644 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -117,7 +117,7 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 -#define CURRENT_METER_SCALE_DEFAULT 200 +#define CURRENT_METER_SCALE_DEFAULT 220 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/BETAFLIGHTF4/target.c b/src/main/target/BETAFLIGHTF4/target.c index 7d30bf3bc7..b9d1c60759 100755 --- a/src/main/target/BETAFLIGHTF4/target.c +++ b/src/main/target/BETAFLIGHTF4/target.c @@ -25,15 +25,22 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM - DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM + // Motors DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S1_OUT D1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S2_OUT D1_ST2 DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S3_OUT D1_ST6 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S4_OUT D1_ST1 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT - DEF_TIM(TIM3, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT D1_ST2 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // UART6 TX Softserial Smartport - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // UART6 RX - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED + + // LED strip + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // D1_ST0 + + // UART Backdoors + DEF_TIM(TIM1, CH2, PA9, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // TX1 Bidir + DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // RX1 Bidir + DEF_TIM(TIM5, CH3, PA2, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // TX2 TX only + DEF_TIM(TIM9, CH2, PA3, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // RX2 RX only + DEF_TIM(TIM2, CH3, PB10, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // TX3 Bidir + DEF_TIM(TIM2, CH4, PB11, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // RX3 Bidir }; diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index ef76e13421..054246b87a 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -24,11 +24,11 @@ #define LED0_PIN PB5 -// Leave beeper here but with none as io - so disabled unless mapped. #define BEEPER PB4 +#define BEEPER_INVERTED -// PC0 used as inverter select GPIO -#define INVERTER_PIN_UART6 PC13 +// PC13 used as inverter select GPIO for UART2 +#define INVERTER_PIN_UART2 PC13 #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -90,10 +90,9 @@ #define UART6_TX_PIN PC6 #define USE_SOFTSERIAL1 -#define SOFTSERIAL1_RX_PIN PC7 -#define SOFTSERIAL1_TX_PIN PC6 +#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 6 //VCP, USART1, USART2, USART3, USART6 +#define SERIAL_PORT_COUNT 7 //VCP, USART1, USART2, USART3, USART6, SOFTSERIAL1, SOFTSERIAL2 #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_PIN PB8 // (Hardware=0, PPM) @@ -114,6 +113,11 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL NONE // PB10, UART3_TX +#define I2C2_SDA NONE // PB11, UART3_RX +#define I2C_DEVICE (I2CDEV_2) #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 @@ -123,11 +127,11 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS -#define SERIALRX_UART SERIAL_PORT_USART2 -#define SBUS_TELEMETRY_UART SERIAL_PORT_SOFTSERIAL1 +#define SERIALRX_UART SERIAL_PORT_USART6 +#define SBUS_TELEMETRY_UART SERIAL_PORT_USART2 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_SOFTSERIAL) +#define DEFAULT_FEATURES ( FEATURE_TELEMETRY | FEATURE_OSD ) #define SPEKTRUM_BIND // USART3, @@ -135,12 +139,11 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) +#define TARGET_IO_PORTB (0xffff & ~(BIT(2))) +#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14))) +#define TARGET_IO_PORTD BIT(2) +#define USABLE_TIMER_CHANNEL_COUNT 12 -#define USABLE_TIMER_CHANNEL_COUNT 10 - -#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9) | TIM_N(10) ) diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index 86ea41bebc..386bd33df4 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -26,17 +26,19 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM4, CH1, PB6, TIM_USE_PWM, 0), // S1_IN/ +// DEF_TIM(TIM4, CH1, PB6, TIM_USE_PWM, 0), // S1_IN/ <--Moved to allow Parallel PWM with proper ESC DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0), // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0), // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0), // S4_IN - Current DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM, 0), // S5_IN - Vbattery - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM | TIM_USE_PPM, 0), // S6_IN - PPM IN + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM| TIM_USE_PPM, 0), // S6_IN - PPM IN + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM, 1), // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1), // S1_OUT DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1), // S2_OUT DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 1), // S3_OUT DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // S4_OUT - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 1), // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0), // S1_IN/ + // DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 1), // S5_OUT <--Moved to allow Parallel PWM with proper ESC DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1) // S6_OUT }; diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index d1b1844fa6..697dbbda95 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -54,8 +54,13 @@ void targetConfiguration(void) //rcControlsConfigMutable()->yaw_deadband = 10; compassConfigMutable()->mag_hardware = 1; - controlRateProfilesMutable(0)->dynThrPID = 45; - controlRateProfilesMutable(0)->tpa_breakpoint = 1700; + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + controlRateConfig->dynThrPID = 45; + controlRateConfig->tpa_breakpoint = 1700; + } + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; } #endif diff --git a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c index 2b18e740cd..cf87516a87 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c +++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c @@ -71,7 +71,7 @@ uint8_t interruptCounter = 0; #define DELAY_SENDING_BYTE 40 void bstProcessInCommand(void); -void I2C_EV_IRQHandler() +void I2C_EV_IRQHandler(void) { if (I2C_GetITStatus(BSTx, I2C_IT_ADDR)) { CRC8 = 0; @@ -154,17 +154,17 @@ void I2C_EV_IRQHandler() } } -void I2C1_EV_IRQHandler() +void I2C1_EV_IRQHandler(void) { I2C_EV_IRQHandler(); } -void I2C2_EV_IRQHandler() +void I2C2_EV_IRQHandler(void) { I2C_EV_IRQHandler(); } -uint32_t bstTimeoutUserCallback() +uint32_t bstTimeoutUserCallback(void) { bstErrorCount++; diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 5c2db86ac7..c23c4d010d 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -32,6 +32,10 @@ #define BEEPER_INVERTED /*---------------------------------*/ +/*----------CAMERA CONTROL---------*/ +#define CAMERA_CONTROL_PIN PB7 +/*---------------------------------*/ + /*------------SENSORS--------------*/ // MPU interrupt #define USE_EXTI diff --git a/src/main/target/FF_PIKOBLX/config.c b/src/main/target/FF_PIKOBLX/config.c index a7a94f5e09..a1e71d6fa8 100644 --- a/src/main/target/FF_PIKOBLX/config.c +++ b/src/main/target/FF_PIKOBLX/config.c @@ -64,22 +64,30 @@ void targetConfiguration(void) #endif parseRcChannels("TAER1234", rxConfigMutable()); - pidProfilesMutable(0)->pid[PID_ROLL].P = 80; - pidProfilesMutable(0)->pid[PID_ROLL].I = 37; - pidProfilesMutable(0)->pid[PID_ROLL].D = 35; - pidProfilesMutable(0)->pid[PID_PITCH].P = 100; - pidProfilesMutable(0)->pid[PID_PITCH].I = 37; - pidProfilesMutable(0)->pid[PID_PITCH].D = 35; - pidProfilesMutable(0)->pid[PID_YAW].P = 180; - pidProfilesMutable(0)->pid[PID_YAW].D = 45; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - controlRateProfilesMutable(0)->rcRate8 = 100; - controlRateProfilesMutable(0)->rcYawRate8 = 100; - controlRateProfilesMutable(0)->rcExpo8 = 15; - controlRateProfilesMutable(0)->rcYawExpo8 = 15; - controlRateProfilesMutable(0)->rates[PID_ROLL] = 80; - controlRateProfilesMutable(0)->rates[PID_PITCH] = 80; - controlRateProfilesMutable(0)->rates[PID_YAW] = 80; + pidProfile->pid[PID_ROLL].P = 80; + pidProfile->pid[PID_ROLL].I = 37; + pidProfile->pid[PID_ROLL].D = 35; + pidProfile->pid[PID_PITCH].P = 100; + pidProfile->pid[PID_PITCH].I = 37; + pidProfile->pid[PID_PITCH].D = 35; + pidProfile->pid[PID_YAW].P = 180; + pidProfile->pid[PID_YAW].D = 45; + } + + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + controlRateConfig->rcRate8 = 100; + controlRateConfig->rcYawRate8 = 100; + controlRateConfig->rcExpo8 = 15; + controlRateConfig->rcYawExpo8 = 15; + controlRateConfig->rates[PID_ROLL] = 80; + controlRateConfig->rates[PID_PITCH] = 80; + controlRateConfig->rates[PID_YAW] = 80; + } } } #endif diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index 3e2213b3cf..e198c367fa 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -79,16 +79,28 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI1 +#define MAX7456_SPI_CS_PIN PA3 +#define MAX7456_NRST_PIN PC14 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #define USE_ADC #define ADC_INSTANCE ADC1 #define CURRENT_METER_ADC_PIN PA0 #define RSSI_ADC_PIN PA1 #define VBAT_ADC_PIN PA2 +#define CURRENT_METER_SCALE_DEFAULT 275 + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/IMPULSERCF3/target.mk b/src/main/target/IMPULSERCF3/target.mk index 53b083d225..db716cfd30 100644 --- a/src/main/target/IMPULSERCF3/target.mk +++ b/src/main/target/IMPULSERCF3/target.mk @@ -4,4 +4,5 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/flash_m25p16.c + drivers/flash_m25p16.c \ + drivers/max7456.c diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 8c22321a1b..5c0c475cbe 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -20,6 +20,8 @@ #define USBD_PRODUCT_STRING "KakuteF4-V1" +#define TARGET_CONFIG + #define LED0_PIN PB5 #define LED1_PIN PB4 #define LED2_PIN PB6 diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index aff23afdea..d1814fbb69 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -77,14 +77,22 @@ void targetConfiguration(void) gyroConfigMutable()->gyro_sync_denom = 4; pidConfigMutable()->pid_process_denom = 1; - pidProfilesMutable(0)->pid[PID_ROLL].P = 70; - pidProfilesMutable(0)->pid[PID_ROLL].I = 62; - pidProfilesMutable(0)->pid[PID_ROLL].D = 19; - pidProfilesMutable(0)->pid[PID_PITCH].P = 70; - pidProfilesMutable(0)->pid[PID_PITCH].I = 62; - pidProfilesMutable(0)->pid[PID_PITCH].D = 19; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - controlRateProfilesMutable(0)->rcRate8 = 70; - pidProfilesMutable(0)->pid[PID_LEVEL].I = 40; + pidProfile->pid[PID_ROLL].P = 70; + pidProfile->pid[PID_ROLL].I = 62; + pidProfile->pid[PID_ROLL].D = 19; + pidProfile->pid[PID_PITCH].P = 70; + pidProfile->pid[PID_PITCH].I = 62; + pidProfile->pid[PID_PITCH].D = 19; + pidProfile->pid[PID_LEVEL].I = 40; + } + + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + controlRateConfig->rcRate8 = 70; + } } #endif diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index dc198f4c6a..1a1f894a87 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -61,29 +61,33 @@ void targetConfiguration(void) rxChannelRangeConfigsMutable(channel)->max = 1860; }*/ - for (int profileId = 0; profileId < 2; profileId++) { - pidProfilesMutable(0)->pid[PID_ROLL].P = 60; - pidProfilesMutable(0)->pid[PID_ROLL].I = 70; - pidProfilesMutable(0)->pid[PID_ROLL].D = 17; - pidProfilesMutable(0)->pid[PID_PITCH].P = 80; - pidProfilesMutable(0)->pid[PID_PITCH].I = 90; - pidProfilesMutable(0)->pid[PID_PITCH].D = 18; - pidProfilesMutable(0)->pid[PID_YAW].P = 200; - pidProfilesMutable(0)->pid[PID_YAW].I = 45; - pidProfilesMutable(0)->pid[PID_LEVEL].P = 30; - pidProfilesMutable(0)->pid[PID_LEVEL].D = 30; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) { - controlRateProfilesMutable(rateProfileId)->rcRate8 = 100; - controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 110; - controlRateProfilesMutable(rateProfileId)->rcExpo8 = 0; - controlRateProfilesMutable(rateProfileId)->rates[FD_ROLL] = 77; - controlRateProfilesMutable(rateProfileId)->rates[FD_PITCH] = 77; - controlRateProfilesMutable(rateProfileId)->rates[FD_YAW] = 80; + pidProfile->pid[PID_ROLL].P = 60; + pidProfile->pid[PID_ROLL].I = 70; + pidProfile->pid[PID_ROLL].D = 17; + pidProfile->pid[PID_PITCH].P = 80; + pidProfile->pid[PID_PITCH].I = 90; + pidProfile->pid[PID_PITCH].D = 18; + pidProfile->pid[PID_YAW].P = 200; + pidProfile->pid[PID_YAW].I = 45; + pidProfile->pid[PID_LEVEL].P = 30; + pidProfile->pid[PID_LEVEL].D = 30; - pidProfilesMutable(0)->dtermSetpointWeight = 200; - pidProfilesMutable(0)->setpointRelaxRatio = 50; - } + pidProfile->dtermSetpointWeight = 200; + pidProfile->setpointRelaxRatio = 50; + } + + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + controlRateConfig->rcRate8 = 100; + controlRateConfig->rcYawRate8 = 110; + controlRateConfig->rcExpo8 = 0; + controlRateConfig->rates[FD_ROLL] = 77; + controlRateConfig->rates[FD_PITCH] = 77; + controlRateConfig->rates[FD_YAW] = 80; } #endif diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 4c24d3f375..28f2d9c452 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -36,6 +36,7 @@ #define TARGET_BOARD_IDENTIFIER "AFMN" #elif defined(BEEBRAIN) #define BRUSHED_MOTORS +#undef USE_SERVOS #define TARGET_BOARD_IDENTIFIER "BEBR" #define TARGET_CONFIG #define DEFAULT_FEATURES FEATURE_MOTOR_STOP diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 2e5353a7a6..183df84896 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -19,7 +19,8 @@ #undef TELEMETRY_IBUS //no space left #undef TELEMETRY_HOTT //no space left -#undef TELEMETRY_JETIEXBUS +#undef TELEMETRY_JETIEXBUS // no space left +#undef TELEMETRY_MAVLINK // no space left #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus diff --git a/src/main/target/OMNIBUSF4/config.c b/src/main/target/OMNIBUSF4/config.c new file mode 100644 index 0000000000..627743767d --- /dev/null +++ b/src/main/target/OMNIBUSF4/config.c @@ -0,0 +1,35 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 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 Cleanflight. If not, see . + */ + +#include +#include + +#include + +#ifdef TARGET_CONFIG + +#include "config/parameter_group.h" +#include "drivers/max7456.h" + +void targetConfiguration(void) +{ +#ifdef OMNIBUSF4BASE + // OMNIBUS F4 AIO (1st gen) has a AB7456 chip that is detected as MAX7456 + max7456ConfigMutable()->clockConfig = MAX7456_CLOCK_CONFIG_FULL; +#endif +} +#endif diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 27bbf82d88..9374a07e69 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -27,7 +27,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #if defined(OMNIBUSF4SD) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM DEF_TIM(TIM4, CH4, PB9, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S2_IN #else DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index dc68273884..3a96413a24 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -15,6 +15,8 @@ #pragma once +#define TARGET_CONFIG + #if defined(OMNIBUSF4SD) #define TARGET_BOARD_IDENTIFIER "OBSD" #elif defined(LUXF4OSD) @@ -23,6 +25,7 @@ #define TARGET_BOARD_IDENTIFIER "DYS4" #else #define TARGET_BOARD_IDENTIFIER "OBF4" +#define OMNIBUSF4BASE // For config.c #endif #if defined(LUXF4OSD) @@ -43,7 +46,10 @@ #define BEEPER_INVERTED #ifdef OMNIBUSF4SD +// These inverter control pins collide with timer channels on CH5 and CH6 pads. +// Users of these timers/pads must un-map the inverter assignment explicitly. #define INVERTER_PIN_UART6 PC8 // Omnibus F4 V3 and later +#define INVERTER_PIN_UART3 PC9 // Omnibus F4 Pro Corners #else #define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO XXX this is not used --- remove it at the next major release #endif @@ -231,7 +237,8 @@ #ifdef OMNIBUSF4SD #define USABLE_TIMER_CHANNEL_COUNT 15 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(10) | TIM_N(12) | TIM_N(8) | TIM_N(9)) #else #define USABLE_TIMER_CHANNEL_COUNT 14 -#endif #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#endif diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 4e000e7c77..c09cce08e4 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -55,6 +55,8 @@ #define MPU6500_SPI_INSTANCE SPI3 #define GYRO_1_CS_PIN MPU6000_CS_PIN #define GYRO_0_CS_PIN MPU6500_CS_PIN +#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6500_ALIGN CW90_DEG #else #define MPU6000_CS_PIN SPI3_NSS_PIN #define MPU6000_SPI_INSTANCE SPI3 diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 22738ba6a7..70d2d872dc 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -254,12 +254,6 @@ void systemResetToBootloader(void) { exit(0); } -// drivers/light_led.c -void ledInit(const statusLedConfig_t *statusLedConfig) { - UNUSED(statusLedConfig); - printf("[led]Init...\n"); -} - void timerInit(void) { printf("[timer]Init...\n"); } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 0ab64e0821..e5ccbc57e0 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -35,7 +35,7 @@ // file name to save config #define EEPROM_FILENAME "eeprom.bin" #define EEPROM_IN_RAM -#define EEPROM_SIZE 8192 +#define EEPROM_SIZE 32768 #define U_ID_0 0 #define U_ID_1 1 @@ -137,6 +137,15 @@ uint32_t SystemCoreClock; +#ifdef EEPROM_IN_RAM +extern uint8_t eepromData[EEPROM_SIZE]; +#define __config_start (*eepromData) +#define __config_end (*ARRAYEND(eepromData)) +#else +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; +#endif + #define UNUSED(x) (void)(x) typedef enum @@ -241,11 +250,11 @@ void FLASH_Lock(void); FLASH_Status FLASH_ErasePage(uintptr_t Page_Address); FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data); -uint64_t nanos64_real(); -uint64_t micros64_real(); -uint64_t millis64_real(); +uint64_t nanos64_real(void); +uint64_t micros64_real(void); +uint64_t millis64_real(void); void delayMicroseconds_real(uint32_t us); -uint64_t micros64(); -uint64_t millis64(); +uint64_t micros64(void); +uint64_t millis64(void); int lockMainPID(void); diff --git a/src/main/target/SPRACINGF3EVO/config.c b/src/main/target/SPRACINGF3EVO/config.c index 91d897aa91..6d9231ce1a 100644 --- a/src/main/target/SPRACINGF3EVO/config.c +++ b/src/main/target/SPRACINGF3EVO/config.c @@ -44,12 +44,16 @@ void targetConfiguration(void) motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - pidProfilesMutable(0)->pid[FD_ROLL].P = 90; - pidProfilesMutable(0)->pid[FD_ROLL].I = 44; - pidProfilesMutable(0)->pid[FD_ROLL].D = 60; - pidProfilesMutable(0)->pid[FD_PITCH].P = 90; - pidProfilesMutable(0)->pid[FD_PITCH].I = 44; - pidProfilesMutable(0)->pid[FD_PITCH].D = 60; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[FD_ROLL].P = 90; + pidProfile->pid[FD_ROLL].I = 44; + pidProfile->pid[FD_ROLL].D = 60; + pidProfile->pid[FD_PITCH].P = 90; + pidProfile->pid[FD_PITCH].I = 44; + pidProfile->pid[FD_PITCH].D = 60; + } #endif } #endif diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 7188b66211..0832f1b996 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -41,6 +41,8 @@ #undef USE_UNCOMMON_MIXERS #endif #undef TELEMETRY_JETIEXBUS +#undef USE_SERIALRX_JETIEXBUS +#undef USE_DASHBOARD #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT @@ -187,3 +189,5 @@ #else #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16)) #endif + +#undef USE_DASHBOARD diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index df20a803c4..cf944f207f 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -24,7 +24,7 @@ #define SPRACINGF4EVO_REV 2 #endif -#define USBD_PRODUCT_STRING "SP Racing F4 NEO" +#define USBD_PRODUCT_STRING "SP Racing F4 EVO" #define LED0_PIN PA0 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 6eabdeeadc..cec8f5e4a2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -31,6 +31,9 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE +#undef TELEMETRY_JETIEXBUS // ROM SAVING + + #define CURRENT_TARGET_CPU_VOLTAGE 3.0 #define LED0_PIN PE8 // Blue LEDs - PE8/PE12 @@ -126,12 +129,12 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 -#define OSD +//#define OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN SPI2_NSS_PIN -#define CMS +//#define CMS //#define USE_SDCARD // diff --git a/src/main/target/YUPIF4/config.c b/src/main/target/YUPIF4/config.c index a72a10e1f2..19d70e8a7a 100644 --- a/src/main/target/YUPIF4/config.c +++ b/src/main/target/YUPIF4/config.c @@ -22,38 +22,49 @@ #ifdef TARGET_CONFIG #include "blackbox/blackbox.h" - #include "fc/config.h" - #include "flight/pid.h" +#include "telemetry/telemetry.h" #include "hardware_revision.h" + // alternative defaults settings for YuPiF4 targets void targetConfiguration(void) { /* Changes depending on versions */ - if (hardwareRevision == YUPIF4_RACE2) { + if (hardwareRevision == YUPIF4_RACE3) { + beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); + telemetryConfigMutable()->halfDuplex = false; + + } else if (hardwareRevision == YUPIF4_RACE2) { beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); + } else if (hardwareRevision == YUPIF4_MINI) { beeperDevConfigMutable()->frequency = 0; blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE; adcConfigMutable()->current.enabled = 0; + } else if (hardwareRevision == YUPIF4_NAV) { beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); + } else { adcConfigMutable()->current.enabled = 0; } /* Specific PID values for YupiF4 */ - pidProfilesMutable(0)->pid[PID_ROLL].P = 30; - pidProfilesMutable(0)->pid[PID_ROLL].I = 45; - pidProfilesMutable(0)->pid[PID_ROLL].D = 20; - pidProfilesMutable(0)->pid[PID_PITCH].P = 30; - pidProfilesMutable(0)->pid[PID_PITCH].I = 50; - pidProfilesMutable(0)->pid[PID_PITCH].D = 20; - pidProfilesMutable(0)->pid[PID_YAW].P = 40; - pidProfilesMutable(0)->pid[PID_YAW].I = 50; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 30; + pidProfile->pid[PID_ROLL].I = 45; + pidProfile->pid[PID_ROLL].D = 20; + pidProfile->pid[PID_PITCH].P = 30; + pidProfile->pid[PID_PITCH].I = 50; + pidProfile->pid[PID_PITCH].D = 20; + pidProfile->pid[PID_YAW].P = 40; + pidProfile->pid[PID_YAW].I = 50; + } } #endif diff --git a/src/main/target/YUPIF4/hardware_revision.c b/src/main/target/YUPIF4/hardware_revision.c index c140355207..5e21a8351d 100644 --- a/src/main/target/YUPIF4/hardware_revision.c +++ b/src/main/target/YUPIF4/hardware_revision.c @@ -52,13 +52,16 @@ void detectHardwareRevision(void) no Hardware pins tied to ground => Race V1 if Pin 1 is the only one tied to ground => Mini if Pin 2 is the only one tied to ground => Race V2 + if Pin 2 and Pin 1 are tied to ground => Race V3 if Pin 3 is the only one tied to ground => Navigation Other combinations available for potential evolutions */ - if (!IORead(pin1)) { + if (!IORead(pin1) && IORead(pin2)) { hardwareRevision = YUPIF4_MINI; - } else if (!IORead(pin2)) { + } else if (IORead(pin1) && !IORead(pin2)) { hardwareRevision = YUPIF4_RACE2; + } else if (!IORead(pin1) && !IORead(pin2)) { + hardwareRevision = YUPIF4_RACE3; } else if (!IORead(pin3)) { hardwareRevision = YUPIF4_NAV; } else { diff --git a/src/main/target/YUPIF4/hardware_revision.h b/src/main/target/YUPIF4/hardware_revision.h index 83d58c1602..7a167ea043 100644 --- a/src/main/target/YUPIF4/hardware_revision.h +++ b/src/main/target/YUPIF4/hardware_revision.h @@ -20,8 +20,9 @@ typedef enum yupif4HardwareRevision_t { UNKNOWN = 0, YUPIF4_RACE1, // Race V1 YUPIF4_RACE2, // Race V2 + YUPIF4_RACE3, // Race V3 YUPIF4_MINI, // Mini - YUPIF4_NAV, // Navigation + YUPIF4_NAV // Navigation } yupif4HardwareRevision_e; extern uint8_t hardwareRevision; diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index cd0864dbab..0d91ddf32c 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -34,8 +34,6 @@ #define BEEPER_OPT PB14 #define BEEPER_PWM_HZ 3150 // Beeper PWM frequency in Hz -#define INVERTER_PIN_UART6 PB15 - // Gyro interrupt #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL @@ -72,6 +70,7 @@ // UART Ports #define USE_UART1 +#define INVERTER_PIN_UART1 PB12 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -80,6 +79,7 @@ #define UART3_TX_PIN PB10 #define USE_UART6 +#define INVERTER_PIN_UART6 PB15 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 @@ -137,10 +137,12 @@ // ADC inputs #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #define USE_ADC #define RSSI_ADC_GPIO_PIN PC0 #define VBAT_ADC_PIN PC1 #define CURRENT_METER_ADC_PIN PC2 +#define CURRENT_METER_SCALE_DEFAULT 150 // Default configuration #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL diff --git a/src/main/target/YUPIF7/config.c b/src/main/target/YUPIF7/config.c index 7d1a6100c0..b83870d235 100644 --- a/src/main/target/YUPIF7/config.c +++ b/src/main/target/YUPIF7/config.c @@ -30,13 +30,17 @@ void targetConfiguration(void) { /* Specific PID values for YupiF4 */ - pidProfilesMutable(0)->pid[PID_ROLL].P = 30; - pidProfilesMutable(0)->pid[PID_ROLL].I = 45; - pidProfilesMutable(0)->pid[PID_ROLL].D = 20; - pidProfilesMutable(0)->pid[PID_PITCH].P = 30; - pidProfilesMutable(0)->pid[PID_PITCH].I = 50; - pidProfilesMutable(0)->pid[PID_PITCH].D = 20; - pidProfilesMutable(0)->pid[PID_YAW].P = 40; - pidProfilesMutable(0)->pid[PID_YAW].I = 50; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 30; + pidProfile->pid[PID_ROLL].I = 45; + pidProfile->pid[PID_ROLL].D = 20; + pidProfile->pid[PID_PITCH].P = 30; + pidProfile->pid[PID_PITCH].I = 50; + pidProfile->pid[PID_PITCH].D = 20; + pidProfile->pid[PID_YAW].P = 40; + pidProfile->pid[PID_YAW].I = 50; + } } #endif diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index 9fecb75121..ac3285cbad 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_fc_post.h @@ -50,3 +50,17 @@ #undef USE_BARO_MS5611 #endif #endif + +#if defined(USE_MSP_OVER_TELEMETRY) +#if !defined(TELEMETRY_SMARTPORT) && !defined(TELEMETRY_CRSF) +#undef USE_MSP_OVER_TELEMETRY +#endif +#endif + +/* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */ +#if !defined(VTX_COMMON) || !defined(VTX_CONTROL) +#undef VTX_COMMON +#undef VTX_CONTROL +#undef VTX_TRAMP +#undef VTX_SMARTAUDIO +#endif diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index c104118700..faf9f078d8 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -26,7 +26,7 @@ //#pragma GCC diagnostic warning "-Wpadded" //#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons -#define DEBUG_MODE DEBUG_GYRO_NOTCH // change this to change initial debug mode +#define DEBUG_MODE DEBUG_NONE // change this to change initial debug mode #define I2C1_OVERCLOCK true #define I2C2_OVERCLOCK true @@ -125,6 +125,8 @@ #define VTX_TRAMP #define USE_CAMERA_CONTROL #define USE_HUFFMAN +#define USE_COPY_PROFILE_CMS_MENU +#define USE_MSP_OVER_TELEMETRY #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 6231b5e6d1..64e054afd4 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -24,6 +24,7 @@ #ifdef TELEMETRY #include "config/feature.h" +#include "build/build_config.h" #include "build/version.h" #include "config/parameter_group.h" @@ -31,6 +32,7 @@ #include "common/crc.h" #include "common/maths.h" +#include "common/printf.h" #include "common/streambuf.h" #include "common/utils.h" @@ -51,12 +53,16 @@ #include "telemetry/telemetry.h" #include "telemetry/crsf.h" +#include "telemetry/msp_shared.h" #include "fc/config.h" #define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz +#define CRSF_DEVICEINFO_VERSION 0x01 +#define CRSF_DEVICEINFO_PARAMETER_COUNT 255 static bool crsfTelemetryEnabled; +static bool deviceInfoReplyPending; static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; static void crsfInitializeFrame(sbuf_t *dst) @@ -135,7 +141,7 @@ void crsfFrameGps(sbuf_t *dst) Payload: uint16_t Voltage ( mV * 100 ) uint16_t Current ( mA * 100 ) -uint24_t Capacity ( mAh ) +uint24_t Fuel ( drawn mAh ) uint8_t Battery remaining ( percent ) */ void crsfFrameBatterySensor(sbuf_t *dst) @@ -145,11 +151,11 @@ void crsfFrameBatterySensor(sbuf_t *dst) sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.1V sbufWriteU16BigEndian(dst, getAmperage() / 10); - const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; + const uint32_t mAhDrawn = getMAhDrawn(); const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining(); - sbufWriteU8(dst, (batteryCapacity >> 16)); - sbufWriteU8(dst, (batteryCapacity >> 8)); - sbufWriteU8(dst, (uint8_t)batteryCapacity); + sbufWriteU8(dst, (mAhDrawn >> 16)); + sbufWriteU8(dst, (mAhDrawn >> 8)); + sbufWriteU8(dst, (uint8_t)mAhDrawn); sbufWriteU8(dst, batteryRemainingPercentage); } @@ -223,13 +229,75 @@ void crsfFrameFlightMode(sbuf_t *dst) *lengthPtr = sbufPtr(dst) - lengthPtr; } +/* +0x29 Device Info +Payload: +uint8_t Destination +uint8_t Origin +char[] Device Name ( Null terminated string ) +uint32_t Null Bytes +uint32_t Null Bytes +uint32_t Null Bytes +uint8_t 255 (Max MSP Parameter) +uint8_t 0x01 (Parameter version 1) +*/ +void crsfFrameDeviceInfo(sbuf_t *dst) { + + char buff[30]; + tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier); + + uint8_t *lengthPtr = sbufPtr(dst); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO); + sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); + sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT); + sbufWriteStringWithZeroTerminator(dst, buff); + for (unsigned int ii=0; ii<12; ii++) { + sbufWriteU8(dst, 0x00); + } + sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT); + sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION); + *lengthPtr = sbufPtr(dst) - lengthPtr; +} + #define BV(x) (1 << (x)) // bit value // schedule array to decide how often each type of frame is sent -#define CRSF_SCHEDULE_COUNT_MAX 5 +typedef enum { + CRSF_FRAME_START_INDEX = 0, + CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX, + CRSF_FRAME_BATTERY_SENSOR_INDEX, + CRSF_FRAME_FLIGHT_MODE_INDEX, + CRSF_FRAME_GPS_INDEX, + CRSF_SCHEDULE_COUNT_MAX +} crsfFrameTypeIndex_e; + static uint8_t crsfScheduleCount; static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; +#if defined(USE_MSP_OVER_TELEMETRY) + +static bool mspReplyPending; + +void crsfScheduleMspResponse(void) +{ + mspReplyPending = true; +} + +void crsfSendMspResponse(uint8_t *payload) +{ + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + + crsfInitializeFrame(dst); + sbufWriteU8(dst, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); + sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); + sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); + sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT); + sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE); + crsfFinalize(dst); +} +#endif static void processCrsf(void) { @@ -239,23 +307,24 @@ static void processCrsf(void) sbuf_t crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf; - if (currentSchedule & BV(CRSF_FRAME_ATTITUDE)) { + if (currentSchedule & BV(CRSF_FRAME_ATTITUDE_INDEX)) { crsfInitializeFrame(dst); crsfFrameAttitude(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR)) { + if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR_INDEX)) { crsfInitializeFrame(dst); crsfFrameBatterySensor(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) { + + if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) { crsfInitializeFrame(dst); crsfFrameFlightMode(dst); crsfFinalize(dst); } #ifdef GPS - if (currentSchedule & BV(CRSF_FRAME_GPS)) { + if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) { crsfInitializeFrame(dst); crsfFrameGps(dst); crsfFinalize(dst); @@ -264,17 +333,28 @@ static void processCrsf(void) crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } +void crsfScheduleDeviceInfoResponse(void) +{ + deviceInfoReplyPending = true; +} + void initCrsfTelemetry(void) { // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) // and feature is enabled, if so, set CRSF telemetry enabled crsfTelemetryEnabled = crsfRxIsActive(); + + deviceInfoReplyPending = false; +#if defined(USE_MSP_OVER_TELEMETRY) + mspReplyPending = false; +#endif + int index = 0; - crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE); - crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR); - crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE); + crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); + crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); + crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); if (feature(FEATURE_GPS)) { - crsfSchedule[index++] = BV(CRSF_FRAME_GPS); + crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); } crsfScheduleCount = (uint8_t)index; @@ -303,7 +383,20 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) { crsfLastCycleTime = currentTimeUs; - processCrsf(); + if (deviceInfoReplyPending) { + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + crsfInitializeFrame(dst); + crsfFrameDeviceInfo(dst); + crsfFinalize(dst); + deviceInfoReplyPending = false; +#if defined(USE_MSP_OVER_TELEMETRY) + } else if (mspReplyPending) { + mspReplyPending = sendMspReply(CRSF_FRAME_TX_MSP_PAYLOAD_SIZE, &crsfSendMspResponse); +#endif + } else { + processCrsf(); + } } } @@ -315,17 +408,17 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) crsfInitializeFrame(sbuf); switch (frameType) { default: - case CRSF_FRAME_ATTITUDE: + case CRSF_FRAMETYPE_ATTITUDE: crsfFrameAttitude(sbuf); break; - case CRSF_FRAME_BATTERY_SENSOR: + case CRSF_FRAMETYPE_BATTERY_SENSOR: crsfFrameBatterySensor(sbuf); break; - case CRSF_FRAME_FLIGHT_MODE: + case CRSF_FRAMETYPE_FLIGHT_MODE: crsfFrameFlightMode(sbuf); break; #if defined(GPS) - case CRSF_FRAME_GPS: + case CRSF_FRAMETYPE_GPS: crsfFrameGps(sbuf); break; #endif diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 980467f517..3ccfc76cac 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -18,17 +18,13 @@ #pragma once #include "common/time.h" - -typedef enum { - CRSF_FRAME_START = 0, - CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, - CRSF_FRAME_BATTERY_SENSOR, - CRSF_FRAME_FLIGHT_MODE, - CRSF_FRAME_GPS -} crsfFrameType_e; +#include "rx/crsf.h" +#include "telemetry/msp_shared.h" void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); +void crsfScheduleDeviceInfoResponse(void); +void crsfScheduleMspResponse(void); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 4ead0d2c0b..a956aebfcb 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -58,6 +58,7 @@ #include "platform.h" + #ifdef TELEMETRY #include "build/build_config.h" @@ -65,29 +66,24 @@ #include "common/axis.h" #include "common/time.h" -#include "common/utils.h" - -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" +#include "drivers/serial.h" #include "drivers/time.h" -#include "io/serial.h" - #include "fc/runtime_config.h" -#include "sensors/sensors.h" -#include "sensors/battery.h" -#include "sensors/barometer.h" - +#include "flight/altitude.h" #include "flight/pid.h" #include "flight/navigation.h" + #include "io/gps.h" -#include "flight/altitude.h" +#include "sensors/battery.h" +#include "sensors/barometer.h" +#include "sensors/sensors.h" -#include "telemetry/telemetry.h" #include "telemetry/hott.h" +#include "telemetry/telemetry.h" //#define HOTT_DEBUG @@ -229,16 +225,13 @@ static bool shouldTriggerBatteryAlarmNow(void) static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) { - batteryState_e batteryState; - if (shouldTriggerBatteryAlarmNow()) { lastHottAlarmSoundTime = millis(); - batteryState = getBatteryState(); + const batteryState_e batteryState = getBatteryState(); if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { hottEAMMessage->warning_beeps = 0x10; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1; - } - else { + } else { hottEAMMessage->warning_beeps = HOTT_EAM_ALARM1_FLAG_NONE; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_NONE; } @@ -257,14 +250,14 @@ static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t amp = getAmperage() / 10; + const int32_t amp = getAmperage() / 10; hottEAMMessage->current_L = amp & 0xFF; hottEAMMessage->current_H = amp >> 8; } static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t mAh = getMAhDrawn() / 10; + const int32_t mAh = getMAhDrawn() / 10; hottEAMMessage->batt_cap_L = mAh & 0xFF; hottEAMMessage->batt_cap_H = mAh >> 8; } @@ -279,7 +272,7 @@ static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t vario = getEstimatedVario(); + const int32_t vario = getEstimatedVario(); hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF; hottEAMMessage->climbrate_H = (30000 + vario) >> 8; hottEAMMessage->climbrate3s = 120 + (vario / 100); @@ -327,7 +320,8 @@ static void flushHottRxBuffer(void) } } -static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e mode) { +static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e mode) +{ closeSerialPort(hottPort); portOptions_e portOptions = telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED; @@ -339,28 +333,34 @@ static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, portOptions); } -static bool hottIsUsingHardwareUART(void) { +static bool hottIsUsingHardwareUART(void) +{ return !(portConfig->identifier == SERIAL_PORT_SOFTSERIAL1 || portConfig->identifier == SERIAL_PORT_SOFTSERIAL2); } -static void hottConfigurePortForTX(void) { +static void hottConfigurePortForTX(void) +{ // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences - if (hottIsUsingHardwareUART()) + if (hottIsUsingHardwareUART()) { workAroundForHottTelemetryOnUsart(hottPort, MODE_TX); - else + } else { serialSetMode(hottPort, MODE_TX); + } } -static void hottConfigurePortForRX(void) { +static void hottConfigurePortForRX(void) +{ // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences - if (hottIsUsingHardwareUART()) + if (hottIsUsingHardwareUART()) { workAroundForHottTelemetryOnUsart(hottPort, MODE_RX); - else + } else { serialSetMode(hottPort, MODE_RX); + } flushHottRxBuffer(); } -static void hottReconfigurePort(void) { +static void hottReconfigurePort(void) +{ if (!hottIsSending) { hottIsSending = true; hottMsgCrc = 0; @@ -426,7 +426,8 @@ static void hottPrepareMessages(void) { #endif } -static void processBinaryModeRequest(uint8_t address) { +static void processBinaryModeRequest(uint8_t address) +{ #ifdef HOTT_DEBUG static uint8_t hottBinaryRequests = 0; @@ -436,24 +437,23 @@ static void processBinaryModeRequest(uint8_t address) { switch (address) { #ifdef GPS - case 0x8A: + case 0x8A: #ifdef HOTT_DEBUG - hottGPSRequests++; + hottGPSRequests++; #endif - if (sensors(SENSOR_GPS)) { - hottSendGPSResponse(); - } - break; + if (sensors(SENSOR_GPS)) { + hottSendGPSResponse(); + } + break; #endif - case 0x8E: + case 0x8E: #ifdef HOTT_DEBUG - hottEAMRequests++; + hottEAMRequests++; #endif - hottSendEAMResponse(); - break; + hottSendEAMResponse(); + break; } - #ifdef HOTT_DEBUG hottBinaryRequests++; debug[0] = hottBinaryRequests; @@ -462,14 +462,13 @@ static void processBinaryModeRequest(uint8_t address) { #endif debug[2] = hottEAMRequests; #endif - } static void hottCheckSerialData(uint32_t currentMicros) { static bool lookingForRequest = true; - uint8_t bytesWaiting = serialRxBytesWaiting(hottPort); + const uint8_t bytesWaiting = serialRxBytesWaiting(hottPort); if (bytesWaiting <= 1) { return; @@ -494,8 +493,8 @@ static void hottCheckSerialData(uint32_t currentMicros) lookingForRequest = true; } - uint8_t requestId = serialRead(hottPort); - uint8_t address = serialRead(hottPort); + const uint8_t requestId = serialRead(hottPort); + const uint8_t address = serialRead(hottPort); if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) { /* @@ -528,7 +527,7 @@ static inline bool shouldPrepareHoTTMessages(uint32_t currentMicros) return currentMicros - lastMessagesPreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ; } -static inline bool shouldCheckForHoTTRequest() +static inline bool shouldCheckForHoTTRequest(void) { if (hottIsSending) { return false; @@ -538,7 +537,7 @@ static inline bool shouldCheckForHoTTRequest() void checkHoTTTelemetryState(void) { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(hottPortSharing); + const bool newTelemetryEnabledValue = telemetryDetermineEnabledState(hottPortSharing); if (newTelemetryEnabledValue == hottTelemetryEnabled) { return; diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index c4cfed8188..4292d386f9 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -202,7 +202,7 @@ static void ltm_sframe(void) * Attitude A-frame - 10 Hz at > 2400 baud * PITCH ROLL HEADING */ -static void ltm_aframe() +static void ltm_aframe(void) { ltm_initialise_packet('A'); ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.pitch)); @@ -216,7 +216,7 @@ static void ltm_aframe() * This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD * home pos, home alt, direction to home */ -static void ltm_oframe() +static void ltm_oframe(void) { ltm_initialise_packet('O'); #if defined(GPS) diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c new file mode 100644 index 0000000000..d80ec2a168 --- /dev/null +++ b/src/main/telemetry/msp_shared.c @@ -0,0 +1,231 @@ +#include +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_MSP_OVER_TELEMETRY) + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "fc/fc_msp.h" + +#include "msp/msp.h" + +#include "rx/crsf.h" +#include "rx/msp.h" + +#include "telemetry/msp_shared.h" +#include "telemetry/smartport.h" + +#define TELEMETRY_MSP_VERSION 1 +#define TELEMETRY_MSP_VER_SHIFT 5 +#define TELEMETRY_MSP_VER_MASK (0x7 << TELEMETRY_MSP_VER_SHIFT) +#define TELEMETRY_MSP_ERROR_FLAG (1 << 5) +#define TELEMETRY_MSP_START_FLAG (1 << 4) +#define TELEMETRY_MSP_SEQ_MASK 0x0F +#define TELEMETRY_MSP_RES_ERROR (-10) + +enum { + TELEMETRY_MSP_VER_MISMATCH=0, + TELEMETRY_MSP_CRC_ERROR=1, + TELEMETRY_MSP_ERROR=2 +}; + +#define REQUEST_BUFFER_SIZE 64 +#define RESPONSE_BUFFER_SIZE 64 + +STATIC_UNIT_TESTED uint8_t checksum = 0; +STATIC_UNIT_TESTED mspPackage_t mspPackage; +static mspRxBuffer_t mspRxBuffer; +static mspTxBuffer_t mspTxBuffer; +static mspPacket_t mspRxPacket; +static mspPacket_t mspTxPacket; + +void initSharedMsp(void) +{ + mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer; + mspPackage.requestPacket = &mspRxPacket; + mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer; + mspPackage.requestPacket->buf.end = mspPackage.requestBuffer; + + mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer; + mspPackage.responsePacket = &mspTxPacket; + mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer; + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; +} + +static void processMspPacket(void) +{ + mspPackage.responsePacket->cmd = 0; + mspPackage.responsePacket->result = 0; + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + + mspPostProcessFnPtr mspPostProcessFn = NULL; + if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { + sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR); + } + if (mspPostProcessFn) { + mspPostProcessFn(NULL); + } + + sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); +} + +void sendMspErrorResponse(uint8_t error, int16_t cmd) +{ + mspPackage.responsePacket->cmd = cmd; + mspPackage.responsePacket->result = 0; + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + + sbufWriteU8(&mspPackage.responsePacket->buf, error); + mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR; + sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); +} + +bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) +{ + static uint8_t mspStarted = 0; + static uint8_t lastSeq = 0; + + if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { + mspStarted = 0; + } + + if (mspStarted == 0) { + initSharedMsp(); + } + + mspPacket_t *packet = mspPackage.requestPacket; + sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd); + sbuf_t *rxBuf = &mspPackage.requestPacket->buf; + const uint8_t header = sbufReadU8(frameBuf); + const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK; + const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT; + + if (version != TELEMETRY_MSP_VERSION) { + sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0); + return true; + } + + if (header & TELEMETRY_MSP_START_FLAG) { + // first packet in sequence + uint8_t mspPayloadSize = sbufReadU8(frameBuf); + + packet->cmd = sbufReadU8(frameBuf); + packet->result = 0; + packet->buf.ptr = mspPackage.requestBuffer; + packet->buf.end = mspPackage.requestBuffer + mspPayloadSize; + + checksum = mspPayloadSize ^ packet->cmd; + mspStarted = 1; + } else if (!mspStarted) { + // no start packet yet, throw this one away + return false; + } else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) { + // packet loss detected! + mspStarted = 0; + return false; + } + + const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf); + const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf); + uint8_t payload[frameBytesRemaining]; + + if (bufferBytesRemaining >= frameBytesRemaining) { + sbufReadData(frameBuf, payload, frameBytesRemaining); + sbufAdvance(frameBuf, frameBytesRemaining); + sbufWriteData(rxBuf, payload, frameBytesRemaining); + lastSeq = seqNumber; + return false; + } else { + sbufReadData(frameBuf, payload, bufferBytesRemaining); + sbufAdvance(frameBuf, bufferBytesRemaining); + sbufWriteData(rxBuf, payload, bufferBytesRemaining); + sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); + while (sbufBytesRemaining(rxBuf)) { + checksum ^= sbufReadU8(rxBuf); + } + + if (checksum != *frameBuf->ptr) { + mspStarted = 0; + sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd); + return true; + } + } + + mspStarted = 0; + sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); + processMspPacket(); + return true; +} + +bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) +{ + static uint8_t checksum = 0; + static uint8_t seq = 0; + + uint8_t payloadOut[payloadSize]; + sbuf_t payload; + sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize); + sbuf_t *txBuf = &mspPackage.responsePacket->buf; + + // detect first reply packet + if (txBuf->ptr == mspPackage.responseBuffer) { + + // header + uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK); + if (mspPackage.responsePacket->result < 0) { + head |= TELEMETRY_MSP_ERROR_FLAG; + } + sbufWriteU8(payloadBuf, head); + + uint8_t size = sbufBytesRemaining(txBuf); + sbufWriteU8(payloadBuf, size); + } else { + // header + sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK)); + } + + const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); + const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); + uint8_t frame[payloadBytesRemaining]; + + if (bufferBytesRemaining >= payloadBytesRemaining) { + + sbufReadData(txBuf, frame, payloadBytesRemaining); + sbufAdvance(txBuf, payloadBytesRemaining); + sbufWriteData(payloadBuf, frame, payloadBytesRemaining); + responseFn(payloadOut); + + return true; + + } else { + + sbufReadData(txBuf, frame, bufferBytesRemaining); + sbufAdvance(txBuf, bufferBytesRemaining); + sbufWriteData(payloadBuf, frame, bufferBytesRemaining); + sbufSwitchToReader(txBuf, mspPackage.responseBuffer); + + checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd; + + while (sbufBytesRemaining(txBuf)) { + checksum ^= sbufReadU8(txBuf); + } + sbufWriteU8(payloadBuf, checksum); + + while (sbufBytesRemaining(payloadBuf)>1) { + sbufWriteU8(payloadBuf, 0); + } + + } + + responseFn(payloadOut); + return false; +} + +#endif diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h new file mode 100644 index 0000000000..85a3e2c703 --- /dev/null +++ b/src/main/telemetry/msp_shared.h @@ -0,0 +1,29 @@ +#pragma once + +#include "msp/msp.h" +#include "rx/crsf.h" +#include "telemetry/smartport.h" + +typedef void (*mspResponseFnPtr)(uint8_t *payload); + +typedef struct mspPackage_s { + sbuf_t requestFrame; + uint8_t *requestBuffer; + uint8_t *responseBuffer; + mspPacket_t *requestPacket; + mspPacket_t *responsePacket; +} mspPackage_t; + +typedef union mspRxBuffer_u { + uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; + uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE]; +} mspRxBuffer_t; + +typedef union mspTxBuffer_u { + uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE]; + uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE]; +} mspTxBuffer_t; + +void initSharedMsp(void); +bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); +bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 080df64911..0ecfaba1c3 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -43,8 +43,6 @@ #include "io/gps.h" #include "io/serial.h" -#include "msp/msp.h" - #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/battery.h" @@ -54,10 +52,10 @@ #include "sensors/gyro.h" #include "rx/rx.h" -#include "rx/msp.h" #include "telemetry/telemetry.h" #include "telemetry/smartport.h" +#include "telemetry/msp_shared.h" enum { @@ -164,7 +162,6 @@ typedef struct smartPortFrame_s { } __attribute__((packed)) smartPortFrame_t; #define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t) -#define SMARTPORT_TX_BUF_SIZE 256 #define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId) #define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1) @@ -172,29 +169,9 @@ typedef struct smartPortFrame_s { static smartPortFrame_t smartPortRxBuffer; static uint8_t smartPortRxBytes = 0; static bool smartPortFrameReceived = false; - -#define SMARTPORT_MSP_VERSION 1 -#define SMARTPORT_MSP_VER_SHIFT 5 -#define SMARTPORT_MSP_VER_MASK (0x7 << SMARTPORT_MSP_VER_SHIFT) -#define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT) - -#define SMARTPORT_MSP_ERROR_FLAG (1 << 5) -#define SMARTPORT_MSP_START_FLAG (1 << 4) -#define SMARTPORT_MSP_SEQ_MASK 0x0F - -#define SMARTPORT_MSP_RX_BUF_SIZE 64 - -static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE]; -static mspPacket_t smartPortMspReply; +#if defined(USE_MSP_OVER_TELEMETRY) static bool smartPortMspReplyPending = false; - -#define SMARTPORT_MSP_RES_ERROR (-10) - -enum { - SMARTPORT_MSP_VER_MISMATCH=0, - SMARTPORT_MSP_CRC_ERROR=1, - SMARTPORT_MSP_ERROR=2 -}; +#endif static void smartPortDataReceive(uint16_t c) { @@ -352,195 +329,11 @@ void checkSmartPortTelemetryState(void) freeSmartPortTelemetryPort(); } -static void initSmartPortMspReply(int16_t cmd) -{ - smartPortMspReply.buf.ptr = smartPortMspTxBuffer; - smartPortMspReply.buf.end = ARRAYEND(smartPortMspTxBuffer); - - smartPortMspReply.cmd = cmd; - smartPortMspReply.result = 0; -} - -static void processMspPacket(mspPacket_t* packet) -{ - initSmartPortMspReply(0); - - if (mspFcProcessCommand(packet, &smartPortMspReply, NULL) == MSP_RESULT_ERROR) { - sbufWriteU8(&smartPortMspReply.buf, SMARTPORT_MSP_ERROR); - } - - // change streambuf direction - sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); - smartPortMspReplyPending = true; -} - -/** - * Request frame format: - * - Header: 1 byte - * - Reserved: 2 bits (future use) - * - Error-flag: 1 bit - * - Start-flag: 1 bit - * - CSeq: 4 bits - * - * - MSP payload: - * - if Error-flag == 0: - * - size: 1 byte - * - payload - * - CRC (request type included) - * - if Error-flag == 1: - * - size: 1 byte (== 1) - * - error: 1 Byte - * - 0: Version mismatch (type=0) - * - 1: Sequence number error - * - 2: MSP error - * - CRC (request type included) - */ -bool smartPortSendMspReply() -{ - static uint8_t checksum = 0; - static uint8_t seq = 0; - - uint8_t packet[SMARTPORT_PAYLOAD_SIZE]; - uint8_t* p = packet; - uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; - - sbuf_t* txBuf = &smartPortMspReply.buf; - - // detect first reply packet - if (txBuf->ptr == smartPortMspTxBuffer) { - - // header - uint8_t head = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); - if (smartPortMspReply.result < 0) { - head |= SMARTPORT_MSP_ERROR_FLAG; - } - *p++ = head; - - uint8_t size = sbufBytesRemaining(txBuf); - *p++ = size; - - checksum = size ^ smartPortMspReply.cmd; - } - else { - // header - *p++ = (seq++ & SMARTPORT_MSP_SEQ_MASK); - } - - while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { - *p = sbufReadU8(txBuf); - checksum ^= *p++; // MSP checksum - } - - // to be continued... - if (p == end) { - smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); - return true; - } - - // nothing left in txBuf, - // append the MSP checksum - *p++ = checksum; - - // pad with zeros - while (p < end) - *p++ = 0; - - smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); - return false; -} - -void smartPortSendErrorReply(uint8_t error, int16_t cmd) -{ - initSmartPortMspReply(cmd); - sbufWriteU8(&smartPortMspReply.buf,error); - smartPortMspReply.result = SMARTPORT_MSP_RES_ERROR; - - sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); - smartPortMspReplyPending = true; -} - -/** - * Request frame format: - * - Header: 1 byte - * - Version: 3 bits - * - Start-flag: 1 bit - * - CSeq: 4 bits - * - * - MSP payload: - * - Size: 1 Byte - * - Type: 1 Byte - * - payload... - * - CRC - */ -void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) -{ - static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; - static uint8_t mspStarted = 0; - static uint8_t lastSeq = 0; - static uint8_t checksum = 0; - static mspPacket_t cmd; - - // re-assemble MSP frame & forward to MSP port when complete - uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET; - uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; - - uint8_t head = *p++; - uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK; - uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT; - - if (version != SMARTPORT_MSP_VERSION) { - mspStarted = 0; - smartPortSendErrorReply(SMARTPORT_MSP_VER_MISMATCH,0); - return; - } - - // check start-flag - if (head & SMARTPORT_MSP_START_FLAG) { - - //TODO: if (p_size > SMARTPORT_MSP_RX_BUF_SIZE) error! - uint8_t p_size = *p++; - cmd.cmd = *p++; - cmd.result = 0; - - cmd.buf.ptr = mspBuffer; - cmd.buf.end = mspBuffer + p_size; - - checksum = p_size ^ cmd.cmd; - mspStarted = 1; - } else if (!mspStarted) { - // no start packet yet, throw this one away - return; - } else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) { - // packet loss detected! - mspStarted = 0; - return; - } - - // copy payload bytes - while ((p < end) && sbufBytesRemaining(&cmd.buf)) { - checksum ^= *p; - sbufWriteU8(&cmd.buf,*p++); - } - - // reached end of smart port frame - if (p == end) { - lastSeq = seq; - return; - } - - // last byte must be the checksum - if (checksum != *p) { - mspStarted = 0; - smartPortSendErrorReply(SMARTPORT_MSP_CRC_ERROR,cmd.cmd); - return; - } - - // end of MSP packet reached - mspStarted = 0; - sbufSwitchToReader(&cmd.buf,mspBuffer); - - processMspPacket(&cmd); +#if defined(USE_MSP_OVER_TELEMETRY) +void smartPortSendMspResponse(uint8_t *payload) { + smartPortSendPackageEx(FSSP_MSPS_FRAME, payload); } +#endif void handleSmartPortTelemetry(void) { @@ -563,11 +356,16 @@ void handleSmartPortTelemetry(void) smartPortFrameReceived = false; // do not check the physical ID here again // unless we start receiving other sensors' packets - if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { +#if defined(USE_MSP_OVER_TELEMETRY) + if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { // Pass only the payload: skip sensorId & frameId - handleSmartPortMspFrame(&smartPortRxBuffer); + uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET; + uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE; + + smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd); } +#endif } while (smartPortHasRequest) { @@ -577,11 +375,13 @@ void handleSmartPortTelemetry(void) return; } +#if defined(USE_MSP_OVER_TELEMETRY) if (smartPortMspReplyPending) { - smartPortMspReplyPending = smartPortSendMspReply(); + smartPortMspReplyPending = sendMspReply(SMARTPORT_PAYLOAD_SIZE, &smartPortSendMspResponse); smartPortHasRequest = 0; return; } +#endif // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 35f18fc4ae..2c918cb249 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,6 +7,9 @@ #pragma once +#define SMARTPORT_MSP_TX_BUF_SIZE 256 +#define SMARTPORT_MSP_RX_BUF_SIZE 64 + void initSmartPortTelemetry(void); void handleSmartPortTelemetry(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 304271d8c9..369d8a38c4 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -52,6 +52,7 @@ #include "telemetry/crsf.h" #include "telemetry/srxl.h" #include "telemetry/ibus.h" +#include "telemetry/msp_shared.h" PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); @@ -100,6 +101,9 @@ void telemetryInit(void) #ifdef TELEMETRY_IBUS initIbusTelemetry(); #endif +#if defined(USE_MSP_OVER_TELEMETRY) + initSharedMsp(); +#endif telemetryCheckState(); } diff --git a/src/main/vcp/usb_pwr.c b/src/main/vcp/usb_pwr.c index 8cdd7eb3d1..e383ab2c81 100644 --- a/src/main/vcp/usb_pwr.c +++ b/src/main/vcp/usb_pwr.c @@ -88,7 +88,7 @@ RESULT PowerOn(void) * Output : None. * Return : USB_SUCCESS. *******************************************************************************/ -RESULT PowerOff() +RESULT PowerOff(void) { /* disable all interrupts and force USB reset */ _SetCNTR(CNTR_FRES); diff --git a/src/test/Makefile b/src/test/Makefile index b9a960ef0d..d89c1aa725 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -11,6 +11,7 @@ # Where to find user code. USER_DIR = ../main +TEST_DIR = unit # specify which files that are included in the test in addition to the unittest file. @@ -35,6 +36,9 @@ arming_prevention_unittest_SRC := \ $(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/common/bitarray.c +atomic_unittest_SRC := \ + $(USER_DIR)/build/atomic.c \ + $(TEST_DIR)/atomic_unittest_c.c baro_bmp085_unittest_SRC := \ $(USER_DIR)/drivers/barometer/barometer_bmp085.c \ @@ -172,7 +176,10 @@ rc_controls_unittest_SRC := \ rx_crsf_unittest_SRC := \ $(USER_DIR)/rx/crsf.c \ $(USER_DIR)/common/crc.c \ - $(USER_DIR)/common/streambuf.c + $(USER_DIR)/common/printf.c \ + $(USER_DIR)/common/typeconversion.c \ + $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/drivers/serial.c rx_ibus_unittest_SRC := \ @@ -207,6 +214,8 @@ telemetry_crsf_unittest_SRC := \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/common/streambuf.c \ $(USER_DIR)/common/gps_conversion.c \ + $(USER_DIR)/common/printf.c \ + $(USER_DIR)/common/typeconversion.c \ $(USER_DIR)/fc/runtime_config.c telemetry_crsf_unittest_DEFINES := \ @@ -216,6 +225,23 @@ telemetry_crsf_unittest_DEFINES := \ __REVISION__="revision" +telemetry_crsf_msp_unittest_SRC := \ + $(USER_DIR)/rx/crsf.c \ + $(USER_DIR)/common/crc.c \ + $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/common/printf.c \ + $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/drivers/serial.c \ + $(USER_DIR)/common/typeconversion.c \ + $(USER_DIR)/telemetry/crsf.c \ + $(USER_DIR)/common/gps_conversion.c \ + $(USER_DIR)/telemetry/msp_shared.c \ + $(USER_DIR)/fc/runtime_config.c + +telemetry_crsf_msp_unittest_DEFINES := \ + USE_MSP_OVER_TELEMETRY + + telemetry_hott_unittest_SRC := \ $(USER_DIR)/telemetry/hott.c \ $(USER_DIR)/common/gps_conversion.c @@ -254,6 +280,9 @@ huffman_unittest_SRC := \ huffman_unittest_DEFINES := \ USE_HUFFMAN +ringbuffer_unittest_SRC := \ + $(USER_DIR)/common/ringbuffer.c + # Please tweak the following variable definitions as needed by your # project, except GTEST_HEADERS, which you can use in your own targets # but shouldn't modify. @@ -263,14 +292,18 @@ huffman_unittest_DEFINES := \ GTEST_DIR = ../../lib/test/gtest -TEST_DIR = unit USER_INCLUDE_DIR = $(USER_DIR) OBJECT_DIR = ../../obj/test +# Determine the OS +UNAME := $(shell uname -s) + # Use clang/clang++ by default CC := clang CXX := clang++ +#CC := gcc +#CXX := g++ COMMON_FLAGS = \ -g \ @@ -278,12 +311,20 @@ COMMON_FLAGS = \ -Wextra \ -Werror \ -ggdb3 \ - -pthread \ -O0 \ -DUNIT_TEST \ -isystem $(GTEST_DIR)/inc \ -MMD -MP +ifeq ($(shell $(CC) -v 2>&1 | grep -q "clang version" && echo "clang"),clang) +COMMON_FLAGS += -fblocks +LDFLAGS += -lBlocksRuntime +endif + +ifneq ($(UNAME), Darwin) +COMMON_FLAGS += -pthread +endif + # Flags passed to the C compiler. C_FLAGS = $(COMMON_FLAGS) \ -std=gnu99 @@ -298,12 +339,11 @@ COVERAGE_FLAGS := --coverage C_FLAGS += $(COVERAGE_FLAGS) CXX_FLAGS += $(COVERAGE_FLAGS) -# Determine the OS and set up the parameter group linker flags accordingly -UNAME := $(shell uname -s) +# Set up the parameter group linker flags according to OS ifeq ($(UNAME), Darwin) -PG_FLAGS = -Wl,-map,$(OBJECT_DIR)/$@.map +LDFLAGS += -Wl,-map,$(OBJECT_DIR)/$@.map else -PG_FLAGS = -Wl,-T,$(TEST_DIR)/parameter_group.ld -Wl,-Map,$(OBJECT_DIR)/$@.map +LDFLAGS += -Wl,-T,$(TEST_DIR)/parameter_group.ld -Wl,-Map,$(OBJECT_DIR)/$@.map endif # Gather up all of the tests. @@ -402,7 +442,7 @@ TEST_CFLAGS = $(addprefix -I,$(TEST_INCLUDE_DIRS)) # param $1 = testname define test-specific-stuff -$$1_OBJS = $$(patsubst $$(USER_DIR)%,$$(OBJECT_DIR)/$1%,$$($1_SRC:=.o)) +$$1_OBJS = $$(patsubst $$(TEST_DIR)%,$$(OBJECT_DIR)/$1%, $$(patsubst $$(USER_DIR)%,$$(OBJECT_DIR)/$1%,$$($1_SRC:=.o))) # $$(info $1 -v-v-------) # $$(info $1_SRC: $($1_SRC)) @@ -422,6 +462,12 @@ $(OBJECT_DIR)/$1/%.c.o: $(USER_DIR)/%.c $(foreach def,$($1_DEFINES),-D $(def)) \ -c $$< -o $$@ +$(OBJECT_DIR)/$1/%.c.o: $(TEST_DIR)/%.c + @echo "compiling test c file: $$<" "$(STDOUT)" + $(V1) mkdir -p $$(dir $$@) + $(V1) $(CC) $(C_FLAGS) $(TEST_CFLAGS) \ + $(foreach def,$($1_DEFINES),-D $(def)) \ + -c $$< -o $$@ $(OBJECT_DIR)/$1/$1.o: $(TEST_DIR)/$1.cc @echo "compiling $$<" "$(STDOUT)" @@ -437,7 +483,7 @@ $(OBJECT_DIR)/$1/$1 : $$($$1_OBJS) \ @echo "linking $$@" "$(STDOUT)" $(V1) mkdir -p $(dir $$@) - $(V1) $(CXX) $(CXX_FLAGS) $(PG_FLAGS) $$^ -o $$@ + $(V1) $(CXX) $(CXX_FLAGS) $(LDFLAGS) $$^ -o $$@ test_$1: $(OBJECT_DIR)/$1/$1 $(V1) $$< $$(EXEC_OPTS) "$(STDOUT)" && echo "running $$@: PASS" diff --git a/src/test/unit/atomic_unittest.cc b/src/test/unit/atomic_unittest.cc new file mode 100644 index 0000000000..39c4bf23a8 --- /dev/null +++ b/src/test/unit/atomic_unittest.cc @@ -0,0 +1,112 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 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 Cleanflight. If not, see . + */ + +#include +#include + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +extern "C" { +#include "build/atomic.h" +} + +TEST(AtomicUnittest, TestAtomicBlock) +{ + atomic_BASEPRI = 0; // reset BASEPRI + + ATOMIC_BLOCK(10) { + EXPECT_EQ(atomic_BASEPRI, 10); // locked + ATOMIC_BLOCK(5) { + EXPECT_EQ(atomic_BASEPRI, 5); // priority increase + } + EXPECT_EQ(atomic_BASEPRI, 10); // restore priority + ATOMIC_BLOCK(20) { + EXPECT_EQ(atomic_BASEPRI, 10); // lower priority, no change to value + } + EXPECT_EQ(atomic_BASEPRI, 10); // restore priority + } + EXPECT_EQ(atomic_BASEPRI, 0); // restore priority to unlocked +} + +TEST(AtomicUnittest, TestAtomicBlockNB) +{ + atomic_BASEPRI = 0; // reset BASEPRI + + ATOMIC_BLOCK_NB(10) { + EXPECT_EQ(atomic_BASEPRI, 10); // locked + ATOMIC_BLOCK_NB(5) { + EXPECT_EQ(atomic_BASEPRI, 5); // priority increase + } + EXPECT_EQ(atomic_BASEPRI, 10); // restore priority + ATOMIC_BLOCK_NB(20) { + EXPECT_EQ(atomic_BASEPRI, 10); // lower priority, no change to value + } + EXPECT_EQ(atomic_BASEPRI, 10); // restore priority + } + EXPECT_EQ(atomic_BASEPRI, 0); // restore priority to unlocked +} + +struct barrierTrace { + int enter, leave; +}; + +extern "C" { + int testAtomicBarrier_C(struct barrierTrace *b0, struct barrierTrace *b1, struct barrierTrace sample[][2]); +} + +TEST(AtomicUnittest, TestAtomicBarrier) +{ + barrierTrace b0,b1,sample[10][2]; + + int sampled = testAtomicBarrier_C(&b0,&b1,sample); + int sIdx = 0; + EXPECT_EQ(sample[sIdx][0].enter, 0); + EXPECT_EQ(sample[sIdx][0].leave, 0); + EXPECT_EQ(sample[sIdx][1].enter, 0); + EXPECT_EQ(sample[sIdx][1].leave, 0); + sIdx++; + // do { + // ATOMIC_BARRIER(*b0); + // ATOMIC_BARRIER(*b1); + EXPECT_EQ(sample[sIdx][0].enter, 1); + EXPECT_EQ(sample[sIdx][0].leave, 0); + EXPECT_EQ(sample[sIdx][1].enter, 1); + EXPECT_EQ(sample[sIdx][1].leave, 0); + sIdx++; + // do { + // ATOMIC_BARRIER(*b0); + EXPECT_EQ(sample[sIdx][0].enter, 2); + EXPECT_EQ(sample[sIdx][0].leave, 0); + EXPECT_EQ(sample[sIdx][1].enter, 1); + EXPECT_EQ(sample[sIdx][1].leave, 0); + sIdx++; + // } while(0); + EXPECT_EQ(sample[sIdx][0].enter, 2); + EXPECT_EQ(sample[sIdx][0].leave, 1); + EXPECT_EQ(sample[sIdx][1].enter, 1); + EXPECT_EQ(sample[sIdx][1].leave, 0); + sIdx++; + //} while(0); + EXPECT_EQ(sample[sIdx][0].enter, 2); + EXPECT_EQ(sample[sIdx][0].leave, 2); + EXPECT_EQ(sample[sIdx][1].enter, 1); + EXPECT_EQ(sample[sIdx][1].leave, 1); + sIdx++; + //return sIdx; + EXPECT_EQ(sIdx, sampled); +} diff --git a/src/test/unit/atomic_unittest_c.c b/src/test/unit/atomic_unittest_c.c new file mode 100644 index 0000000000..a502e0de46 --- /dev/null +++ b/src/test/unit/atomic_unittest_c.c @@ -0,0 +1,37 @@ +#include "build/atomic.h" + +struct barrierTrace { + int enter, leave; +}; + +int testAtomicBarrier_C(struct barrierTrace *b0, struct barrierTrace *b1, struct barrierTrace sample[][2]) +{ + int sIdx = 0; + +// replace barrier macros to track barrier invocation +// pass known struct as barrier variable, keep track inside it +#undef ATOMIC_BARRIER_ENTER +#undef ATOMIC_BARRIER_LEAVE +#define ATOMIC_BARRIER_ENTER(ptr, refStr) do {(ptr)->enter++; } while(0) +#define ATOMIC_BARRIER_LEAVE(ptr, refStr) do {(ptr)->leave++; } while(0) + + b0->enter = 0; b0->leave = 0; + b1->enter = 0; b1->leave = 0; + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + do { + ATOMIC_BARRIER(*b0); + ATOMIC_BARRIER(*b1); + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + do { + ATOMIC_BARRIER(*b0); + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + } while(0); + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + } while(0); + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + return sIdx; + +// ATOMIC_BARRIER is broken in rest of this file +#undef ATOMIC_BARRIER_ENTER +#undef ATOMIC_BARRIER_LEAVE +} diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index b9b42bf201..3ac929f891 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -365,7 +365,7 @@ int32_t GPS_home[2]; gyro_t gyro; -uint16_t motorOutputHigh, motorOutputLow; +float motorOutputHigh, motorOutputLow; float motor_disarmed[MAX_SUPPORTED_MOTORS]; struct pidProfile_s; struct pidProfile_s *currentPidProfile; diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index 68a1bbb5c6..34f70c81fd 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -53,7 +53,6 @@ extern "C" { void cliSet(char *cmdline); void cliGet(char *cmdline); - void *getValuePointer(const clivalue_t *value); const clivalue_t valueTable[] = { { "array_unit_test", VAR_INT8 | MODE_ARRAY | MASTER_VALUE, .config.array.length = 3, PG_RESERVED_FOR_TESTING_1, 0 } @@ -99,7 +98,7 @@ TEST(CLIUnittest, TestCliSet) }; printf("\n===============================\n"); - int8_t *data = (int8_t *)getValuePointer(&cval); + int8_t *data = (int8_t *)cliGetValuePointer(&cval); for(int i=0; i<3; i++){ printf("data[%d] = %d\n", i, data[i]); } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index f45005fc4a..b995f4c344 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -604,6 +604,68 @@ TEST(OsdTest, TestElementMahDrawn) displayPortTestBufferSubstring(1, 11, "1042%c", SYM_MAH); } +/* + * Tests the instantaneous electrical power OSD element. + */ +TEST(OsdTest, TestElementPower) +{ + // given + osdConfigMutable()->item_pos[OSD_POWER] = OSD_POS(1, 10) | VISIBLE_FLAG; + + // and + simulationBatteryVoltage = 100; // 10V + + // and + simulationBatteryAmperage = 0; // 0A + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, " 0W"); + + // given + simulationBatteryAmperage = 10; // 0.1A + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, " 1W"); + + // given + simulationBatteryAmperage = 120; // 1.2A + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, " 12W"); + + // given + simulationBatteryAmperage = 1230; // 12.3A + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, " 123W"); + + // given + simulationBatteryAmperage = 12340; // 123.4A + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, "1234W"); +} + /* * Tests the altitude OSD element. */ diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 262fe8d98f..f48b34b5ce 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -17,6 +17,8 @@ #pragma once +#include + #define USE_PARAMETER_GROUPS #define U_ID_0 0 diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 1e85421cdc..47481adffe 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -48,6 +48,8 @@ extern "C" { #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" + + #include "scheduler/scheduler.h" } #include "unittest_macros.h" @@ -703,4 +705,5 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); void resetArmingDisabled(void) {} +timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 20000; } } diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index a98ed6c9e4..1aa2e89642 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -26,14 +26,19 @@ extern "C" { #include "build/debug.h" + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" #include "common/crc.h" #include "common/utils.h" + #include "drivers/serial.h" #include "io/serial.h" #include "rx/rx.h" #include "rx/crsf.h" + #include "telemetry/msp_shared.h" + void crsfDataReceive(uint16_t c); uint8_t crsfFrameCRC(void); uint8_t crsfFrameStatus(void); @@ -44,6 +49,8 @@ extern "C" { extern uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; uint32_t dummyTimeUs; + + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); } #include "unittest_macros.h" @@ -277,7 +284,9 @@ int16_t debug[DEBUG16_VALUE_COUNT]; uint32_t micros(void) {return dummyTimeUs;} serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;} serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} -void serialWriteBuf(serialPort_t *, const uint8_t *, int) {} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} serialPort_t *telemetrySharedPort = NULL; +void crsfScheduleDeviceInfoResponse(void) {}; +void crsfScheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; +bool handleMspFrame(uint8_t *, uint8_t *) { return false; } } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc new file mode 100644 index 0000000000..2913bc6867 --- /dev/null +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -0,0 +1,300 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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 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 Cleanflight. If not, see . + */ + +#include +#include +#include + +#include +#include + +extern "C" { + #include + + #include "build/debug.h" + + #include "common/crc.h" + #include "common/utils.h" + #include "common/printf.h" + #include "common/gps_conversion.h" + #include "common/streambuf.h" + #include "common/typeconversion.h" + + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "drivers/serial.h" + #include "drivers/system.h" + + #include "fc/runtime_config.h" + #include "fc/config.h" + #include "flight/imu.h" + #include "fc/fc_msp.h" + + #include "io/serial.h" + #include "io/gps.h" + + #include "msp/msp.h" + + #include "rx/rx.h" + #include "rx/crsf.h" + + #include "sensors/battery.h" + #include "sensors/sensors.h" + + #include "telemetry/telemetry.h" + #include "telemetry/msp_shared.h" + #include "telemetry/smartport.h" + + bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); + bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); + uint8_t sbufReadU8(sbuf_t *src); + int sbufBytesRemaining(sbuf_t *buf); + void initSharedMsp(); + uint16_t testBatteryVoltage = 0; + int32_t testAmperage = 0; + uint8_t mspTxData[64]; //max frame size + sbuf_t mspTxDataBuf; + uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX]; + uint8_t payloadOutput[64]; + sbuf_t payloadOutputBuf; + int32_t testmAhDrawn = 0; + + PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); + + extern bool crsfFrameDone; + extern crsfFrame_t crsfFrame; + extern mspPackage_t mspPackage; + extern uint8_t checksum; + + uint32_t dummyTimeUs; + +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +typedef struct crsfMspFrame_s { + uint8_t deviceAddress; + uint8_t frameLength; + uint8_t type; + uint8_t destination; + uint8_t origin; + uint8_t payload[CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_CRC]; +} crsfMspFrame_t; + +const uint8_t crsfPidRequest[] = { + 0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69 +}; + +TEST(CrossFireMSPTest, RequestBufferTest) +{ + initSharedMsp(); + const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest; + crsfFrame = *(const crsfFrame_t*)framePtr; + crsfFrameDone = true; + EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress); + EXPECT_EQ(CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE, crsfFrame.frame.frameLength); + EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, crsfFrame.frame.type); + uint8_t *destination = (uint8_t *)&crsfFrame.frame.payload; + uint8_t *origin = (uint8_t *)&crsfFrame.frame.payload + 1; + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + EXPECT_EQ(0xC8, *destination); + EXPECT_EQ(0xEA, *origin); + EXPECT_EQ(0x30, *frameStart); + EXPECT_EQ(0x69, *frameEnd); +} + +TEST(CrossFireMSPTest, ResponsePacketTest) +{ + initSharedMsp(); + const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest; + crsfFrame = *(const crsfFrame_t*)framePtr; + crsfFrameDone = true; + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + handleMspFrame(frameStart, frameEnd); + for (unsigned int ii=1; ii<30; ii++) { + EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf)); + } + sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); +} + +const uint8_t crsfPidWrite1[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32}; +const uint8_t crsfPidWrite2[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00}; +const uint8_t crsfPidWrite3[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19}; +const uint8_t crsfPidWrite4[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00}; +const uint8_t crsfPidWrite5[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00}; + +TEST(CrossFireMSPTest, WriteResponseTest) +{ + initSharedMsp(); + const crsfMspFrame_t *framePtr1 = (const crsfMspFrame_t*)crsfPidWrite1; + crsfFrame = *(const crsfFrame_t*)framePtr1; + crsfFrameDone = true; + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + bool pending1 = handleMspFrame(frameStart, frameEnd); + EXPECT_FALSE(pending1); // not done yet*/ + EXPECT_EQ(0x29, mspPackage.requestBuffer[0]); + EXPECT_EQ(0x28, mspPackage.requestBuffer[1]); + EXPECT_EQ(0x1E, mspPackage.requestBuffer[2]); + EXPECT_EQ(0x3A, mspPackage.requestBuffer[3]); + EXPECT_EQ(0x32, mspPackage.requestBuffer[4]); + + const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2; + crsfFrame = *(const crsfFrame_t*)framePtr2; + crsfFrameDone = true; + uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + bool pending2 = handleMspFrame(frameStart2, frameEnd2); + EXPECT_FALSE(pending2); // not done yet + EXPECT_EQ(0x23, mspPackage.requestBuffer[5]); + EXPECT_EQ(0x46, mspPackage.requestBuffer[6]); + EXPECT_EQ(0x2D, mspPackage.requestBuffer[7]); + EXPECT_EQ(0x14, mspPackage.requestBuffer[8]); + EXPECT_EQ(0x32, mspPackage.requestBuffer[9]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[10]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[11]); + + const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3; + crsfFrame = *(const crsfFrame_t*)framePtr3; + crsfFrameDone = true; + uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; + bool pending3 = handleMspFrame(frameStart3, frameEnd3); + EXPECT_FALSE(pending3); // not done yet + EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[13]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[14]); + EXPECT_EQ(0x22, mspPackage.requestBuffer[15]); + EXPECT_EQ(0x0E, mspPackage.requestBuffer[16]); + EXPECT_EQ(0x35, mspPackage.requestBuffer[17]); + EXPECT_EQ(0x19, mspPackage.requestBuffer[18]); + + const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4; + crsfFrame = *(const crsfFrame_t*)framePtr4; + crsfFrameDone = true; + uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; + bool pending4 = handleMspFrame(frameStart4, frameEnd4); + EXPECT_FALSE(pending4); // not done yet + EXPECT_EQ(0x21, mspPackage.requestBuffer[19]); + EXPECT_EQ(0x53, mspPackage.requestBuffer[20]); + EXPECT_EQ(0x32, mspPackage.requestBuffer[21]); + EXPECT_EQ(0x32, mspPackage.requestBuffer[22]); + EXPECT_EQ(0x4B, mspPackage.requestBuffer[23]); + EXPECT_EQ(0x28, mspPackage.requestBuffer[24]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[25]); + //EXPECT_EQ(0xB3,checksum); + + const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5; + crsfFrame = *(const crsfFrame_t*)framePtr5; + crsfFrameDone = true; + uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; + bool pending5 = handleMspFrame(frameStart5, frameEnd5); + EXPECT_TRUE(pending5); // not done yet + EXPECT_EQ(0x00, mspPackage.requestBuffer[26]); + EXPECT_EQ(0x37, mspPackage.requestBuffer[27]); + EXPECT_EQ(0x37, mspPackage.requestBuffer[28]); + EXPECT_EQ(0x4B, mspPackage.requestBuffer[29]); + EXPECT_EQ(0xF8,checksum); + +} + +void testSendMspResponse(uint8_t *payload) { + sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64); + sbufWriteData(plOut, payload, *payload + 64); + sbufSwitchToReader(&payloadOutputBuf, payloadOutput); +} + +TEST(CrossFireMSPTest, SendMspReply) { + initSharedMsp(); + const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest; + crsfFrame = *(const crsfFrame_t*)framePtr; + crsfFrameDone = true; + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + bool handled = handleMspFrame(frameStart, frameEnd); + EXPECT_TRUE(handled); + bool replyPending = sendMspReply(64, &testSendMspResponse); + EXPECT_FALSE(replyPending); + EXPECT_EQ(0x10, sbufReadU8(&payloadOutputBuf)); + EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf)); + for (unsigned int ii=1; ii<=30; ii++) { + EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf)); + } + EXPECT_EQ(0x71, sbufReadU8(&payloadOutputBuf)); // CRC +} + +// STUBS + +extern "C" { + + gpsSolutionData_t gpsSol; + attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; + + uint32_t micros(void) {return dummyTimeUs;} + serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;} + serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} + uint16_t getBatteryVoltage(void) { + return testBatteryVoltage; + } + int32_t getAmperage(void) { + return testAmperage; + } + + uint8_t calculateBatteryPercentageRemaining(void) { + return 67; + } + + bool feature(uint32_t) {return false;} + + bool isAirmodeActive(void) {return true;} + + mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { + + UNUSED(mspPostProcessFn); + + sbuf_t *dst = &reply->buf; + //sbuf_t *src = &cmd->buf; + const uint8_t cmdMSP = cmd->cmd; + reply->cmd = cmd->cmd; + + if (cmdMSP == 0x70) { + for (unsigned int ii=1; ii<=30; ii++) { + sbufWriteU8(dst, ii); + } + } else if (cmdMSP == 0xCA) { + return MSP_RESULT_ACK; + } + + return MSP_RESULT_ACK; + } + + void beeperConfirmationBeeps(uint8_t ) {} + + int32_t getMAhDrawn(void) { + return testmAhDrawn; + } + +} diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 947be24296..50f84bf393 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -31,6 +31,8 @@ extern "C" { #include "common/filter.h" #include "common/gps_conversion.h" #include "common/maths.h" + #include "common/printf.h" + #include "common/typeconversion.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -38,6 +40,7 @@ extern "C" { #include "drivers/serial.h" #include "drivers/system.h" + #include "fc/config.h" #include "fc/runtime_config.h" #include "flight/pid.h" @@ -46,6 +49,7 @@ extern "C" { #include "io/gps.h" #include "io/serial.h" + #include "rx/rx.h" #include "rx/crsf.h" #include "sensors/battery.h" @@ -53,15 +57,19 @@ extern "C" { #include "telemetry/crsf.h" #include "telemetry/telemetry.h" + #include "telemetry/msp_shared.h" bool airMode; uint16_t testBatteryVoltage = 0; int32_t testAmperage = 0; + int32_t testmAhDrawn = 0; serialPort_t *telemetrySharedPort; PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); } #include "unittest_macros.h" @@ -91,7 +99,7 @@ TEST(TelemetryCrsfTest, TestGPS) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; - int frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); + int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS); EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(17, frame[1]); // length @@ -117,7 +125,7 @@ TEST(TelemetryCrsfTest, TestGPS) gpsSol.groundSpeed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 gpsSol.numSat = 9; gpsSol.groundCourse = 1479; // degrees * 10 - frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS); lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6]; EXPECT_EQ(560000000, lattitude); longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10]; @@ -138,7 +146,7 @@ TEST(TelemetryCrsfTest, TestBattery) uint8_t frame[CRSF_FRAME_SIZE_MAX]; testBatteryVoltage = 0; // 0.1V units - int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); + int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR); EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(10, frame[1]); // length @@ -155,8 +163,8 @@ TEST(TelemetryCrsfTest, TestBattery) testBatteryVoltage = 33; // 3.3V = 3300 mv testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps - batteryConfigMutable()->batteryCapacity = 1234; - frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); + testmAhDrawn = 1234; + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR); voltage = frame[3] << 8 | frame[4]; // mV * 100 EXPECT_EQ(33, voltage); current = frame[5] << 8 | frame[6]; // mA * 100 @@ -175,7 +183,7 @@ TEST(TelemetryCrsfTest, TestAttitude) attitude.values.pitch = 0; attitude.values.roll = 0; attitude.values.yaw = 0; - int frameLen = getCrsfFrame(frame, CRSF_FRAME_ATTITUDE); + int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_ATTITUDE); EXPECT_EQ(CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(8, frame[1]); // length @@ -191,7 +199,7 @@ TEST(TelemetryCrsfTest, TestAttitude) attitude.values.pitch = 678; // decidegrees == 1.183333232852155 rad attitude.values.roll = 1495; // 2.609267231731523 rad attitude.values.yaw = -1799; //3.139847324337799 rad - frameLen = getCrsfFrame(frame, CRSF_FRAME_ATTITUDE); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_ATTITUDE); pitch = frame[3] << 8 | frame[4]; // rad / 10000 EXPECT_EQ(11833, pitch); roll = frame[5] << 8 | frame[6]; @@ -207,7 +215,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) // nothing set, so ACRO mode airMode = false; - int frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(7, frame[1]); // length @@ -222,7 +230,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) enableFlightMode(ANGLE_MODE); EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE)); - frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(7, frame[1]); // length @@ -237,7 +245,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) disableFlightMode(ANGLE_MODE); enableFlightMode(HORIZON_MODE); EXPECT_EQ(HORIZON_MODE, FLIGHT_MODE(HORIZON_MODE)); - frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(6, frame[1]); // length @@ -250,7 +258,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) disableFlightMode(HORIZON_MODE); airMode = true; - frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(6, frame[1]); // length @@ -292,6 +300,7 @@ void serialWriteBuf(serialPort_t *, const uint8_t *, int) {} void serialSetMode(serialPort_t *, portMode_e) {} serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;} void closeSerialPort(serialPort_t *) {} +bool isSerialTransmitBufferEmpty(const serialPort_t *) { return true; } serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;} @@ -318,4 +327,11 @@ uint8_t calculateBatteryPercentageRemaining(void) { return 67; } +int32_t getMAhDrawn(void){ + return testmAhDrawn; +} + +bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; } +bool handleMspFrame(uint8_t *, uint8_t *) { return false; } + }