From a13ee14bbb4eb5339682b04c1d83247bbd5ba516 Mon Sep 17 00:00:00 2001 From: Bertrand Songis Date: Tue, 2 Apr 2019 20:12:50 +0200 Subject: [PATCH] [X-LiteS] Gyro added --- radio/src/dataconstants.h | 9 +- radio/src/gui/128x64/lcd.cpp | 5 + radio/src/gui/128x64/model_module_options.cpp | 24 +- .../gui/common/stdlcd/radio_power_meter.cpp | 3 +- .../common/stdlcd/radio_spectrum_analyser.cpp | 3 +- radio/src/gui/common/stdlcd/radio_tools.cpp | 62 +--- radio/src/gui/gui_common_arm.cpp | 28 +- radio/src/gyro.cpp | 70 ++++ radio/src/gyro.h | 50 +++ radio/src/mixer.cpp | 22 +- radio/src/opentx.h | 4 + radio/src/pulses/modules.h | 31 ++ .../common/arm/stm32/lsm6ds3_driver.cpp | 338 +++++++++--------- radio/src/targets/simu/simpgmspace.cpp | 94 +++-- radio/src/targets/taranis/CMakeLists.txt | 18 +- radio/src/targets/taranis/board.cpp | 5 + radio/src/targets/taranis/board.h | 6 + radio/src/tasks.cpp | 14 +- 18 files changed, 468 insertions(+), 318 deletions(-) create mode 100644 radio/src/gyro.cpp create mode 100644 radio/src/gyro.h diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index b8ac92e12..e32ae241d 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -566,8 +566,8 @@ enum MixSources { #else MIXSRC_P1 = MIXSRC_FIRST_POT, MIXSRC_P2, - MIXSRC_P3, - MIXSRC_LAST_POT = MIXSRC_P3, + MIXSRC_P3, + MIXSRC_LAST_POT = MIXSRC_P3, #endif #if defined(PCBHORUS) @@ -575,6 +575,11 @@ enum MixSources { MIXSRC_MOUSE2, LUA_EXPORT("jsy", "Joystick Y") #endif +#if defined(GYRO) + MIXSRC_GYRO1, LUA_EXPORT("gyr1", "Gyro X") + MIXSRC_GYRO2, LUA_EXPORT("gyr2", "Gyro Y") +#endif + #if defined(PCBSKY9X) MIXSRC_REa, MIXSRC_LAST_ROTARY_ENCODER = MIXSRC_REa, diff --git a/radio/src/gui/128x64/lcd.cpp b/radio/src/gui/128x64/lcd.cpp index 100d59334..30411c0ce 100644 --- a/radio/src/gui/128x64/lcd.cpp +++ b/radio/src/gui/128x64/lcd.cpp @@ -723,6 +723,11 @@ void drawSource(coord_t x, coord_t y, uint32_t idx, LcdFlags att) lcdDrawTextAtIndex(x, y, STR_VSRCRAW, idx + 1, att); } } +#if defined(GYRO) + else if (idx <= MIXSRC_GYRO2) { + drawStringWithIndex(x, y, "Gyr", idx - MIXSRC_GYRO1 + 1, att); + } +#endif else if (idx >= MIXSRC_FIRST_SWITCH && idx <= MIXSRC_LAST_SWITCH) { idx = idx-MIXSRC_FIRST_SWITCH; if (ZEXIST(g_eeGeneral.switchNames[idx])) { diff --git a/radio/src/gui/128x64/model_module_options.cpp b/radio/src/gui/128x64/model_module_options.cpp index 8116654ca..0191f092c 100644 --- a/radio/src/gui/128x64/model_module_options.cpp +++ b/radio/src/gui/128x64/model_module_options.cpp @@ -45,23 +45,7 @@ enum { ITEM_MODULE_SETTINGS_COUNT }; -/* Options order: - * - RF Protocol (0x01) - * - External antenna (0x02) - * - Power (0x04) - */ -const uint8_t moduleOptions[] = { - 0b11111111, // None = display all options - 0b11111011, // XJT - 0b11111011, // IXJT - 0b11111011, // IXJT-PRO - 0b11111011, // IXJT-S - 0b11111100, // R9M - 0b11111100, // R9MLite - 0b11111100, // R9MLite-PRO -}; - -#define IF_MODULE_OPTIONS(option, count) uint8_t((moduleOptions[modelId] & (1 << option)) ? count : HIDDEN_ROW) +#define IF_MODULE_OPTIONS(option, count) uint8_t(isModuleOptionAvailable(modelId, option) ? count : HIDDEN_ROW) void drawPower(coord_t x, coord_t y, int8_t dBm) { @@ -123,9 +107,9 @@ void menuModelModuleOptions(event_t event) // uint8_t variant = reusableBuffer.hardwareAndSettings.modules[g_moduleIdx].information.variant; SUBMENU_NOTITLE(ITEM_MODULE_SETTINGS_COUNT, { - IF_MODULE_OPTIONS(0, 0), - IF_MODULE_OPTIONS(1, 0), - IF_MODULE_OPTIONS(2, 0), + IF_MODULE_OPTIONS(MODULE_OPTION_RF_PROTOCOL, 0), + IF_MODULE_OPTIONS(MODULE_OPTION_EXTERNAL_ANTENNA, 0), + IF_MODULE_OPTIONS(MODULE_OPTION_POWER, 0), }); if (event == EVT_ENTRY) { diff --git a/radio/src/gui/common/stdlcd/radio_power_meter.cpp b/radio/src/gui/common/stdlcd/radio_power_meter.cpp index 1d04c6eef..7bfe6e33c 100644 --- a/radio/src/gui/common/stdlcd/radio_power_meter.cpp +++ b/radio/src/gui/common/stdlcd/radio_power_meter.cpp @@ -21,14 +21,13 @@ #include #include "opentx.h" -void pxx2ModuleRequiredScreen(event_t event); extern uint8_t g_moduleIdx; void menuRadioPowerMeter(event_t event) { if (TELEMETRY_STREAMING()) { lcdDrawCenteredText(LCD_H/2, "Turn off receiver"); - if(event == EVT_KEY_FIRST(KEY_EXIT)) { + if (event == EVT_KEY_FIRST(KEY_EXIT)) { killEvents(event); popMenu(); } diff --git a/radio/src/gui/common/stdlcd/radio_spectrum_analyser.cpp b/radio/src/gui/common/stdlcd/radio_spectrum_analyser.cpp index 6c7e7e44a..292998ff4 100644 --- a/radio/src/gui/common/stdlcd/radio_spectrum_analyser.cpp +++ b/radio/src/gui/common/stdlcd/radio_spectrum_analyser.cpp @@ -20,14 +20,13 @@ #include "opentx.h" -extern void pxx2ModuleRequiredScreen(event_t event); extern uint8_t g_moduleIdx; void menuRadioSpectrumAnalyser(event_t event) { if (TELEMETRY_STREAMING()) { lcdDrawCenteredText(15, "Turn off receiver"); - if(event == EVT_KEY_FIRST(KEY_EXIT)) { + if (event == EVT_KEY_FIRST(KEY_EXIT)) { killEvents(event); popMenu(); } diff --git a/radio/src/gui/common/stdlcd/radio_tools.cpp b/radio/src/gui/common/stdlcd/radio_tools.cpp index dbf098b6a..7421f3d43 100644 --- a/radio/src/gui/common/stdlcd/radio_tools.cpp +++ b/radio/src/gui/common/stdlcd/radio_tools.cpp @@ -22,19 +22,6 @@ extern uint8_t g_moduleIdx; -void pxx2ModuleRequiredScreen(event_t event) -{ - lcdClear(); - lcdDrawCenteredText(15, "THIS FEATURE REQUIRES"); - lcdDrawCenteredText(30, "ACCESS UPGRADE ON"); - lcdDrawCenteredText(45, "YOUR INTERNAL MODULE"); - - if (event == EVT_KEY_FIRST(KEY_EXIT)) { - killEvents(event); - popMenu(); - } -} - void addRadioTool(uint8_t index, const char * label, void (* tool)(event_t event), uint8_t module, event_t event) { int8_t sub = menuVerticalPosition - HEADER_LINE; @@ -49,64 +36,43 @@ void addRadioTool(uint8_t index, const char * label, void (* tool)(event_t event } } -bool hasSpektrumAnalyserCapability(uint8_t module) -{ -#if defined(SIMU) - return true; -#else - return (reusableBuffer.hardwareAndSettings.modules[module].information.modelID == PXX2_MODULE_IXJT_S || - reusableBuffer.hardwareAndSettings.modules[module].information.modelID == PXX2_MODULE_IXJT_PRO || - reusableBuffer.hardwareAndSettings.modules[module].information.modelID >= PXX2_MODULE_R9M_LITE_PRO); -#endif -} - -bool hasPowerMeterCapablity(uint8_t module) -{ -#if defined(SIMU) - return true; -#else - return (reusableBuffer.hardwareAndSettings.modules[module].information.modelID == PXX2_MODULE_IXJT_PRO || - reusableBuffer.hardwareAndSettings.modules[module].information.modelID == PXX2_MODULE_R9M_LITE_PRO); -#endif -} - void menuRadioTools(event_t event) { uint8_t spektrum_modules = 0, power_modules = 0; - if(event == EVT_ENTRY || event == EVT_ENTRY_UP) { + if (event == EVT_ENTRY || event == EVT_ENTRY_UP) { memclear(&reusableBuffer.hardwareAndSettings, sizeof(reusableBuffer.hardwareAndSettings)); #if defined(PXX2) - for (uint8_t idx=0; idx < NUM_MODULES; idx++) { - if (isModulePXX2(idx) && (idx == INTERNAL_MODULE ? IS_INTERNAL_MODULE_ON() : IS_EXTERNAL_MODULE_ON())) { - reusableBuffer.hardwareAndSettings.modules[idx].current = PXX2_HW_INFO_TX_ID; - reusableBuffer.hardwareAndSettings.modules[idx].maximum = PXX2_HW_INFO_TX_ID; - moduleSettings[idx].mode = MODULE_MODE_GET_HARDWARE_INFO; + for (uint8_t module = 0; module < NUM_MODULES; module++) { + if (isModulePXX2(module) && (module == INTERNAL_MODULE ? IS_INTERNAL_MODULE_ON() : IS_EXTERNAL_MODULE_ON())) { + reusableBuffer.hardwareAndSettings.modules[module].current = PXX2_HW_INFO_TX_ID; + reusableBuffer.hardwareAndSettings.modules[module].maximum = PXX2_HW_INFO_TX_ID; + moduleSettings[module].mode = MODULE_MODE_GET_HARDWARE_INFO; } } #endif } - for (uint8_t idx=0; idx < NUM_MODULES; idx++) { - if(hasSpektrumAnalyserCapability(idx)) { + for (uint8_t module = 0; module < NUM_MODULES; module++) { + if (isModuleOptionAvailable(module, MODULE_OPTION_SPEKTRUM_ANALYSER)) { spektrum_modules++; } - if(hasPowerMeterCapablity(idx)) { + if (isModuleOptionAvailable(module, MODULE_OPTION_POWER_METER)) { power_modules++; } } SIMPLE_MENU("TOOLS", menuTabGeneral, MENU_RADIO_TOOLS, HEADER_LINE + spektrum_modules + power_modules); - uint8_t menu_index=0; + uint8_t menu_index = 0; #if defined(PXX2) - if(hasSpektrumAnalyserCapability(INTERNAL_MODULE)) + if (isModuleOptionAvailable(INTERNAL_MODULE, MODULE_OPTION_SPEKTRUM_ANALYSER)) addRadioTool(menu_index++, "Spectrum (INT)", menuRadioSpectrumAnalyser, INTERNAL_MODULE, event); - if(hasPowerMeterCapablity(INTERNAL_MODULE)) + if (isModuleOptionAvailable(INTERNAL_MODULE, MODULE_OPTION_POWER_METER)) addRadioTool(menu_index++, "Power Meter (INT)", menuRadioPowerMeter, INTERNAL_MODULE, event); - if(hasSpektrumAnalyserCapability(EXTERNAL_MODULE)) + if (isModuleOptionAvailable(EXTERNAL_MODULE, MODULE_OPTION_SPEKTRUM_ANALYSER)) addRadioTool(menu_index++, "Spectrum (EXT)", menuRadioSpectrumAnalyser, EXTERNAL_MODULE, event); - if(hasPowerMeterCapablity(EXTERNAL_MODULE)) + if (isModuleOptionAvailable(EXTERNAL_MODULE, MODULE_OPTION_POWER_METER)) addRadioTool(menu_index++, "Power Meter (EXT)", menuRadioPowerMeter, EXTERNAL_MODULE, event); #endif } \ No newline at end of file diff --git a/radio/src/gui/gui_common_arm.cpp b/radio/src/gui/gui_common_arm.cpp index 49281630c..278da5772 100644 --- a/radio/src/gui/gui_common_arm.cpp +++ b/radio/src/gui/gui_common_arm.cpp @@ -224,32 +224,36 @@ bool isSourceAvailableInCustomSwitches(int source) bool isInputSourceAvailable(int source) { - if (source>=MIXSRC_FIRST_POT && source<=MIXSRC_LAST_POT) { + if (source >= MIXSRC_FIRST_POT && source <= MIXSRC_LAST_POT) return IS_POT_SLIDER_AVAILABLE(POT1+source-MIXSRC_FIRST_POT); - } - if (source>=MIXSRC_Rud && source<=MIXSRC_MAX) +#if defined(GYRO) + if (source >= MIXSRC_GYRO1 && source <= MIXSRC_GYRO2) + return true; +#endif + + if (source >= MIXSRC_Rud && source <= MIXSRC_MAX) return true; - if (source>=MIXSRC_FIRST_TRIM && source<=MIXSRC_LAST_TRIM) + if (source >= MIXSRC_FIRST_TRIM && source <= MIXSRC_LAST_TRIM) return true; - if (source>=MIXSRC_FIRST_SWITCH && source<=MIXSRC_LAST_SWITCH) - return SWITCH_EXISTS(source-MIXSRC_FIRST_SWITCH); + if (source >= MIXSRC_FIRST_SWITCH && source <= MIXSRC_LAST_SWITCH) + return SWITCH_EXISTS(source - MIXSRC_FIRST_SWITCH); - if (source>=MIXSRC_FIRST_CH && source<=MIXSRC_LAST_CH) + if (source >= MIXSRC_FIRST_CH && source <= MIXSRC_LAST_CH) return true; - if (source>=MIXSRC_FIRST_LOGICAL_SWITCH && source<=MIXSRC_LAST_LOGICAL_SWITCH) { - LogicalSwitchData * cs = lswAddress(source-MIXSRC_SW1); + if (source >= MIXSRC_FIRST_LOGICAL_SWITCH && source <= MIXSRC_LAST_LOGICAL_SWITCH) { + LogicalSwitchData * cs = lswAddress(source - MIXSRC_SW1); return (cs->func != LS_FUNC_NONE); } - if (source>=MIXSRC_FIRST_TRAINER && source<=MIXSRC_LAST_TRAINER) + if (source >= MIXSRC_FIRST_TRAINER && source <= MIXSRC_LAST_TRAINER) return true; - if (source>=MIXSRC_FIRST_TELEM && source<=MIXSRC_LAST_TELEM) { - div_t qr = div(source-MIXSRC_FIRST_TELEM, 3); + if (source >= MIXSRC_FIRST_TELEM && source <= MIXSRC_LAST_TELEM) { + div_t qr = div(source - MIXSRC_FIRST_TELEM, 3); return isTelemetryFieldAvailable(qr.quot) && isTelemetryFieldComparisonAvailable(qr.quot); } diff --git a/radio/src/gyro.cpp b/radio/src/gyro.cpp new file mode 100644 index 000000000..121735561 --- /dev/null +++ b/radio/src/gyro.cpp @@ -0,0 +1,70 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program 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. + */ + +#include "opentx.h" + +#define ACC_LSB_VALUE 0.000488 // 0.488 mg/LSB + +Gyro gyro; + +void GyroBuffer::read(int32_t values[GYRO_SAMPLES_COUNT]) +{ + for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) { + sums[index] -= samples[index].values[i]; + } + + index = (index + 1) & (GYRO_SAMPLES_COUNT - 1); + + gyroRead(samples[index].raw); + + for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) { + sums[index] += samples[index].values[i]; + values[i] = sums[index] >> GYRO_SAMPLES_EXPONENT; + } +} + +float rad2RESX(float rad) +{ + return (rad * float(RESX)) / M_PI; +} + +void Gyro::wakeup() +{ + static tmr10ms_t gyroWakeupTime = 0; + + tmr10ms_t now = get_tmr10ms(); + if (now < gyroWakeupTime) + return; + + gyroWakeupTime = now + 1; /* 10ms default */ + + int32_t values[GYRO_VALUES_COUNT]; + gyroBuffer.read(values); + + float accValues[3]; // m^2 / s + accValues[0] = -9.81 * float(values[3]) * ACC_LSB_VALUE; + accValues[1] = 9.81 * float(values[4]) * ACC_LSB_VALUE; + accValues[2] = 9.81 * float(values[5]) * ACC_LSB_VALUE; + + outputs[0] = rad2RESX(atan2f(accValues[1], accValues[2])); + outputs[1] = rad2RESX(atan2f(-accValues[0], accValues[2])); + + // TRACE("%d %d", values[0], values[1]); +} diff --git a/radio/src/gyro.h b/radio/src/gyro.h new file mode 100644 index 000000000..db29b0836 --- /dev/null +++ b/radio/src/gyro.h @@ -0,0 +1,50 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program 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. + */ + +#include + +#define GYRO_SAMPLES_EXPONENT 3 +#define GYRO_SAMPLES_COUNT (2 ^ GYRO_SAMPLES_EXPONENT) + +class GyroBuffer { + protected: + union { + int16_t values[GYRO_VALUES_COUNT]; + uint8_t raw[GYRO_BUFFER_LENGTH]; + } samples[GYRO_SAMPLES_COUNT]; + + int32_t sums[GYRO_VALUES_COUNT]; + + uint8_t index; + + public: + void read(int32_t values[GYRO_SAMPLES_COUNT]); +}; + +class Gyro { + protected: + GyroBuffer gyroBuffer; + + public: + int32_t outputs[2]; + void wakeup(); +}; + +extern Gyro gyro; diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index edfa31aca..d42e5b01d 100644 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -301,7 +301,7 @@ getvalue_t getValue(mixsrc_t i) return anas[i-MIXSRC_FIRST_INPUT]; } #if defined(LUA_INPUTS) - else if (i < MIXSRC_LAST_LUA) { + else if (i <= MIXSRC_LAST_LUA) { #if defined(LUA_MODEL_SCRIPTS) div_t qr = div(i-MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS); return scriptInputsOutputs[qr.quot].outputs[qr.rem].value; @@ -311,19 +311,19 @@ getvalue_t getValue(mixsrc_t i) } #endif -#if defined(LUA_INPUTS) else if (i <= MIXSRC_LAST_POT+NUM_MOUSE_ANALOGS) { - return calibratedAnalogs[i-MIXSRC_Rud]; + return calibratedAnalogs[i - MIXSRC_Rud]; } -#else - else if (i>=MIXSRC_FIRST_STICK && i<=MIXSRC_LAST_POT+NUM_MOUSE_ANALOGS) { - return calibratedAnalogs[i-MIXSRC_Rud]; + +#if defined(GYRO) + else if (i <= MIXSRC_GYRO2) { + return gyro.outputs[i - MIXSRC_GYRO1]; } #endif -#if defined(PCBGRUVIN9X) || defined(PCBMEGA2560) || defined(ROTARY_ENCODERS) +#if defined(PCBSKY9X) else if (i <= MIXSRC_LAST_ROTARY_ENCODER) { - return getRotaryEncoder(i-MIXSRC_REa); + return getRotaryEncoder(i - MIXSRC_REa); } #endif @@ -377,11 +377,13 @@ getvalue_t getValue(mixsrc_t i) return ex_chans[i-MIXSRC_CH1]; } -#if defined(GVARS) else if (i <= MIXSRC_LAST_GVAR) { +#if defined(GVARS) return GVAR_VALUE(i-MIXSRC_GVAR1, getGVarFlightMode(mixerCurrentFlightMode, i - MIXSRC_GVAR1)); - } +#else + return 0; #endif + } else if (i == MIXSRC_TX_VOLTAGE) { return g_vbat100mV; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index acfc457e4..238d9afab 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -1380,4 +1380,8 @@ enum JackMode { }; #endif +#if defined(GYRO) +#include "gyro.h" +#endif + #endif // _OPENTX_H_ diff --git a/radio/src/pulses/modules.h b/radio/src/pulses/modules.h index d73f367dd..a91ff04f6 100644 --- a/radio/src/pulses/modules.h +++ b/radio/src/pulses/modules.h @@ -232,4 +232,35 @@ inline int8_t sentModuleChannels(uint8_t idx) return 8 + g_model.moduleData[idx].channelsCount; } +enum { + MODULE_OPTION_RF_PROTOCOL, + MODULE_OPTION_EXTERNAL_ANTENNA, + MODULE_OPTION_POWER, + MODULE_OPTION_SPEKTRUM_ANALYSER, + MODULE_OPTION_POWER_METER, +}; + +/* Options order: + * - RF Protocol (0x01) + * - External antenna (0x02) + * - Power (0x04) + * - Spektrum analyser (0x08) + * - Power meter (0x10) + */ +static const uint8_t moduleOptions[] = { + 0b11111111, // None = display all options + 0b11100011, // XJT + 0b11100011, // IXJT + 0b11111011, // IXJT-PRO + 0b11101011, // IXJT-S + 0b11100100, // R9M + 0b11100100, // R9MLite + 0b11111100, // R9MLite-PRO +}; + +inline bool isModuleOptionAvailable(uint8_t modelId, uint8_t option) +{ + return moduleOptions[modelId] & (1 << option); +} + #endif // _MODULES_H_ diff --git a/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp b/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp index 8b8699b41..174499b1a 100644 --- a/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp +++ b/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp @@ -20,180 +20,175 @@ #include "opentx.h" -//#define ACC_LSB_VALUE 0.000488 // 0.488 mg/LSB -//#define GYRO_LSB_VALUE 0.07 // 70mDPS in 2000 deg/second /* COMMON VALUES FOR ACCEL-GYRO SENSORS */ -#define LSM6DS3_WHO_AM_I 0x0f -#define LSM6DS3_WHO_AM_I_DEF 0x69 -#define LSM6DS3_AXIS_EN_MASK 0x38 -#define LSM6DS3_INT1_CTRL_ADDR 0x0d -#define LSM6DS3_INT2_CTRL_ADDR 0x0e +#define LSM6DS3_WHO_AM_I 0x0f +#define LSM6DS3_WHO_AM_I_DEF 0x69 +#define LSM6DS3_AXIS_EN_MASK 0x38 +#define LSM6DS3_INT1_CTRL_ADDR 0x0d +#define LSM6DS3_INT2_CTRL_ADDR 0x0e #define LSM6DS3_INT1_FULL 0x20 -#define LSM6DS3_INT1_FTH 0x08 -#define LSM6DS3_MD1_ADDR 0x5e -#define LSM6DS3_ODR_LIST_NUM 5 +#define LSM6DS3_INT1_FTH 0x08 +#define LSM6DS3_MD1_ADDR 0x5e +#define LSM6DS3_ODR_LIST_NUM 5 #define LSM6DS3_ODR_POWER_OFF_VAL 0x00 -#define LSM6DS3_ODR_26HZ_VAL 0x02 -#define LSM6DS3_ODR_52HZ_VAL 0x03 -#define LSM6DS3_ODR_104HZ_VAL 0x04 -#define LSM6DS3_ODR_208HZ_VAL 0x05 -#define LSM6DS3_ODR_416HZ_VAL 0x06 -#define LSM6DS3_FS_LIST_NUM 4 -#define LSM6DS3_BDU_ADDR 0x12 -#define LSM6DS3_BDU_MASK 0x40 -#define LSM6DS3_EN_BIT 0x01 -#define LSM6DS3_DIS_BIT 0x00 -#define LSM6DS3_FUNC_EN_ADDR 0x19 -#define LSM6DS3_FUNC_EN_MASK 0x04 -#define LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01 -#define LSM6DS3_FUNC_CFG_ACCESS_MASK 0x01 -#define LSM6DS3_FUNC_CFG_ACCESS_MASK2 0x04 -#define LSM6DS3_FUNC_CFG_REG2_MASK 0x80 -#define LSM6DS3_FUNC_CFG_START1_ADDR 0x62 -#define LSM6DS3_FUNC_CFG_START2_ADDR 0x63 -#define LSM6DS3_SELFTEST_ADDR 0x14 -#define LSM6DS3_SELFTEST_ACCEL_MASK 0x03 -#define LSM6DS3_SELFTEST_GYRO_MASK 0x0c -#define LSM6DS3_SELF_TEST_DISABLED_VAL 0x00 -#define LSM6DS3_SELF_TEST_POS_SIGN_VAL 0x01 -#define LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02 -#define LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03 -#define LSM6DS3_LIR_ADDR 0x58 -#define LSM6DS3_LIR_MASK 0x01 -#define LSM6DS3_TIMER_EN_ADDR 0x58 -#define LSM6DS3_TIMER_EN_MASK 0x80 +#define LSM6DS3_ODR_26HZ_VAL 0x02 +#define LSM6DS3_ODR_52HZ_VAL 0x03 +#define LSM6DS3_ODR_104HZ_VAL 0x04 +#define LSM6DS3_ODR_208HZ_VAL 0x05 +#define LSM6DS3_ODR_416HZ_VAL 0x06 +#define LSM6DS3_FS_LIST_NUM 4 +#define LSM6DS3_BDU_ADDR 0x12 +#define LSM6DS3_BDU_MASK 0x40 +#define LSM6DS3_EN_BIT 0x01 +#define LSM6DS3_DIS_BIT 0x00 +#define LSM6DS3_FUNC_EN_ADDR 0x19 +#define LSM6DS3_FUNC_EN_MASK 0x04 +#define LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01 +#define LSM6DS3_FUNC_CFG_ACCESS_MASK 0x01 +#define LSM6DS3_FUNC_CFG_ACCESS_MASK2 0x04 +#define LSM6DS3_FUNC_CFG_REG2_MASK 0x80 +#define LSM6DS3_FUNC_CFG_START1_ADDR 0x62 +#define LSM6DS3_FUNC_CFG_START2_ADDR 0x63 +#define LSM6DS3_SELFTEST_ADDR 0x14 +#define LSM6DS3_SELFTEST_ACCEL_MASK 0x03 +#define LSM6DS3_SELFTEST_GYRO_MASK 0x0c +#define LSM6DS3_SELF_TEST_DISABLED_VAL 0x00 +#define LSM6DS3_SELF_TEST_POS_SIGN_VAL 0x01 +#define LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02 +#define LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03 +#define LSM6DS3_LIR_ADDR 0x58 +#define LSM6DS3_LIR_MASK 0x01 +#define LSM6DS3_TIMER_EN_ADDR 0x58 +#define LSM6DS3_TIMER_EN_MASK 0x80 #define LSM6DS3_PEDOMETER_EN_ADDR 0x58 #define LSM6DS3_PEDOMETER_EN_MASK 0x40 #define LSM6DS3_INT2_ON_INT1_ADDR 0x13 #define LSM6DS3_INT2_ON_INT1_MASK 0x20 -#define LSM6DS3_MIN_DURATION_MS 1638 -#define LSM6DS3_ROUNDING_ADDR 0x16 -#define LSM6DS3_ROUNDING_MASK 0x04 -#define LSM6DS3_FIFO_MODE_ADDR 0x0a -#define LSM6DS3_FIFO_MODE_MASK 0x07 -#define LSM6DS3_FIFO_MODE_BYPASS 0x00 -#define LSM6DS3_FIFO_MODE_CONTINUOS 0x06 -#define LSM6DS3_FIFO_THRESHOLD_IRQ_MASK 0x08 -#define LSM6DS3_FIFO_ODR_ADDR 0x0a -#define LSM6DS3_FIFO_ODR_MASK 0x78 -#define LSM6DS3_FIFO_ODR_MAX 0x07 -#define LSM6DS3_FIFO_ODR_MAX_HZ 800 -#define LSM6DS3_FIFO_ODR_OFF 0x00 -#define LSM6DS3_FIFO_CTRL3_ADDR 0x08 +#define LSM6DS3_MIN_DURATION_MS 1638 +#define LSM6DS3_ROUNDING_ADDR 0x16 +#define LSM6DS3_ROUNDING_MASK 0x04 +#define LSM6DS3_FIFO_MODE_ADDR 0x0a +#define LSM6DS3_FIFO_MODE_MASK 0x07 +#define LSM6DS3_FIFO_MODE_BYPASS 0x00 +#define LSM6DS3_FIFO_MODE_CONTINUOS 0x06 +#define LSM6DS3_FIFO_THRESHOLD_IRQ_MASK 0x08 +#define LSM6DS3_FIFO_ODR_ADDR 0x0a +#define LSM6DS3_FIFO_ODR_MASK 0x78 +#define LSM6DS3_FIFO_ODR_MAX 0x07 +#define LSM6DS3_FIFO_ODR_MAX_HZ 800 +#define LSM6DS3_FIFO_ODR_OFF 0x00 +#define LSM6DS3_FIFO_CTRL3_ADDR 0x08 #define LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK 0x07 -#define LSM6DS3_FIFO_GYRO_DECIMATOR_MASK 0x38 -#define LSM6DS3_FIFO_CTRL4_ADDR 0x09 -#define LSM6DS3_FIFO_STEP_C_DECIMATOR_MASK 0x38 -#define LSM6DS3_FIFO_THR_L_ADDR 0x06 -#define LSM6DS3_FIFO_THR_H_ADDR 0x07 -#define LSM6DS3_FIFO_THR_H_MASK 0x0f +#define LSM6DS3_FIFO_GYRO_DECIMATOR_MASK 0x38 +#define LSM6DS3_FIFO_CTRL4_ADDR 0x09 +#define LSM6DS3_FIFO_STEP_C_DECIMATOR_MASK 0x38 +#define LSM6DS3_FIFO_THR_L_ADDR 0x06 +#define LSM6DS3_FIFO_THR_H_ADDR 0x07 +#define LSM6DS3_FIFO_THR_H_MASK 0x0f #define LSM6DS3_FIFO_THR_IRQ_MASK 0x08 -#define LSM6DS3_FIFO_PEDO_E_ADDR 0x07 -#define LSM6DS3_FIFO_PEDO_E_MASK 0x80 -#define LSM6DS3_FIFO_STEP_C_FREQ 25 +#define LSM6DS3_FIFO_PEDO_E_ADDR 0x07 +#define LSM6DS3_FIFO_PEDO_E_MASK 0x80 +#define LSM6DS3_FIFO_STEP_C_FREQ 25 /* CUSTOM VALUES FOR ACCEL SENSOR */ -#define LSM6DS3_ACCEL_ODR_ADDR 0x10 -#define LSM6DS3_ACCEL_ODR_MASK 0xf0 -#define LSM6DS3_ACCEL_FS_ADDR 0x10 -#define LSM6DS3_ACCEL_FS_MASK 0x0c -#define LSM6DS3_ACCEL_FS_2G_VAL 0x00 -#define LSM6DS3_ACCEL_FS_4G_VAL 0x02 -#define LSM6DS3_ACCEL_FS_8G_VAL 0x03 -#define LSM6DS3_ACCEL_FS_16G_VAL 0x01 -#define LSM6DS3_ACCEL_FS_2G_GAIN 61 -#define LSM6DS3_ACCEL_FS_4G_GAIN 122 -#define LSM6DS3_ACCEL_FS_8G_GAIN 244 +#define LSM6DS3_ACCEL_ODR_ADDR 0x10 +#define LSM6DS3_ACCEL_ODR_MASK 0xf0 +#define LSM6DS3_ACCEL_FS_ADDR 0x10 +#define LSM6DS3_ACCEL_FS_MASK 0x0c +#define LSM6DS3_ACCEL_FS_2G_VAL 0x00 +#define LSM6DS3_ACCEL_FS_4G_VAL 0x02 +#define LSM6DS3_ACCEL_FS_8G_VAL 0x03 +#define LSM6DS3_ACCEL_FS_16G_VAL 0x01 +#define LSM6DS3_ACCEL_FS_2G_GAIN 61 +#define LSM6DS3_ACCEL_FS_4G_GAIN 122 +#define LSM6DS3_ACCEL_FS_8G_GAIN 244 #define LSM6DS3_ACCEL_FS_16G_GAIN 488 -#define LSM6DS3_ACCEL_OUT_X_L_ADDR 0x28 -#define LSM6DS3_ACCEL_OUT_Y_L_ADDR 0x2a -#define LSM6DS3_ACCEL_OUT_Z_L_ADDR 0x2c -#define LSM6DS3_ACCEL_AXIS_EN_ADDR 0x18 -#define LSM6DS3_ACCEL_DRDY_IRQ_MASK 0x01 +#define LSM6DS3_ACCEL_OUT_X_L_ADDR 0x28 +#define LSM6DS3_ACCEL_OUT_Y_L_ADDR 0x2a +#define LSM6DS3_ACCEL_OUT_Z_L_ADDR 0x2c +#define LSM6DS3_ACCEL_AXIS_EN_ADDR 0x18 +#define LSM6DS3_ACCEL_DRDY_IRQ_MASK 0x01 #define LSM6DS3_ACCEL_STD 1 #define LSM6DS3_ACCEL_STD_FROM_PD 2 /* CUSTOM VALUES FOR GYRO SENSOR */ -#define LSM6DS3_GYRO_ODR_ADDR 0x11 -#define LSM6DS3_GYRO_ODR_MASK 0xf0 -#define LSM6DS3_GYRO_FS_ADDR 0x11 -#define LSM6DS3_GYRO_FS_MASK 0x0c -#define LSM6DS3_GYRO_FS_245_VAL 0x00 -#define LSM6DS3_GYRO_FS_500_VAL 0x01 -#define LSM6DS3_GYRO_FS_1000_VAL 0x02 -#define LSM6DS3_GYRO_FS_2000_VAL 0x03 -#define LSM6DS3_GYRO_FS_245_GAIN 8750 -#define LSM6DS3_GYRO_FS_500_GAIN 17500 +#define LSM6DS3_GYRO_ODR_ADDR 0x11 +#define LSM6DS3_GYRO_ODR_MASK 0xf0 +#define LSM6DS3_GYRO_FS_ADDR 0x11 +#define LSM6DS3_GYRO_FS_MASK 0x0c +#define LSM6DS3_GYRO_FS_245_VAL 0x00 +#define LSM6DS3_GYRO_FS_500_VAL 0x01 +#define LSM6DS3_GYRO_FS_1000_VAL 0x02 +#define LSM6DS3_GYRO_FS_2000_VAL 0x03 +#define LSM6DS3_GYRO_FS_245_GAIN 8750 +#define LSM6DS3_GYRO_FS_500_GAIN 17500 #define LSM6DS3_GYRO_FS_1000_GAIN 35000 #define LSM6DS3_GYRO_FS_2000_GAIN 70000 #define LSM6DS3_GYRO_OUT_X_L_ADDR 0x22 #define LSM6DS3_GYRO_OUT_Y_L_ADDR 0x24 #define LSM6DS3_GYRO_OUT_Z_L_ADDR 0x26 #define LSM6DS3_GYRO_AXIS_EN_ADDR 0x19 -#define LSM6DS3_GYRO_DRDY_IRQ_MASK 0x02 -#define LSM6DS3_GYRO_STD 6 -#define LSM6DS3_GYRO_STD_FROM_PD 2 +#define LSM6DS3_GYRO_DRDY_IRQ_MASK 0x02 +#define LSM6DS3_GYRO_STD 6 +#define LSM6DS3_GYRO_STD_FROM_PD 2 -#define LSM6DS3_OUT_XYZ_SIZE 8 +#define LSM6DS3_OUT_XYZ_SIZE 8 /* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */ -#define LSM6DS3_SIGN_MOTION_EN_ADDR 0x19 -#define LSM6DS3_SIGN_MOTION_EN_MASK 0x01 +#define LSM6DS3_SIGN_MOTION_EN_ADDR 0x19 +#define LSM6DS3_SIGN_MOTION_EN_MASK 0x01 #define LSM6DS3_SIGN_MOTION_DRDY_IRQ_MASK 0x40 /* CUSTOM VALUES FOR STEP DETECTOR SENSOR */ -#define LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK 0x80 +#define LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK 0x80 /* CUSTOM VALUES FOR STEP COUNTER SENSOR */ -#define LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK 0x80 -#define LSM6DS3_STEP_COUNTER_OUT_L_ADDR 0x4b -#define LSM6DS3_STEP_COUNTER_OUT_SIZE 2 -#define LSM6DS3_STEP_COUNTER_RES_ADDR 0x19 -#define LSM6DS3_STEP_COUNTER_RES_MASK 0x06 -#define LSM6DS3_STEP_COUNTER_RES_ALL_EN 0x03 -#define LSM6DS3_STEP_COUNTER_RES_FUNC_EN 0x02 -#define LSM6DS3_STEP_COUNTER_DURATION_ADDR 0x15 +#define LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK 0x80 +#define LSM6DS3_STEP_COUNTER_OUT_L_ADDR 0x4b +#define LSM6DS3_STEP_COUNTER_OUT_SIZE 2 +#define LSM6DS3_STEP_COUNTER_RES_ADDR 0x19 +#define LSM6DS3_STEP_COUNTER_RES_MASK 0x06 +#define LSM6DS3_STEP_COUNTER_RES_ALL_EN 0x03 +#define LSM6DS3_STEP_COUNTER_RES_FUNC_EN 0x02 +#define LSM6DS3_STEP_COUNTER_DURATION_ADDR 0x15 /* CUSTOM VALUES FOR TILT SENSOR */ -#define LSM6DS3_TILT_EN_ADDR 0x58 -#define LSM6DS3_TILT_EN_MASK 0x20 -#define LSM6DS3_TILT_DRDY_IRQ_MASK 0x02 +#define LSM6DS3_TILT_EN_ADDR 0x58 +#define LSM6DS3_TILT_EN_MASK 0x20 +#define LSM6DS3_TILT_DRDY_IRQ_MASK 0x02 -#define LSM6DS3_ENABLE_AXIS 0x07 -#define LSM6DS3_FIFO_DIFF_L 0x3a -#define LSM6DS3_FIFO_DIFF_MASK 0x0fff -#define LSM6DS3_FIFO_DATA_OUT_L 0x3e -#define LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6 -#define LSM6DS3_FIFO_BYTE_FOR_CHANNEL 2 -#define LSM6DS3_FIFO_DATA_OVR_2REGS 0x4000 -#define LSM6DS3_FIFO_DATA_OVR 0x40 +#define LSM6DS3_ENABLE_AXIS 0x07 +#define LSM6DS3_FIFO_DIFF_L 0x3a +#define LSM6DS3_FIFO_DIFF_MASK 0x0fff +#define LSM6DS3_FIFO_DATA_OUT_L 0x3e +#define LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6 +#define LSM6DS3_FIFO_BYTE_FOR_CHANNEL 2 +#define LSM6DS3_FIFO_DATA_OVR_2REGS 0x4000 +#define LSM6DS3_FIFO_DATA_OVR 0x40 -#define LSM6DS3_SRC_FUNC_ADDR 0x53 -#define LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b +#define LSM6DS3_SRC_FUNC_ADDR 0x53 +#define LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b -#define LSM6DS3_SRC_SIGN_MOTION_DATA_AVL 0x40 -#define LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10 -#define LSM6DS3_SRC_TILT_DATA_AVL 0x20 -#define LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80 -#define LSM6DS3_FIFO_DATA_AVL 0x80 -#define LSM6DS3_RESET_ADDR 0x12 -#define LSM6DS3_RESET_MASK 0x01 +#define LSM6DS3_SRC_SIGN_MOTION_DATA_AVL 0x40 +#define LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10 +#define LSM6DS3_SRC_TILT_DATA_AVL 0x20 +#define LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80 +#define LSM6DS3_FIFO_DATA_AVL 0x80 +#define LSM6DS3_RESET_ADDR 0x12 +#define LSM6DS3_RESET_MASK 0x01 -#define LSM6DS3_ADDRESS 0xD6 -#define LSM6DSLTR_ID 0x6A +#define LSM6DS3_ADDRESS 0xD6 +#define LSM6DSLTR_ID 0x6A -static const char configure[][2] = - { - {LSM6DS3_ACCEL_AXIS_EN_ADDR, 0x38}, ///< xyz - {LSM6DS3_ACCEL_ODR_ADDR, (0x3 << 4) | (0x1 << 2) | (0x3 << 0)}, //ODR_52Hz|16G|BW_50Hz - {LSM6DS3_GYRO_AXIS_EN_ADDR, 0x38}, ///< xyz - {LSM6DS3_GYRO_ODR_ADDR, (3 << 4) | (3 << 2) | (0 << 0)}, ///< ODR_52Hz|2000dps| - {LSM6DS3_INT1_CTRL_ADDR, 0x3}, ///< GYRO_READY | ACCL_READY - {LSM6DS3_INT2_CTRL_ADDR, 0x3}, ///< GYRO_READY | ACCL_READY - }; - -volatile int lsm6ds3_state = 1; +static const char configure[][2] = { + {LSM6DS3_ACCEL_AXIS_EN_ADDR, 0x38}, + {LSM6DS3_ACCEL_ODR_ADDR, (0x3 << 4) | (0x1 << 2) | (0x3 << 0)}, + {LSM6DS3_GYRO_AXIS_EN_ADDR, 0x38}, + {LSM6DS3_GYRO_ODR_ADDR, (3 << 4) | (3 << 2) | (0 << 0)}, + {LSM6DS3_INT1_CTRL_ADDR, 0x3}, + {LSM6DS3_INT2_CTRL_ADDR, 0x3}, +}; static void i2c2Init() { @@ -238,7 +233,10 @@ bool I2C2_WaitEventCleared(uint32_t event) { uint32_t timeout = I2C_TIMEOUT_MAX; while (I2C_CheckEvent(I2CX, event)) { - if ((timeout--) == 0) return false; + if ((timeout--) == 0) { + TRACE("I2C Timeout!"); + return false; + } } return true; } @@ -257,7 +255,7 @@ int setGyroRegister(uint8_t address, uint8_t value) return -1; I2C_SendData(I2CX, address); - if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) // TODO TRANSMITTING + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTING)) return -1; I2C_SendData(I2CX, value); @@ -312,64 +310,50 @@ int gyroInit() return -1; } - TRACE("OK"); - setGyroRegister(0x12, 0x05); delay_ms(1); setGyroRegister(0x12, 0x04); delay_ms(1); - TRACE("OK2"); - for (uint8_t i = 0; i < DIM(configure); i++) { setGyroRegister(configure[i][0], configure[i][1]); } - TRACE("OK4"); - return 0; } -#if 0 -int gyroRead(void *buffer, int len, OFFSET offset1) +int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH]) { - char t_char[10]; - short t_short[7]; + if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY)) + return -1; - short *p_short; - I2C_transfer_block i2c_block[2]; + I2C_GenerateSTART(I2CX, ENABLE); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) + return -1; - xSemaphoreTake(lsm6ds3_sema, portMAX_DELAY); + I2C_Send7bitAddress(I2CX, LSM6DS3_ADDRESS, I2C_Direction_Transmitter); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) + return -1; - t_char[0] = LSM6DS3_GYRO_OUT_X_L_ADDR; + I2C_SendData(I2CX, LSM6DS3_GYRO_OUT_X_L_ADDR); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + return -1; - i2c_block[0].addr = LSM6DS3_ADDRESS; - i2c_block[0].rw = I2C_WRITE; - i2c_block[0].stop_mode = I2C_WRITE_BYPASS_STOP; - i2c_block[0].len = 1; - i2c_block[0].buf = t_char; + I2C_GenerateSTART(I2CX, ENABLE); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) + return -1; - i2c_block[1].addr = LSM6DS3_ADDRESS; - i2c_block[1].rw = I2C_READ; - i2c_block[1].stop_mode = I2C_WRITE_APPEND_STOP; + I2C_Send7bitAddress(I2CX, LSM6DS3_ADDRESS, I2C_Direction_Receiver); - i2c_block[1].len = len; - i2c_block[1].buf = (char *) t_short; - - i2c_dma_transfer(i2c_block, 2); - - { - p_short = (short *) buffer; - *p_short++ = t_short[0]; - *p_short++ = -t_short[1]; - *p_short++ = t_short[2]; - - *p_short++ = -t_short[3]; - *p_short++ = t_short[4]; - *p_short++ = -t_short[5]; + I2C_AcknowledgeConfig(I2CX, ENABLE); + for (uint8_t i=0; iRTC_Date = timeinfo->tm_mday; } -void RTC_TimeStructInit(RTC_TimeTypeDef* RTC_TimeStruct) { } -void RTC_DateStructInit(RTC_DateTypeDef* RTC_DateStruct) { } -void PWR_BackupAccessCmd(FunctionalState NewState) { } -void PWR_BackupRegulatorCmd(FunctionalState NewState) { } -void RCC_RTCCLKConfig(uint32_t RCC_RTCCLKSource) { } -void RCC_APB1PeriphClockCmd(uint32_t RCC_APB1Periph, FunctionalState NewState) { } -void RCC_RTCCLKCmd(FunctionalState NewState) { } -ErrorStatus RTC_Init(RTC_InitTypeDef* RTC_InitStruct) { return SUCCESS; } -void USART_SendData(USART_TypeDef* USARTx, uint16_t Data) { } -FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint16_t USART_FLAG) { return SET; } -void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF) { } -void USART_Init(USART_TypeDef* USARTx, USART_InitTypeDef* USART_InitStruct) { } -void USART_Cmd(USART_TypeDef* USARTx, FunctionalState NewState) { } -void USART_ClearITPendingBit(USART_TypeDef*, unsigned short) { } -uint16_t USART_ReceiveData(USART_TypeDef*) { return 0; } -void USART_DMACmd(USART_TypeDef* USARTx, uint16_t USART_DMAReq, FunctionalState NewState) { } -void USART_ITConfig(USART_TypeDef* USARTx, uint16_t USART_IT, FunctionalState NewState) { } -// void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct) { } -// void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) { } -void TIM_DMAConfig(TIM_TypeDef* TIMx, uint16_t TIM_DMABase, uint16_t TIM_DMABurstLength) { } -void TIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState) { } -void TIM_CtrlPWMOutputs(TIM_TypeDef* TIMx, FunctionalState NewState) { } -void RCC_PLLI2SConfig(uint32_t PLLI2SN, uint32_t PLLI2SR) { } -void RCC_PLLI2SCmd(FunctionalState NewState) { } -void RCC_I2SCLKConfig(uint32_t RCC_I2SCLKSource) { } -void SPI_I2S_DeInit(SPI_TypeDef* SPIx) { } -void I2S_Init(SPI_TypeDef* SPIx, I2S_InitTypeDef* I2S_InitStruct) { } -void I2S_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState) { } -void SPI_I2S_ITConfig(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT, FunctionalState NewState) { } -void RCC_LSEConfig(uint8_t RCC_LSE) { } -void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks) { }; -FlagStatus RCC_GetFlagStatus(uint8_t RCC_FLAG) { return SET; } -ErrorStatus RTC_WaitForSynchro(void) { return SUCCESS; } + void unlockFlash() { } void lockFlash() { } void flashWrite(uint32_t *address, uint32_t *buffer) { simuSleep(100); } diff --git a/radio/src/targets/taranis/CMakeLists.txt b/radio/src/targets/taranis/CMakeLists.txt index 0a6a952eb..5375700b0 100644 --- a/radio/src/targets/taranis/CMakeLists.txt +++ b/radio/src/targets/taranis/CMakeLists.txt @@ -143,13 +143,17 @@ elseif(PCB STREQUAL XLITES) set(STATUS_LEDS YES) endif() -#if(PCB STREQUAL XLITES) -# add_definitions(-DGYRO) -# set(TARGET_SRC -# ${TARGET_SRC} -# ../common/arm/stm32/lsm6ds3_driver.cpp -# ) -#endif() +if(PCB STREQUAL XLITES) + add_definitions(-DGYRO) + set(TARGET_SRC + ${TARGET_SRC} + ../common/arm/stm32/lsm6ds3_driver.cpp + ) + set(SRC + ${SRC} + gyro.cpp + ) +endif() if(PCB STREQUAL XLITE OR PCB STREQUAL XLITES OR PCB STREQUAL X3) option(PXX1 "PXX1 protocol support" OFF) diff --git a/radio/src/targets/taranis/board.cpp b/radio/src/targets/taranis/board.cpp index 6a8bfcf5f..d47ac94f4 100644 --- a/radio/src/targets/taranis/board.cpp +++ b/radio/src/targets/taranis/board.cpp @@ -290,6 +290,11 @@ void boardInit() initHeadphoneTrainerSwitch(); vbattRTC = getRTCBattVoltage(); + +#if defined(GYRO) + gyroInit(); +#endif + #endif // !defined(SIMU) } diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h index 650e6625e..b02ab879e 100644 --- a/radio/src/targets/taranis/board.h +++ b/radio/src/targets/taranis/board.h @@ -805,4 +805,10 @@ extern Fifo telemetryFifo; extern DMAFifo<32> serial2RxFifo; #endif +// Gyro driver +#define GYRO_VALUES_COUNT 6 +#define GYRO_BUFFER_LENGTH (GYRO_VALUES_COUNT * sizeof(int16_t)) +int gyroInit(); +int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH]); + #endif // _BOARD_H_ diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 2c787860b..6ed29b678 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -106,13 +106,21 @@ TASK_FUNCTION(mixerTask) static uint32_t lastRunTime; s_pulses_paused = true; - while(1) { + while (1) { #if defined(PCBX9D) || defined(PCBX7) // SBUS on Hearbeat PIN (which is a serial RX) processSbusInput(); #endif +#if defined(GYRO) + gyro.wakeup(); +#endif + +#if defined(BLUETOOTH) + bluetoothWakeup(); +#endif + RTOS_WAIT_TICKS(1); #if defined(SIMU) @@ -171,10 +179,6 @@ TASK_FUNCTION(mixerTask) DEBUG_TIMER_STOP(debugTimerTelemetryWakeup); #endif -#if defined(BLUETOOTH) - bluetoothWakeup(); -#endif - if (heartbeat == HEART_WDT_CHECK) { wdt_reset(); heartbeat = 0;