From 472bf113adad5b5f72ebec5d6b209d8b997f2b86 Mon Sep 17 00:00:00 2001 From: James Harrison Date: Fri, 16 Oct 2015 14:33:09 +0100 Subject: [PATCH 1/4] Add mention of power requirements for gpassthrough This change adds a brief mention of power requirements and USB power pass-through to the GPS docks. This small but important fact is not mentioned anywhere, though it is obvious if you stare at the board specific documentation long enough. Many larger flight controllers (eg Pixhawk) do pass through 5V from USB, so this is non-obvious to users of those FCs. --- docs/Gps.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/Gps.md b/docs/Gps.md index a9fb36ed07..e9f685f215 100644 --- a/docs/Gps.md +++ b/docs/Gps.md @@ -66,6 +66,8 @@ UBlox GPS units can either be configured using the FC or manually. Use UBox U-Center and connect your GPS to your computer. The CLI `gpspassthrough` command may be of use if you do not have a spare USART to USB adapter. +Note that many boards will not provide +5V from USB to the GPS module, such as the SPRacingF3; if you are using `gpspassthrough` you may need to connect a BEC to the controller if your board permits it, or use a standalone UART adapter. Check your board documentation to see if your GPS port is powered from USB. + Display the Packet Console (so you can see what messages your receiver is sending to your computer). Display the Configation View. From 8ecbd775789884d57b60a92a120d2df34c09b5a9 Mon Sep 17 00:00:00 2001 From: Pierre-A Date: Mon, 2 Nov 2015 09:47:30 +0100 Subject: [PATCH 2/4] Fix bug on Hott GPS speed --- src/main/telemetry/hott.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 0142e3b532..6c997cb50f 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -193,8 +193,8 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]); - // GPS Speed in km/h - uint16_t speed = (GPS_speed * 36) / 100; // 0->1m/s * 0->36 = km/h + // GPS Speed is returned in cm/s (from io/gps.c) and must be sent in km/h (Hott requirement) + uint16_t speed = (GPS_speed * 36) / 1000; hottGPSMessage->gps_speed_L = speed & 0x00FF; hottGPSMessage->gps_speed_H = speed >> 8; From 9be5abf0d316cc33eb643e6d8869f0a9d158b88e Mon Sep 17 00:00:00 2001 From: 4712 <4712@outlook.de> Date: Wed, 4 Nov 2015 14:30:25 +0100 Subject: [PATCH 3/4] Fix 1wire pass through for F3 + target config changes --- docs/Board - RMDO.md | 2 +- src/main/io/serial_1wire.c | 27 ++++++++++------------- src/main/io/serial_msp.c | 27 +++++++++++++++++++---- src/main/target/RMDO/target.h | 6 +++++ src/main/target/SPARKY/target.h | 6 +++++ src/main/target/STM32F3DISCOVERY/target.h | 12 +++++----- 6 files changed, 54 insertions(+), 26 deletions(-) diff --git a/docs/Board - RMDO.md b/docs/Board - RMDO.md index abd4cf6454..b90ccf5f3d 100644 --- a/docs/Board - RMDO.md +++ b/docs/Board - RMDO.md @@ -4,7 +4,7 @@ The DoDo board is a clone of the SPRacingF3 board in terms of CPU pin mappings. Hardware differences compared to SPRacingF3 are as follows: -* The CPU is the cheaper version of the F3 with only 128KB FLASH. +* Rev 1 and Rev 2: the CPU is the cheaper version of the F3 with only 128KB FLASH. Rev 3: the CPU is a F3 version with 256KB FLASH. * The external flash rom is the same size as found on the Naze32 (2MBit) * The barometer is the cheaper BMP280. * It does not have any compass sensor. diff --git a/src/main/io/serial_1wire.c b/src/main/io/serial_1wire.c index 62178f4df2..175edcf03e 100644 --- a/src/main/io/serial_1wire.c +++ b/src/main/io/serial_1wire.c @@ -26,7 +26,6 @@ #include "drivers/gpio.h" #include "drivers/light_led.h" -#include "drivers/system.h" #include "io/serial_1wire.h" const escHardware_t escHardware[ESC_COUNT] = { @@ -71,6 +70,13 @@ const escHardware_t escHardware[ESC_COUNT] = { { GPIOA, 2 }, { GPIOA, 3 }, { GPIOA, 8 } +#elif SPARKY + { GPIOB, 15 }, + { GPIOB, 14 }, + { GPIOA, 8 }, + { GPIOB, 0 }, + { GPIOA, 6 }, + { GPIOA, 2 } #endif }; @@ -90,11 +96,10 @@ void usb1WireInitialize() #ifdef BEEPER // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper off until reboot - gpio_set_mode(BEEP_GPIO, BEEP_PIN, Mode_IN_FLOATING); //GPIO_Mode_IPU + gpio_set_mode(BEEP_GPIO, BEEP_PIN, Mode_IN_FLOATING); // set input no pull up or down #endif } - #ifdef STM32F10X static volatile uint32_t in_cr_mask, out_cr_mask; @@ -132,8 +137,6 @@ static void gpioSetOne(uint32_t escIndex, GPIO_Mode mode) { } #endif -#define disable_hardware_uart __disable_irq() -#define enable_hardware_uart __enable_irq() #define ESC_HI(escIndex) ((escHardware[escIndex].gpio->IDR & (1U << escHardware[escIndex].pinpos)) != (uint32_t)Bit_RESET) #define RX_HI ((S1W_RX_GPIO->IDR & S1W_RX_PIN) != (uint32_t)Bit_RESET) #define ESC_SET_HI(escIndex) escHardware[escIndex].gpio->BSRR = (1U << escHardware[escIndex].pinpos) @@ -184,7 +187,7 @@ void usb1WirePassthrough(uint8_t escIndex) #endif //Disable all interrupts - disable_hardware_uart; + __disable_irq(); // reset all the pins GPIO_ResetBits(S1W_RX_GPIO, S1W_RX_PIN); @@ -219,7 +222,7 @@ void usb1WirePassthrough(uint8_t escIndex) // Wait for programmer to go 0 -> 1 uint32_t ct=3333; while(!RX_HI) { - if (ct > 0) ct--; //count down until 0; + if (ct > 0) ct--; // count down until 0; // check for low time ->ct=3333 ~600uS //byte LO time for 0 @ 19200 baud -> 9*52 uS => 468.75uS // App must send a 0 at 9600 baud (or lower) which has a LO time of at 104uS (or more) > 0 = 937.5uS LO // BLHeliSuite will use 4800 baud @@ -243,17 +246,11 @@ void usb1WirePassthrough(uint8_t escIndex) } } } - // we get here in case ct reached zero TX_SET_HIGH; RX_LED_OFF; - // Programmer TX - gpio_set_mode(S1W_TX_GPIO, S1W_TX_PIN, Mode_AF_PP); - // Enable Hardware UART - enable_hardware_uart; - // Wait a bit more to let App read the 0 byte and switch baudrate - // 2ms will most likely do the job, but give some grace time - delay(10); + // Enable all irq (for Hardware UART) + __enable_irq(); return; } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3b885dad28..02257e9880 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1756,13 +1756,32 @@ static bool processInCommand(void) headSerialReply(0); tailSerialReply(); // wait for all data to send - while (!isSerialTransmitBufferEmpty(mspSerialPort)) { - delay(50); - } + waitForSerialPortToFinishTransmitting(currentPort->port); // Start to activate here // motor 1 => index 0 + + // search currentPort portIndex + /* next lines seems to be unnecessary, because the currentPort always point to the same mspPorts[portIndex] + uint8_t portIndex; + for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + if (currentPort == &mspPorts[portIndex]) { + break; + } + } + */ + mspReleasePortIfAllocated(mspSerialPort); // CloseSerialPort also marks currentPort as UNUSED_PORT usb1WirePassthrough(i); - // MPS uart is active again + // Wait a bit more to let App read the 0 byte and switch baudrate + // 2ms will most likely do the job, but give some grace time + delay(10); + // rebuild/refill currentPort structure, does openSerialPort if marked UNUSED_PORT - used ports are skiped + mspAllocateSerialPorts(&masterConfig.serialConfig); + /* restore currentPort and mspSerialPort + setCurrentPort(&mspPorts[portIndex]); // not needed same index will be restored + */ + // former used MSP uart is active again + // restore MSP_SET_1WIRE as current command for correct headSerialReply(0) + currentPort->cmdMSP = MSP_SET_1WIRE; } else { // ESC channel higher than max. allowed // rem: BLHeliSuite will not support more than 8 diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index faccf9b806..1b8a73e3c9 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -163,3 +163,9 @@ // USART3, #define BIND_PORT GPIOB #define BIND_PIN Pin_11 +#define USE_SERIAL_1WIRE +#define ESC_COUNT 8 +#define S1W_TX_GPIO GPIOA +#define S1W_TX_PIN GPIO_Pin_9 +#define S1W_RX_GPIO GPIOA +#define S1W_RX_PIN GPIO_Pin_10 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index c40e3122e6..b107ebe71f 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -150,6 +150,12 @@ #define WS2811_IRQ DMA1_Channel7_IRQn #endif +#define USE_SERIAL_1WIRE +#define ESC_COUNT 6 +#define S1W_TX_GPIO GPIOB +#define S1W_TX_PIN GPIO_Pin_6 +#define S1W_RX_GPIO GPIOB +#define S1W_RX_PIN GPIO_Pin_7 #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 12e0aa39a6..844949ed3c 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -102,9 +102,9 @@ #define USE_SERIAL_1WIRE // How many escs does this board support? #define ESC_COUNT 6 -// STM32F3DISCOVERY TX - PC3 connects to UART RX -#define S1W_TX_GPIO GPIOC -#define S1W_TX_PIN GPIO_Pin_3 -// STM32F3DISCOVERY RX - PC1 connects to UART TX -#define S1W_RX_GPIO GPIOC -#define S1W_RX_PIN GPIO_Pin_1 +// STM32F3DISCOVERY TX - PD5 connects to UART RX +#define S1W_TX_GPIO GPIOD +#define S1W_TX_PIN GPIO_Pin_5 +// STM32F3DISCOVERY RX - PD6 connects to UART TX +#define S1W_RX_GPIO GPIOD +#define S1W_RX_PIN GPIO_Pin_6 From 9da66d5005f57192cde5db55fd9df2f963cc3d28 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 16 Oct 2015 22:00:25 +0200 Subject: [PATCH 4/4] I2C Overclock Feature This speeds up IMU speed significantly. F1 Targets like NAZE Looptime of ~900 is possible F3 Targets like SPRACINGF3 looptime of 370 is posible --- docs/Cli.md | 1 + src/main/config/config.c | 3 ++- src/main/config/config_master.h | 1 + src/main/drivers/bus_i2c.h | 1 + src/main/drivers/bus_i2c_stm32f10x.c | 13 ++++++++++++- src/main/drivers/bus_i2c_stm32f30x.c | 18 ++++++++++++++++-- src/main/io/serial_cli.c | 1 + src/main/main.c | 1 + 8 files changed, 35 insertions(+), 4 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 62f75cc2ad..7b5105da21 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -105,6 +105,7 @@ Re-apply any new defaults as desired. |---------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------|--------|---------------|--------------|----------| | `looptime` | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). Default of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. | 0 | 9000 | 3500 | Master | UINT16 | | `emf_avoidance` | Default value is 0 for 72MHz processor speed. Setting this to 1 increases the processor speed, to move the 6th harmonic away from 432MHz. | 0 | 1 | 0 | Master | UINT8 | +| `i2c_overclock` | Default value is 0 for disabled. Enabling this feature speeds up IMU speed significantly and faster looptimes are possible. | 0 | 1 | 0 | Master | UINT8 | | `mid_rc` | This is an important number to set in order to avoid trimming receiver/transmitter. Most standard receivers will have this at 1500, however Futaba transmitters will need this set to 1520. A way to find out if this needs to be changed, is to clear all trim/subtrim on transmitter, and connect to GUI. Note the value most channels idle at - this should be the number to choose. Once midrc is set, use subtrim on transmitter to make sure all channels (except throttle of course) are centered at midrc value. | 1200 | 1700 | 1500 | Master | UINT16 | | `min_check` | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | 0 | 2000 | 1100 | Master | UINT16 | | `max_check` | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | 0 | 2000 | 1900 | Master | UINT16 | diff --git a/src/main/config/config.c b/src/main/config/config.c index c88c60fb0e..f3387c1d15 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -132,7 +132,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 106; +static const uint8_t EEPROM_CONF_VERSION = 107; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -476,6 +476,7 @@ static void resetConf(void) masterConfig.looptime = 3500; masterConfig.emf_avoidance = 0; + masterConfig.i2c_overclock = 0; resetPidProfile(¤tProfile->pidProfile); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 71b29f22f4..b7836fef1f 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -27,6 +27,7 @@ typedef struct master_t { uint32_t enabledFeatures; uint16_t looptime; // imu loop time in us uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band + uint8_t i2c_overclock; // Overclock i2c Bus for faster IMU readings motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS]; #ifdef USE_SERVOS diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index 39db2094ec..68d1a52fc4 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -28,3 +28,4 @@ bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data); bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data); bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf); uint16_t i2cGetErrorCounter(void); +void i2cSetOverclock(uint8_t OverClock); diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index 38fde8e45c..591843c528 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -61,6 +61,11 @@ static const i2cDevice_t i2cHardwareMap[] = { static I2C_TypeDef *I2Cx = NULL; // Copy of device index for reinit, etc purposes static I2CDevice I2Cx_index; +static bool i2cOverClock; + +void i2cSetOverclock(uint8_t OverClock) { + i2cOverClock = (OverClock) ? true : false; +} void I2C1_ER_IRQHandler(void) { @@ -340,7 +345,13 @@ void i2cInit(I2CDevice index) i2c.I2C_Mode = I2C_Mode_I2C; i2c.I2C_DutyCycle = I2C_DutyCycle_2; i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - i2c.I2C_ClockSpeed = 400000; + + if (i2cOverClock) { + i2c.I2C_ClockSpeed = 800000; // 800khz Maximum speed tested on various boards without issues + } else { + i2c.I2C_ClockSpeed = 400000; // 400khz Operation according specs + } + I2C_Cmd(I2Cx, ENABLE); I2C_Init(I2Cx, &i2c); diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 80141ac636..9bd089d46d 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -69,6 +69,12 @@ static I2C_TypeDef *I2Cx = NULL; // I2C TimeoutUserCallback /////////////////////////////////////////////////////////////////////////////// +static bool i2cOverClock; + +void i2cSetOverclock(uint8_t OverClock) { + i2cOverClock = (OverClock) ? true : false; +} + uint32_t i2cTimeoutUserCallback(I2C_TypeDef *I2Cx) { if (I2Cx == I2C1) { @@ -118,7 +124,11 @@ void i2cInitPort(I2C_TypeDef *I2Cx) I2C_InitStructure.I2C_OwnAddress1 = 0x00; I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - I2C_InitStructure.I2C_Timing = 0x00E0257A; // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + if (i2cOverClock) { + I2C_InitStructure.I2C_Timing = 0x00500E30; // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. + } else { + I2C_InitStructure.I2C_Timing = 0x00E0257A; // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10 + } //I2C_InitStructure.I2C_Timing = 0x8000050B; I2C_Init(I2C1, &I2C_InitStructure); @@ -165,7 +175,11 @@ void i2cInitPort(I2C_TypeDef *I2Cx) // ^ when using this setting and after a few seconds of a scope probe being attached to the I2C bus it was observed that the bus enters // a busy state and does not recover. - I2C_InitStructure.I2C_Timing = 0x00E0257A; // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + if (i2cOverClock) { + I2C_InitStructure.I2C_Timing = 0x00500E30; // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. + } else { + I2C_InitStructure.I2C_Timing = 0x00E0257A; // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10 + } //I2C_InitStructure.I2C_Timing = 0x8000050B; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c3458a930b..430c357bf3 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -325,6 +325,7 @@ typedef struct { const clivalue_t valueTable[] = { { "looptime", VAR_UINT16 | MASTER_VALUE, &masterConfig.looptime, 0, 9000 }, { "emf_avoidance", VAR_UINT8 | MASTER_VALUE, &masterConfig.emf_avoidance, 0, 1 }, + { "i2c_overclock", VAR_UINT8 | MASTER_VALUE, &masterConfig.i2c_overclock, 0, 1 }, { "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, 1200, 1700 }, { "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, diff --git a/src/main/main.c b/src/main/main.c index 36c9d7c090..49c5b32288 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -169,6 +169,7 @@ void init(void) // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif + i2cSetOverclock(masterConfig.i2c_overclock); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision();