Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-24 16:55:20 +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

5
radio/src/dataconstants.h
View file

@ -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,

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

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

@ -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) {

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

@ -21,14 +21,13 @@
#include <opentx.h>
#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();
}

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

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

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

@ -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
}

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

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

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];
}
#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;

4
radio/src/opentx.h
View file

@ -1380,4 +1380,8 @@ enum JackMode {
};
#endif
#if defined(GYRO)
#include "gyro.h"
#endif
#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;
}
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_

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

@ -20,8 +20,6 @@
#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
@ -183,17 +181,14 @@
#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; i<GYRO_BUFFER_LENGTH; i++) {
if (i == GYRO_BUFFER_LENGTH - 1)
I2C_AcknowledgeConfig(I2CX, DISABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED))
return -1;
buffer[i] = I2C_ReceiveData(I2CX);
}
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 getSelectedUsbMode() { return USB_JOYSTICK_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_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_SetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct) { return SUCCESS; }
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;
}
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); }

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

@ -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)

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

@ -290,6 +290,11 @@ void boardInit()
initHeadphoneTrainerSwitch();
vbattRTC = getRTCBattVoltage();
#if defined(GYRO)
gyroInit();
#endif
#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;
#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_

14
radio/src/tasks.cpp
View file

@ -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;