Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-25 01:05:10 +03:00
Code Issues Wiki Activity

[X-LiteS] Gyro added

Browse source
This commit is contained in:
Bertrand Songis 2019-04-02 20:12:50 +02:00
parent b967d5b08a
commit a13ee14bbb
18 changed files with 468 additions and 318 deletions
Download patch file Download diff file Expand all files Collapse all files

9
radio/src/dataconstants.h
View file

@ -566,8 +566,8 @@ enum MixSources {
#else #else
MIXSRC_P1 = MIXSRC_FIRST_POT, MIXSRC_P1 = MIXSRC_FIRST_POT,
MIXSRC_P2, MIXSRC_P2,
MIXSRC_P3, MIXSRC_P3,
MIXSRC_LAST_POT = MIXSRC_P3, MIXSRC_LAST_POT = MIXSRC_P3,
#endif #endif
#if defined(PCBHORUS) #if defined(PCBHORUS)
@ -575,6 +575,11 @@ enum MixSources {
MIXSRC_MOUSE2, LUA_EXPORT("jsy", "Joystick Y") MIXSRC_MOUSE2, LUA_EXPORT("jsy", "Joystick Y")
#endif #endif
#if defined(GYRO)
MIXSRC_GYRO1, LUA_EXPORT("gyr1", "Gyro X")
MIXSRC_GYRO2, LUA_EXPORT("gyr2", "Gyro Y")
#endif
#if defined(PCBSKY9X) #if defined(PCBSKY9X)
MIXSRC_REa, MIXSRC_REa,
MIXSRC_LAST_ROTARY_ENCODER = MIXSRC_REa, MIXSRC_LAST_ROTARY_ENCODER = MIXSRC_REa,

5
radio/src/gui/128x64/lcd.cpp
View file

@ -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); 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) { else if (idx >= MIXSRC_FIRST_SWITCH && idx <= MIXSRC_LAST_SWITCH) {
idx = idx-MIXSRC_FIRST_SWITCH; idx = idx-MIXSRC_FIRST_SWITCH;
if (ZEXIST(g_eeGeneral.switchNames[idx])) { if (ZEXIST(g_eeGeneral.switchNames[idx])) {

24
radio/src/gui/128x64/model_module_options.cpp
View file

@ -45,23 +45,7 @@ enum {
ITEM_MODULE_SETTINGS_COUNT ITEM_MODULE_SETTINGS_COUNT
}; };
/* Options order: #define IF_MODULE_OPTIONS(option, count) uint8_t(isModuleOptionAvailable(modelId, option) ? count : HIDDEN_ROW)
* - 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)
void drawPower(coord_t x, coord_t y, int8_t dBm) 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; // uint8_t variant = reusableBuffer.hardwareAndSettings.modules[g_moduleIdx].information.variant;
SUBMENU_NOTITLE(ITEM_MODULE_SETTINGS_COUNT, { SUBMENU_NOTITLE(ITEM_MODULE_SETTINGS_COUNT, {
IF_MODULE_OPTIONS(0, 0), IF_MODULE_OPTIONS(MODULE_OPTION_RF_PROTOCOL, 0),
IF_MODULE_OPTIONS(1, 0), IF_MODULE_OPTIONS(MODULE_OPTION_EXTERNAL_ANTENNA, 0),
IF_MODULE_OPTIONS(2, 0), IF_MODULE_OPTIONS(MODULE_OPTION_POWER, 0),
}); });
if (event == EVT_ENTRY) { if (event == EVT_ENTRY) {

3
radio/src/gui/common/stdlcd/radio_power_meter.cpp
View file

@ -21,14 +21,13 @@
#include <opentx.h> #include <opentx.h>
#include "opentx.h" #include "opentx.h"
void pxx2ModuleRequiredScreen(event_t event);
extern uint8_t g_moduleIdx; extern uint8_t g_moduleIdx;
void menuRadioPowerMeter(event_t event) void menuRadioPowerMeter(event_t event)
{ {
if (TELEMETRY_STREAMING()) { if (TELEMETRY_STREAMING()) {
lcdDrawCenteredText(LCD_H/2, "Turn off receiver"); lcdDrawCenteredText(LCD_H/2, "Turn off receiver");
if(event == EVT_KEY_FIRST(KEY_EXIT)) { if (event == EVT_KEY_FIRST(KEY_EXIT)) {
killEvents(event); killEvents(event);
popMenu(); popMenu();
} }

3
radio/src/gui/common/stdlcd/radio_spectrum_analyser.cpp
View file

@ -20,14 +20,13 @@
#include "opentx.h" #include "opentx.h"
extern void pxx2ModuleRequiredScreen(event_t event);
extern uint8_t g_moduleIdx; extern uint8_t g_moduleIdx;
void menuRadioSpectrumAnalyser(event_t event) void menuRadioSpectrumAnalyser(event_t event)
{ {
if (TELEMETRY_STREAMING()) { if (TELEMETRY_STREAMING()) {
lcdDrawCenteredText(15, "Turn off receiver"); lcdDrawCenteredText(15, "Turn off receiver");
if(event == EVT_KEY_FIRST(KEY_EXIT)) { if (event == EVT_KEY_FIRST(KEY_EXIT)) {
killEvents(event); killEvents(event);
popMenu(); popMenu();
} }

62
radio/src/gui/common/stdlcd/radio_tools.cpp
View file

@ -22,19 +22,6 @@
extern uint8_t g_moduleIdx; 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) 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; 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) void menuRadioTools(event_t event)
{ {
uint8_t spektrum_modules = 0, power_modules = 0; 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)); memclear(&reusableBuffer.hardwareAndSettings, sizeof(reusableBuffer.hardwareAndSettings));
#if defined(PXX2) #if defined(PXX2)
for (uint8_t idx=0; idx < NUM_MODULES; idx++) { for (uint8_t module = 0; module < NUM_MODULES; module++) {
if (isModulePXX2(idx) && (idx == INTERNAL_MODULE ? IS_INTERNAL_MODULE_ON() : IS_EXTERNAL_MODULE_ON())) { if (isModulePXX2(module) && (module == INTERNAL_MODULE ? IS_INTERNAL_MODULE_ON() : IS_EXTERNAL_MODULE_ON())) {
reusableBuffer.hardwareAndSettings.modules[idx].current = PXX2_HW_INFO_TX_ID; reusableBuffer.hardwareAndSettings.modules[module].current = PXX2_HW_INFO_TX_ID;
reusableBuffer.hardwareAndSettings.modules[idx].maximum = PXX2_HW_INFO_TX_ID; reusableBuffer.hardwareAndSettings.modules[module].maximum = PXX2_HW_INFO_TX_ID;
moduleSettings[idx].mode = MODULE_MODE_GET_HARDWARE_INFO; moduleSettings[module].mode = MODULE_MODE_GET_HARDWARE_INFO;
} }
} }
#endif #endif
} }
for (uint8_t idx=0; idx < NUM_MODULES; idx++) { for (uint8_t module = 0; module < NUM_MODULES; module++) {
if(hasSpektrumAnalyserCapability(idx)) { if (isModuleOptionAvailable(module, MODULE_OPTION_SPEKTRUM_ANALYSER)) {
spektrum_modules++; spektrum_modules++;
} }
if(hasPowerMeterCapablity(idx)) { if (isModuleOptionAvailable(module, MODULE_OPTION_POWER_METER)) {
power_modules++; power_modules++;
} }
} }
SIMPLE_MENU("TOOLS", menuTabGeneral, MENU_RADIO_TOOLS, HEADER_LINE + spektrum_modules + 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 defined(PXX2)
if(hasSpektrumAnalyserCapability(INTERNAL_MODULE)) if (isModuleOptionAvailable(INTERNAL_MODULE, MODULE_OPTION_SPEKTRUM_ANALYSER))
addRadioTool(menu_index++, "Spectrum (INT)", menuRadioSpectrumAnalyser, INTERNAL_MODULE, event); 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); 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); 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); addRadioTool(menu_index++, "Power Meter (EXT)", menuRadioPowerMeter, EXTERNAL_MODULE, event);
#endif #endif
} }

28
radio/src/gui/gui_common_arm.cpp
View file

@ -224,32 +224,36 @@ bool isSourceAvailableInCustomSwitches(int source)
bool isInputSourceAvailable(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); 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; return true;
if (source>=MIXSRC_FIRST_TRIM && source<=MIXSRC_LAST_TRIM) if (source >= MIXSRC_FIRST_TRIM && source <= MIXSRC_LAST_TRIM)
return true; return true;
if (source>=MIXSRC_FIRST_SWITCH && source<=MIXSRC_LAST_SWITCH) if (source >= MIXSRC_FIRST_SWITCH && source <= MIXSRC_LAST_SWITCH)
return SWITCH_EXISTS(source-MIXSRC_FIRST_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; return true;
if (source>=MIXSRC_FIRST_LOGICAL_SWITCH && source<=MIXSRC_LAST_LOGICAL_SWITCH) { if (source >= MIXSRC_FIRST_LOGICAL_SWITCH && source <= MIXSRC_LAST_LOGICAL_SWITCH) {
LogicalSwitchData * cs = lswAddress(source-MIXSRC_SW1); LogicalSwitchData * cs = lswAddress(source - MIXSRC_SW1);
return (cs->func != LS_FUNC_NONE); 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; return true;
if (source>=MIXSRC_FIRST_TELEM && source<=MIXSRC_LAST_TELEM) { if (source >= MIXSRC_FIRST_TELEM && source <= MIXSRC_LAST_TELEM) {
div_t qr = div(source-MIXSRC_FIRST_TELEM, 3); div_t qr = div(source - MIXSRC_FIRST_TELEM, 3);
return isTelemetryFieldAvailable(qr.quot) && isTelemetryFieldComparisonAvailable(qr.quot); return isTelemetryFieldAvailable(qr.quot) && isTelemetryFieldComparisonAvailable(qr.quot);
} }

70
radio/src/gyro.cpp Normal file
View file

@ -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]);
}

50
radio/src/gyro.h Normal file
View file

@ -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 <inttypes.h>
#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;

22
radio/src/mixer.cpp
View file

@ -301,7 +301,7 @@ getvalue_t getValue(mixsrc_t i)
return anas[i-MIXSRC_FIRST_INPUT]; return anas[i-MIXSRC_FIRST_INPUT];
} }
#if defined(LUA_INPUTS) #if defined(LUA_INPUTS)
else if (i < MIXSRC_LAST_LUA) { else if (i <= MIXSRC_LAST_LUA) {
#if defined(LUA_MODEL_SCRIPTS) #if defined(LUA_MODEL_SCRIPTS)
div_t qr = div(i-MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS); div_t qr = div(i-MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS);
return scriptInputsOutputs[qr.quot].outputs[qr.rem].value; return scriptInputsOutputs[qr.quot].outputs[qr.rem].value;
@ -311,19 +311,19 @@ getvalue_t getValue(mixsrc_t i)
} }
#endif #endif
#if defined(LUA_INPUTS)
else if (i <= MIXSRC_LAST_POT+NUM_MOUSE_ANALOGS) { 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) { #if defined(GYRO)
return calibratedAnalogs[i-MIXSRC_Rud]; else if (i <= MIXSRC_GYRO2) {
return gyro.outputs[i - MIXSRC_GYRO1];
} }
#endif #endif
#if defined(PCBGRUVIN9X) || defined(PCBMEGA2560) || defined(ROTARY_ENCODERS) #if defined(PCBSKY9X)
else if (i <= MIXSRC_LAST_ROTARY_ENCODER) { else if (i <= MIXSRC_LAST_ROTARY_ENCODER) {
return getRotaryEncoder(i-MIXSRC_REa); return getRotaryEncoder(i - MIXSRC_REa);
} }
#endif #endif
@ -377,11 +377,13 @@ getvalue_t getValue(mixsrc_t i)
return ex_chans[i-MIXSRC_CH1]; return ex_chans[i-MIXSRC_CH1];
} }
#if defined(GVARS)
else if (i <= MIXSRC_LAST_GVAR) { else if (i <= MIXSRC_LAST_GVAR) {
#if defined(GVARS)
return GVAR_VALUE(i-MIXSRC_GVAR1, getGVarFlightMode(mixerCurrentFlightMode, i - MIXSRC_GVAR1)); return GVAR_VALUE(i-MIXSRC_GVAR1, getGVarFlightMode(mixerCurrentFlightMode, i - MIXSRC_GVAR1));
} #else
return 0;
#endif #endif
}
else if (i == MIXSRC_TX_VOLTAGE) { else if (i == MIXSRC_TX_VOLTAGE) {
return g_vbat100mV; return g_vbat100mV;

4
radio/src/opentx.h
View file

@ -1380,4 +1380,8 @@ enum JackMode {
}; };
#endif #endif
#if defined(GYRO)
#include "gyro.h"
#endif
#endif // _OPENTX_H_ #endif // _OPENTX_H_

31
radio/src/pulses/modules.h
View file

@ -232,4 +232,35 @@ inline int8_t sentModuleChannels(uint8_t idx)
return 8 + g_model.moduleData[idx].channelsCount; 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_ #endif // _MODULES_H_

338
radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp
View file

@ -20,180 +20,175 @@
#include "opentx.h" #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 */ /* COMMON VALUES FOR ACCEL-GYRO SENSORS */
#define LSM6DS3_WHO_AM_I 0x0f #define LSM6DS3_WHO_AM_I 0x0f
#define LSM6DS3_WHO_AM_I_DEF 0x69 #define LSM6DS3_WHO_AM_I_DEF 0x69
#define LSM6DS3_AXIS_EN_MASK 0x38 #define LSM6DS3_AXIS_EN_MASK 0x38
#define LSM6DS3_INT1_CTRL_ADDR 0x0d #define LSM6DS3_INT1_CTRL_ADDR 0x0d
#define LSM6DS3_INT2_CTRL_ADDR 0x0e #define LSM6DS3_INT2_CTRL_ADDR 0x0e
#define LSM6DS3_INT1_FULL 0x20 #define LSM6DS3_INT1_FULL 0x20
#define LSM6DS3_INT1_FTH 0x08 #define LSM6DS3_INT1_FTH 0x08
#define LSM6DS3_MD1_ADDR 0x5e #define LSM6DS3_MD1_ADDR 0x5e
#define LSM6DS3_ODR_LIST_NUM 5 #define LSM6DS3_ODR_LIST_NUM 5
#define LSM6DS3_ODR_POWER_OFF_VAL 0x00 #define LSM6DS3_ODR_POWER_OFF_VAL 0x00
#define LSM6DS3_ODR_26HZ_VAL 0x02 #define LSM6DS3_ODR_26HZ_VAL 0x02
#define LSM6DS3_ODR_52HZ_VAL 0x03 #define LSM6DS3_ODR_52HZ_VAL 0x03
#define LSM6DS3_ODR_104HZ_VAL 0x04 #define LSM6DS3_ODR_104HZ_VAL 0x04
#define LSM6DS3_ODR_208HZ_VAL 0x05 #define LSM6DS3_ODR_208HZ_VAL 0x05
#define LSM6DS3_ODR_416HZ_VAL 0x06 #define LSM6DS3_ODR_416HZ_VAL 0x06
#define LSM6DS3_FS_LIST_NUM 4 #define LSM6DS3_FS_LIST_NUM 4
#define LSM6DS3_BDU_ADDR 0x12 #define LSM6DS3_BDU_ADDR 0x12
#define LSM6DS3_BDU_MASK 0x40 #define LSM6DS3_BDU_MASK 0x40
#define LSM6DS3_EN_BIT 0x01 #define LSM6DS3_EN_BIT 0x01
#define LSM6DS3_DIS_BIT 0x00 #define LSM6DS3_DIS_BIT 0x00
#define LSM6DS3_FUNC_EN_ADDR 0x19 #define LSM6DS3_FUNC_EN_ADDR 0x19
#define LSM6DS3_FUNC_EN_MASK 0x04 #define LSM6DS3_FUNC_EN_MASK 0x04
#define LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01 #define LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01
#define LSM6DS3_FUNC_CFG_ACCESS_MASK 0x01 #define LSM6DS3_FUNC_CFG_ACCESS_MASK 0x01
#define LSM6DS3_FUNC_CFG_ACCESS_MASK2 0x04 #define LSM6DS3_FUNC_CFG_ACCESS_MASK2 0x04
#define LSM6DS3_FUNC_CFG_REG2_MASK 0x80 #define LSM6DS3_FUNC_CFG_REG2_MASK 0x80
#define LSM6DS3_FUNC_CFG_START1_ADDR 0x62 #define LSM6DS3_FUNC_CFG_START1_ADDR 0x62
#define LSM6DS3_FUNC_CFG_START2_ADDR 0x63 #define LSM6DS3_FUNC_CFG_START2_ADDR 0x63
#define LSM6DS3_SELFTEST_ADDR 0x14 #define LSM6DS3_SELFTEST_ADDR 0x14
#define LSM6DS3_SELFTEST_ACCEL_MASK 0x03 #define LSM6DS3_SELFTEST_ACCEL_MASK 0x03
#define LSM6DS3_SELFTEST_GYRO_MASK 0x0c #define LSM6DS3_SELFTEST_GYRO_MASK 0x0c
#define LSM6DS3_SELF_TEST_DISABLED_VAL 0x00 #define LSM6DS3_SELF_TEST_DISABLED_VAL 0x00
#define LSM6DS3_SELF_TEST_POS_SIGN_VAL 0x01 #define LSM6DS3_SELF_TEST_POS_SIGN_VAL 0x01
#define LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02 #define LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02
#define LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03 #define LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03
#define LSM6DS3_LIR_ADDR 0x58 #define LSM6DS3_LIR_ADDR 0x58
#define LSM6DS3_LIR_MASK 0x01 #define LSM6DS3_LIR_MASK 0x01
#define LSM6DS3_TIMER_EN_ADDR 0x58 #define LSM6DS3_TIMER_EN_ADDR 0x58
#define LSM6DS3_TIMER_EN_MASK 0x80 #define LSM6DS3_TIMER_EN_MASK 0x80
#define LSM6DS3_PEDOMETER_EN_ADDR 0x58 #define LSM6DS3_PEDOMETER_EN_ADDR 0x58
#define LSM6DS3_PEDOMETER_EN_MASK 0x40 #define LSM6DS3_PEDOMETER_EN_MASK 0x40
#define LSM6DS3_INT2_ON_INT1_ADDR 0x13 #define LSM6DS3_INT2_ON_INT1_ADDR 0x13
#define LSM6DS3_INT2_ON_INT1_MASK 0x20 #define LSM6DS3_INT2_ON_INT1_MASK 0x20
#define LSM6DS3_MIN_DURATION_MS 1638 #define LSM6DS3_MIN_DURATION_MS 1638
#define LSM6DS3_ROUNDING_ADDR 0x16 #define LSM6DS3_ROUNDING_ADDR 0x16
#define LSM6DS3_ROUNDING_MASK 0x04 #define LSM6DS3_ROUNDING_MASK 0x04
#define LSM6DS3_FIFO_MODE_ADDR 0x0a #define LSM6DS3_FIFO_MODE_ADDR 0x0a
#define LSM6DS3_FIFO_MODE_MASK 0x07 #define LSM6DS3_FIFO_MODE_MASK 0x07
#define LSM6DS3_FIFO_MODE_BYPASS 0x00 #define LSM6DS3_FIFO_MODE_BYPASS 0x00
#define LSM6DS3_FIFO_MODE_CONTINUOS 0x06 #define LSM6DS3_FIFO_MODE_CONTINUOS 0x06
#define LSM6DS3_FIFO_THRESHOLD_IRQ_MASK 0x08 #define LSM6DS3_FIFO_THRESHOLD_IRQ_MASK 0x08
#define LSM6DS3_FIFO_ODR_ADDR 0x0a #define LSM6DS3_FIFO_ODR_ADDR 0x0a
#define LSM6DS3_FIFO_ODR_MASK 0x78 #define LSM6DS3_FIFO_ODR_MASK 0x78
#define LSM6DS3_FIFO_ODR_MAX 0x07 #define LSM6DS3_FIFO_ODR_MAX 0x07
#define LSM6DS3_FIFO_ODR_MAX_HZ 800 #define LSM6DS3_FIFO_ODR_MAX_HZ 800
#define LSM6DS3_FIFO_ODR_OFF 0x00 #define LSM6DS3_FIFO_ODR_OFF 0x00
#define LSM6DS3_FIFO_CTRL3_ADDR 0x08 #define LSM6DS3_FIFO_CTRL3_ADDR 0x08
#define LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK 0x07 #define LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK 0x07
#define LSM6DS3_FIFO_GYRO_DECIMATOR_MASK 0x38 #define LSM6DS3_FIFO_GYRO_DECIMATOR_MASK 0x38
#define LSM6DS3_FIFO_CTRL4_ADDR 0x09 #define LSM6DS3_FIFO_CTRL4_ADDR 0x09
#define LSM6DS3_FIFO_STEP_C_DECIMATOR_MASK 0x38 #define LSM6DS3_FIFO_STEP_C_DECIMATOR_MASK 0x38
#define LSM6DS3_FIFO_THR_L_ADDR 0x06 #define LSM6DS3_FIFO_THR_L_ADDR 0x06
#define LSM6DS3_FIFO_THR_H_ADDR 0x07 #define LSM6DS3_FIFO_THR_H_ADDR 0x07
#define LSM6DS3_FIFO_THR_H_MASK 0x0f #define LSM6DS3_FIFO_THR_H_MASK 0x0f
#define LSM6DS3_FIFO_THR_IRQ_MASK 0x08 #define LSM6DS3_FIFO_THR_IRQ_MASK 0x08
#define LSM6DS3_FIFO_PEDO_E_ADDR 0x07 #define LSM6DS3_FIFO_PEDO_E_ADDR 0x07
#define LSM6DS3_FIFO_PEDO_E_MASK 0x80 #define LSM6DS3_FIFO_PEDO_E_MASK 0x80
#define LSM6DS3_FIFO_STEP_C_FREQ 25 #define LSM6DS3_FIFO_STEP_C_FREQ 25
/* CUSTOM VALUES FOR ACCEL SENSOR */ /* CUSTOM VALUES FOR ACCEL SENSOR */
#define LSM6DS3_ACCEL_ODR_ADDR 0x10 #define LSM6DS3_ACCEL_ODR_ADDR 0x10
#define LSM6DS3_ACCEL_ODR_MASK 0xf0 #define LSM6DS3_ACCEL_ODR_MASK 0xf0
#define LSM6DS3_ACCEL_FS_ADDR 0x10 #define LSM6DS3_ACCEL_FS_ADDR 0x10
#define LSM6DS3_ACCEL_FS_MASK 0x0c #define LSM6DS3_ACCEL_FS_MASK 0x0c
#define LSM6DS3_ACCEL_FS_2G_VAL 0x00 #define LSM6DS3_ACCEL_FS_2G_VAL 0x00
#define LSM6DS3_ACCEL_FS_4G_VAL 0x02 #define LSM6DS3_ACCEL_FS_4G_VAL 0x02
#define LSM6DS3_ACCEL_FS_8G_VAL 0x03 #define LSM6DS3_ACCEL_FS_8G_VAL 0x03
#define LSM6DS3_ACCEL_FS_16G_VAL 0x01 #define LSM6DS3_ACCEL_FS_16G_VAL 0x01
#define LSM6DS3_ACCEL_FS_2G_GAIN 61 #define LSM6DS3_ACCEL_FS_2G_GAIN 61
#define LSM6DS3_ACCEL_FS_4G_GAIN 122 #define LSM6DS3_ACCEL_FS_4G_GAIN 122
#define LSM6DS3_ACCEL_FS_8G_GAIN 244 #define LSM6DS3_ACCEL_FS_8G_GAIN 244
#define LSM6DS3_ACCEL_FS_16G_GAIN 488 #define LSM6DS3_ACCEL_FS_16G_GAIN 488
#define LSM6DS3_ACCEL_OUT_X_L_ADDR 0x28 #define LSM6DS3_ACCEL_OUT_X_L_ADDR 0x28
#define LSM6DS3_ACCEL_OUT_Y_L_ADDR 0x2a #define LSM6DS3_ACCEL_OUT_Y_L_ADDR 0x2a
#define LSM6DS3_ACCEL_OUT_Z_L_ADDR 0x2c #define LSM6DS3_ACCEL_OUT_Z_L_ADDR 0x2c
#define LSM6DS3_ACCEL_AXIS_EN_ADDR 0x18 #define LSM6DS3_ACCEL_AXIS_EN_ADDR 0x18
#define LSM6DS3_ACCEL_DRDY_IRQ_MASK 0x01 #define LSM6DS3_ACCEL_DRDY_IRQ_MASK 0x01
#define LSM6DS3_ACCEL_STD 1 #define LSM6DS3_ACCEL_STD 1
#define LSM6DS3_ACCEL_STD_FROM_PD 2 #define LSM6DS3_ACCEL_STD_FROM_PD 2
/* CUSTOM VALUES FOR GYRO SENSOR */ /* CUSTOM VALUES FOR GYRO SENSOR */
#define LSM6DS3_GYRO_ODR_ADDR 0x11 #define LSM6DS3_GYRO_ODR_ADDR 0x11
#define LSM6DS3_GYRO_ODR_MASK 0xf0 #define LSM6DS3_GYRO_ODR_MASK 0xf0
#define LSM6DS3_GYRO_FS_ADDR 0x11 #define LSM6DS3_GYRO_FS_ADDR 0x11
#define LSM6DS3_GYRO_FS_MASK 0x0c #define LSM6DS3_GYRO_FS_MASK 0x0c
#define LSM6DS3_GYRO_FS_245_VAL 0x00 #define LSM6DS3_GYRO_FS_245_VAL 0x00
#define LSM6DS3_GYRO_FS_500_VAL 0x01 #define LSM6DS3_GYRO_FS_500_VAL 0x01
#define LSM6DS3_GYRO_FS_1000_VAL 0x02 #define LSM6DS3_GYRO_FS_1000_VAL 0x02
#define LSM6DS3_GYRO_FS_2000_VAL 0x03 #define LSM6DS3_GYRO_FS_2000_VAL 0x03
#define LSM6DS3_GYRO_FS_245_GAIN 8750 #define LSM6DS3_GYRO_FS_245_GAIN 8750
#define LSM6DS3_GYRO_FS_500_GAIN 17500 #define LSM6DS3_GYRO_FS_500_GAIN 17500
#define LSM6DS3_GYRO_FS_1000_GAIN 35000 #define LSM6DS3_GYRO_FS_1000_GAIN 35000
#define LSM6DS3_GYRO_FS_2000_GAIN 70000 #define LSM6DS3_GYRO_FS_2000_GAIN 70000
#define LSM6DS3_GYRO_OUT_X_L_ADDR 0x22 #define LSM6DS3_GYRO_OUT_X_L_ADDR 0x22
#define LSM6DS3_GYRO_OUT_Y_L_ADDR 0x24 #define LSM6DS3_GYRO_OUT_Y_L_ADDR 0x24
#define LSM6DS3_GYRO_OUT_Z_L_ADDR 0x26 #define LSM6DS3_GYRO_OUT_Z_L_ADDR 0x26
#define LSM6DS3_GYRO_AXIS_EN_ADDR 0x19 #define LSM6DS3_GYRO_AXIS_EN_ADDR 0x19
#define LSM6DS3_GYRO_DRDY_IRQ_MASK 0x02 #define LSM6DS3_GYRO_DRDY_IRQ_MASK 0x02
#define LSM6DS3_GYRO_STD 6 #define LSM6DS3_GYRO_STD 6
#define LSM6DS3_GYRO_STD_FROM_PD 2 #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 */ /* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */
#define LSM6DS3_SIGN_MOTION_EN_ADDR 0x19 #define LSM6DS3_SIGN_MOTION_EN_ADDR 0x19
#define LSM6DS3_SIGN_MOTION_EN_MASK 0x01 #define LSM6DS3_SIGN_MOTION_EN_MASK 0x01
#define LSM6DS3_SIGN_MOTION_DRDY_IRQ_MASK 0x40 #define LSM6DS3_SIGN_MOTION_DRDY_IRQ_MASK 0x40
/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */ /* 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 */ /* CUSTOM VALUES FOR STEP COUNTER SENSOR */
#define LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK 0x80 #define LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK 0x80
#define LSM6DS3_STEP_COUNTER_OUT_L_ADDR 0x4b #define LSM6DS3_STEP_COUNTER_OUT_L_ADDR 0x4b
#define LSM6DS3_STEP_COUNTER_OUT_SIZE 2 #define LSM6DS3_STEP_COUNTER_OUT_SIZE 2
#define LSM6DS3_STEP_COUNTER_RES_ADDR 0x19 #define LSM6DS3_STEP_COUNTER_RES_ADDR 0x19
#define LSM6DS3_STEP_COUNTER_RES_MASK 0x06 #define LSM6DS3_STEP_COUNTER_RES_MASK 0x06
#define LSM6DS3_STEP_COUNTER_RES_ALL_EN 0x03 #define LSM6DS3_STEP_COUNTER_RES_ALL_EN 0x03
#define LSM6DS3_STEP_COUNTER_RES_FUNC_EN 0x02 #define LSM6DS3_STEP_COUNTER_RES_FUNC_EN 0x02
#define LSM6DS3_STEP_COUNTER_DURATION_ADDR 0x15 #define LSM6DS3_STEP_COUNTER_DURATION_ADDR 0x15
/* CUSTOM VALUES FOR TILT SENSOR */ /* CUSTOM VALUES FOR TILT SENSOR */
#define LSM6DS3_TILT_EN_ADDR 0x58 #define LSM6DS3_TILT_EN_ADDR 0x58
#define LSM6DS3_TILT_EN_MASK 0x20 #define LSM6DS3_TILT_EN_MASK 0x20
#define LSM6DS3_TILT_DRDY_IRQ_MASK 0x02 #define LSM6DS3_TILT_DRDY_IRQ_MASK 0x02
#define LSM6DS3_ENABLE_AXIS 0x07 #define LSM6DS3_ENABLE_AXIS 0x07
#define LSM6DS3_FIFO_DIFF_L 0x3a #define LSM6DS3_FIFO_DIFF_L 0x3a
#define LSM6DS3_FIFO_DIFF_MASK 0x0fff #define LSM6DS3_FIFO_DIFF_MASK 0x0fff
#define LSM6DS3_FIFO_DATA_OUT_L 0x3e #define LSM6DS3_FIFO_DATA_OUT_L 0x3e
#define LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6 #define LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6
#define LSM6DS3_FIFO_BYTE_FOR_CHANNEL 2 #define LSM6DS3_FIFO_BYTE_FOR_CHANNEL 2
#define LSM6DS3_FIFO_DATA_OVR_2REGS 0x4000 #define LSM6DS3_FIFO_DATA_OVR_2REGS 0x4000
#define LSM6DS3_FIFO_DATA_OVR 0x40 #define LSM6DS3_FIFO_DATA_OVR 0x40
#define LSM6DS3_SRC_FUNC_ADDR 0x53 #define LSM6DS3_SRC_FUNC_ADDR 0x53
#define LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b #define LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b
#define LSM6DS3_SRC_SIGN_MOTION_DATA_AVL 0x40 #define LSM6DS3_SRC_SIGN_MOTION_DATA_AVL 0x40
#define LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10 #define LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10
#define LSM6DS3_SRC_TILT_DATA_AVL 0x20 #define LSM6DS3_SRC_TILT_DATA_AVL 0x20
#define LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80 #define LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80
#define LSM6DS3_FIFO_DATA_AVL 0x80 #define LSM6DS3_FIFO_DATA_AVL 0x80
#define LSM6DS3_RESET_ADDR 0x12 #define LSM6DS3_RESET_ADDR 0x12
#define LSM6DS3_RESET_MASK 0x01 #define LSM6DS3_RESET_MASK 0x01
#define LSM6DS3_ADDRESS 0xD6 #define LSM6DS3_ADDRESS 0xD6
#define LSM6DSLTR_ID 0x6A #define LSM6DSLTR_ID 0x6A
static const char configure[][2] = static const char configure[][2] = {
{ {LSM6DS3_ACCEL_AXIS_EN_ADDR, 0x38},
{LSM6DS3_ACCEL_AXIS_EN_ADDR, 0x38}, ///< xyz {LSM6DS3_ACCEL_ODR_ADDR, (0x3 << 4) | (0x1 << 2) | (0x3 << 0)},
{LSM6DS3_ACCEL_ODR_ADDR, (0x3 << 4) | (0x1 << 2) | (0x3 << 0)}, //ODR_52Hz|16G|BW_50Hz {LSM6DS3_GYRO_AXIS_EN_ADDR, 0x38},
{LSM6DS3_GYRO_AXIS_EN_ADDR, 0x38}, ///< xyz {LSM6DS3_GYRO_ODR_ADDR, (3 << 4) | (3 << 2) | (0 << 0)},
{LSM6DS3_GYRO_ODR_ADDR, (3 << 4) | (3 << 2) | (0 << 0)}, ///< ODR_52Hz|2000dps| {LSM6DS3_INT1_CTRL_ADDR, 0x3},
{LSM6DS3_INT1_CTRL_ADDR, 0x3}, ///< GYRO_READY | ACCL_READY {LSM6DS3_INT2_CTRL_ADDR, 0x3},
{LSM6DS3_INT2_CTRL_ADDR, 0x3}, ///< GYRO_READY | ACCL_READY };
};
volatile int lsm6ds3_state = 1;
static void i2c2Init() static void i2c2Init()
{ {
@ -238,7 +233,10 @@ bool I2C2_WaitEventCleared(uint32_t event)
{ {
uint32_t timeout = I2C_TIMEOUT_MAX; uint32_t timeout = I2C_TIMEOUT_MAX;
while (I2C_CheckEvent(I2CX, event)) { while (I2C_CheckEvent(I2CX, event)) {
if ((timeout--) == 0) return false; if ((timeout--) == 0) {
TRACE("I2C Timeout!");
return false;
}
} }
return true; return true;
} }
@ -257,7 +255,7 @@ int setGyroRegister(uint8_t address, uint8_t value)
return -1; return -1;
I2C_SendData(I2CX, address); I2C_SendData(I2CX, address);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) // TODO TRANSMITTING if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTING))
return -1; return -1;
I2C_SendData(I2CX, value); I2C_SendData(I2CX, value);
@ -312,64 +310,50 @@ int gyroInit()
return -1; return -1;
} }
TRACE("OK");
setGyroRegister(0x12, 0x05); setGyroRegister(0x12, 0x05);
delay_ms(1); delay_ms(1);
setGyroRegister(0x12, 0x04); setGyroRegister(0x12, 0x04);
delay_ms(1); delay_ms(1);
TRACE("OK2");
for (uint8_t i = 0; i < DIM(configure); i++) { for (uint8_t i = 0; i < DIM(configure); i++) {
setGyroRegister(configure[i][0], configure[i][1]); setGyroRegister(configure[i][0], configure[i][1]);
} }
TRACE("OK4");
return 0; return 0;
} }
#if 0 int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH])
int gyroRead(void *buffer, int len, OFFSET offset1)
{ {
char t_char[10]; if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY))
short t_short[7]; return -1;
short *p_short; I2C_GenerateSTART(I2CX, ENABLE);
I2C_transfer_block i2c_block[2]; 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_GenerateSTART(I2CX, ENABLE);
i2c_block[0].rw = I2C_WRITE; if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT))
i2c_block[0].stop_mode = I2C_WRITE_BYPASS_STOP; return -1;
i2c_block[0].len = 1;
i2c_block[0].buf = t_char;
i2c_block[1].addr = LSM6DS3_ADDRESS; I2C_Send7bitAddress(I2CX, LSM6DS3_ADDRESS, I2C_Direction_Receiver);
i2c_block[1].rw = I2C_READ;
i2c_block[1].stop_mode = I2C_WRITE_APPEND_STOP;
i2c_block[1].len = len; I2C_AcknowledgeConfig(I2CX, ENABLE);
i2c_block[1].buf = (char *) t_short; for (uint8_t i=0; i<GYRO_BUFFER_LENGTH; i++) {
if (i == GYRO_BUFFER_LENGTH - 1)
i2c_dma_transfer(i2c_block, 2); I2C_AcknowledgeConfig(I2CX, DISABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED))
{ return -1;
p_short = (short *) buffer; buffer[i] = I2C_ReceiveData(I2CX);
*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];
} }
return len; I2C_GenerateSTOP(I2CX, ENABLE);
return 0;
} }
#endif

94
radio/src/targets/simu/simpgmspace.cpp
View file

@ -634,7 +634,67 @@ void pwrInit() { }
int usbPlugged() { return false; } int usbPlugged() { return false; }
int getSelectedUsbMode() { return USB_JOYSTICK_MODE; } int getSelectedUsbMode() { return USB_JOYSTICK_MODE; }
void setSelectedUsbMode(int mode) {} void setSelectedUsbMode(int mode) {}
void delay_ms(uint32_t ms) { }
// GPIO fake functions
void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF) { }
// PWR fake functions
void PWR_BackupAccessCmd(FunctionalState NewState) { }
void PWR_BackupRegulatorCmd(FunctionalState NewState) { }
// USART fake functions
void USART_DeInit(USART_TypeDef* ) { } void USART_DeInit(USART_TypeDef* ) { }
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) { }
void USART_SendData(USART_TypeDef* USARTx, uint16_t Data) { }
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) { }
FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint16_t USART_FLAG) { return SET; }
// TIM fake functions
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) { }
// I2C fake functions
void I2C_DeInit(I2C_TypeDef*) { }
void I2C_Init(I2C_TypeDef*, I2C_InitTypeDef*) { }
void I2C_Cmd(I2C_TypeDef*, FunctionalState) { }
void I2C_Send7bitAddress(I2C_TypeDef*, unsigned char, unsigned char) { }
void I2C_SendData(I2C_TypeDef*, unsigned char) { }
void I2C_GenerateSTART(I2C_TypeDef*, FunctionalState) { }
void I2C_GenerateSTOP(I2C_TypeDef*, FunctionalState) { }
void I2C_AcknowledgeConfig(I2C_TypeDef*, FunctionalState) { }
uint8_t I2C_ReceiveData(I2C_TypeDef*) { return 0; }
ErrorStatus I2C_CheckEvent(I2C_TypeDef*, unsigned int) { return ERROR; }
// I2S fake functions
void I2S_Init(SPI_TypeDef* SPIx, I2S_InitTypeDef* I2S_InitStruct) { }
void I2S_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState) { }
// SPI fake functions
void SPI_I2S_DeInit(SPI_TypeDef* SPIx) { }
void SPI_I2S_ITConfig(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT, FunctionalState NewState) { }
// RCC fake functions
void RCC_RTCCLKConfig(uint32_t RCC_RTCCLKSource) { }
void RCC_APB1PeriphClockCmd(uint32_t RCC_APB1Periph, FunctionalState NewState) { }
void RCC_RTCCLKCmd(FunctionalState NewState) { }
void RCC_PLLI2SConfig(uint32_t PLLI2SN, uint32_t PLLI2SR) { }
void RCC_PLLI2SCmd(FunctionalState NewState) { }
void RCC_I2SCLKConfig(uint32_t RCC_I2SCLKSource) { }
void RCC_LSEConfig(uint8_t RCC_LSE) { }
void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks) { };
FlagStatus RCC_GetFlagStatus(uint8_t RCC_FLAG) { return SET; }
// RTC fake functions
ErrorStatus RTC_Init(RTC_InitTypeDef* RTC_InitStruct) { return SUCCESS; }
void RTC_TimeStructInit(RTC_TimeTypeDef* RTC_TimeStruct) { }
void RTC_DateStructInit(RTC_DateTypeDef* RTC_DateStruct) { }
ErrorStatus RTC_WaitForSynchro(void) { return SUCCESS; }
ErrorStatus RTC_SetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeStruct) { return SUCCESS; } ErrorStatus RTC_SetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeStruct) { return SUCCESS; }
ErrorStatus RTC_SetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct) { return SUCCESS; } ErrorStatus RTC_SetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct) { return SUCCESS; }
void RTC_GetTime(uint32_t RTC_Format, RTC_TimeTypeDef * RTC_TimeStruct) void RTC_GetTime(uint32_t RTC_Format, RTC_TimeTypeDef * RTC_TimeStruct)
@ -657,39 +717,7 @@ void RTC_GetDate(uint32_t RTC_Format, RTC_DateTypeDef * RTC_DateStruct)
RTC_DateStruct->RTC_Date = timeinfo->tm_mday; RTC_DateStruct->RTC_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 unlockFlash() { }
void lockFlash() { } void lockFlash() { }
void flashWrite(uint32_t *address, uint32_t *buffer) { simuSleep(100); } void flashWrite(uint32_t *address, uint32_t *buffer) { simuSleep(100); }

18
radio/src/targets/taranis/CMakeLists.txt
View file

@ -143,13 +143,17 @@ elseif(PCB STREQUAL XLITES)
set(STATUS_LEDS YES) set(STATUS_LEDS YES)
endif() endif()
#if(PCB STREQUAL XLITES) if(PCB STREQUAL XLITES)
# add_definitions(-DGYRO) add_definitions(-DGYRO)
# set(TARGET_SRC set(TARGET_SRC
# ${TARGET_SRC} ${TARGET_SRC}
# ../common/arm/stm32/lsm6ds3_driver.cpp ../common/arm/stm32/lsm6ds3_driver.cpp
# ) )
#endif() set(SRC
${SRC}
gyro.cpp
)
endif()
if(PCB STREQUAL XLITE OR PCB STREQUAL XLITES OR PCB STREQUAL X3) if(PCB STREQUAL XLITE OR PCB STREQUAL XLITES OR PCB STREQUAL X3)
option(PXX1 "PXX1 protocol support" OFF) option(PXX1 "PXX1 protocol support" OFF)

5
radio/src/targets/taranis/board.cpp
View file

@ -290,6 +290,11 @@ void boardInit()
initHeadphoneTrainerSwitch(); initHeadphoneTrainerSwitch();
vbattRTC = getRTCBattVoltage(); vbattRTC = getRTCBattVoltage();
#if defined(GYRO)
gyroInit();
#endif
#endif // !defined(SIMU) #endif // !defined(SIMU)
} }

6
radio/src/targets/taranis/board.h
View file

@ -805,4 +805,10 @@ extern Fifo<uint8_t, TELEMETRY_FIFO_SIZE> telemetryFifo;
extern DMAFifo<32> serial2RxFifo; extern DMAFifo<32> serial2RxFifo;
#endif #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_ #endif // _BOARD_H_

14
radio/src/tasks.cpp
View file

@ -106,13 +106,21 @@ TASK_FUNCTION(mixerTask)
static uint32_t lastRunTime; static uint32_t lastRunTime;
s_pulses_paused = true; s_pulses_paused = true;
while(1) { while (1) {
#if defined(PCBX9D) || defined(PCBX7) #if defined(PCBX9D) || defined(PCBX7)
// SBUS on Hearbeat PIN (which is a serial RX) // SBUS on Hearbeat PIN (which is a serial RX)
processSbusInput(); processSbusInput();
#endif #endif
#if defined(GYRO)
gyro.wakeup();
#endif
#if defined(BLUETOOTH)
bluetoothWakeup();
#endif
RTOS_WAIT_TICKS(1); RTOS_WAIT_TICKS(1);
#if defined(SIMU) #if defined(SIMU)
@ -171,10 +179,6 @@ TASK_FUNCTION(mixerTask)
DEBUG_TIMER_STOP(debugTimerTelemetryWakeup); DEBUG_TIMER_STOP(debugTimerTelemetryWakeup);
#endif #endif
#if defined(BLUETOOTH)
bluetoothWakeup();
#endif
if (heartbeat == HEART_WDT_CHECK) { if (heartbeat == HEART_WDT_CHECK) {
wdt_reset(); wdt_reset();
heartbeat = 0; heartbeat = 0;