From d12d1952ebcb8c59de71ba8ad080448baea94ff6 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 12 Feb 2015 01:58:59 +0000 Subject: [PATCH 01/11] Fix PWM/UART2 clash on F1 targets caused by using wrong #define value. --- src/main/main.c | 2 +- src/main/version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index b3f01f9e6..d99a40fdc 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -295,7 +295,7 @@ void init(void) pwm_params.airplane = true; else pwm_params.airplane = false; -#if defined(SERIAL_PORT_USART2) && defined(STM32F10X) +#if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif pwm_params.useVbat = feature(FEATURE_VBAT); diff --git a/src/main/version.h b/src/main/version.h index faf7da75e..eb315aaae 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 7 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) From 9a7de3cf3c691b8cfe0d2723d9cfbaf823f2df89 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 14 Feb 2015 23:49:55 +0000 Subject: [PATCH 02/11] Improve ESC compatibility by enabling PWM output as soon as possible. A user reported a problem where if the board was powered up before connecting the main battery the ESCs would work, however when the board and ESCs were powered at the same time the ESCs would not initialise correctly. --- src/main/main.c | 72 ++++++++++++++++++++++++------------------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index d99a40fdc..cbf0e50be 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -178,6 +178,42 @@ void init(void) timerInit(); // timer must be initialized before any channel is allocated + memset(&pwm_params, 0, sizeof(pwm_params)); + // when using airplane/wing mixer, servo/motor outputs are remapped + if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) + pwm_params.airplane = true; + else + pwm_params.airplane = false; +#if defined(USE_USART2) && defined(STM32F10X) + pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); +#endif + pwm_params.useVbat = feature(FEATURE_VBAT); + pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); + pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); + pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); + pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) + && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; + pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); + pwm_params.usePPM = feature(FEATURE_RX_PPM); + pwm_params.useOneshot = feature(FEATURE_ONESHOT125); + pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); + pwm_params.useServos = isMixerUsingServos(); + pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; + pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; + pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; + pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) + if (feature(FEATURE_3D)) + pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; + if (pwm_params.motorPwmRate > 500) + pwm_params.idlePulse = 0; // brushed motors + pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; + + pwmRxInit(masterConfig.inputFilteringMode); + + pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); + + mixerUsePWMOutputConfiguration(pwmOutputConfiguration); + #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPin = BEEP_PIN, @@ -289,42 +325,6 @@ void init(void) serialInit(&masterConfig.serialConfig); - memset(&pwm_params, 0, sizeof(pwm_params)); - // when using airplane/wing mixer, servo/motor outputs are remapped - if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) - pwm_params.airplane = true; - else - pwm_params.airplane = false; -#if defined(USE_USART2) && defined(STM32F10X) - pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); -#endif - pwm_params.useVbat = feature(FEATURE_VBAT); - pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); - pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); - pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); - pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) - && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; - pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); - pwm_params.usePPM = feature(FEATURE_RX_PPM); - pwm_params.useOneshot = feature(FEATURE_ONESHOT125); - pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); - pwm_params.useServos = isMixerUsingServos(); - pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; - pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; - pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; - pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) - if (feature(FEATURE_3D)) - pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; - if (pwm_params.motorPwmRate > 500) - pwm_params.idlePulse = 0; // brushed motors - pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; - - pwmRxInit(masterConfig.inputFilteringMode); - - pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); - - mixerUsePWMOutputConfiguration(pwmOutputConfiguration); - failsafe = failsafeInit(&masterConfig.rxConfig); beepcodeInit(failsafe); rxInit(&masterConfig.rxConfig, failsafe); From cd73c253525fe1eb06f91882d1dbeb530fcb835e Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 14 Feb 2015 23:50:51 +0000 Subject: [PATCH 03/11] PORT103R - Update default hardware support. --- src/main/target/PORT103R/target.h | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index d371f6972..0f4aa3904 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -19,9 +19,17 @@ #define TARGET_BOARD_IDENTIFIER "103R" -#define LED0_GPIO GPIOD -#define LED0_PIN Pin_2 // PD2 (LED) -#define LED0_PERIPHERAL RCC_APB2Periph_GPIOD +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_3 // PB3 (LED) +#define LED0_PERIPHERAL RCC_APB2Periph_GPIOB + +#define LED1_GPIO GPIOB +#define LED1_PIN Pin_4 // PB4 (LED) +#define LED1_PERIPHERAL RCC_APB2Periph_GPIOB + +#define LED2_GPIO GPIOD +#define LED2_PIN Pin_2 // PD2 (LED) - Labelled LED4 +#define LED2_PERIPHERAL RCC_APB2Periph_GPIOD #define BEEP_GPIO GPIOA #define BEEP_PIN Pin_12 // PA12 (Beeper) @@ -66,12 +74,17 @@ #define BARO #define USE_BARO_MS5611 -//#define USE_BARO_BMP085 +#define USE_BARO_BMP085 #define MAG +#define USE_MAG_HMC5883 +#define USE_MAG_AK8975 + #define SONAR #define BEEPER #define LED0 +#define LED1 +#define LED2 #define INVERTER #define DISPLAY From 02166a4fd0891b1df58bb4edf3ea7a9bad8b408f Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 15 Feb 2015 09:20:33 +0000 Subject: [PATCH 04/11] Ensure mixer is configured so that PWM initialisation is correct. The isMixerUsingServos() was returing the wrong value in 9a7de3cf3c691b8cfe0d2723d9cfbaf823f2df89 --- src/main/main.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index cbf0e50be..f83d5e23f 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -178,6 +178,8 @@ void init(void) timerInit(); // timer must be initialized before any channel is allocated + mixerInit(masterConfig.mixerMode, masterConfig.customMixer); + memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) @@ -314,15 +316,13 @@ void init(void) LED0_OFF; LED1_OFF; - - imuInit(); - mixerInit(masterConfig.mixerMode, masterConfig.customMixer); - #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif + imuInit(); + serialInit(&masterConfig.serialConfig); failsafe = failsafeInit(&masterConfig.rxConfig); From 36c7d5d30de70b5a9bc215145a2457f710bcddc4 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 16 Feb 2015 19:40:56 +0000 Subject: [PATCH 05/11] SPRACINGF3 - Fix typo in UART3 configuration. --- src/main/drivers/serial_uart_stm32f30x.c | 8 ++++---- src/main/target/SPRACINGF3/target.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 3d8ee674c..f7bc087e0 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -64,7 +64,7 @@ #ifndef UART3_GPIO #define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7) #define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7) -#define UART2_GPIO_AF GPIO_AF_7 +#define UART3_GPIO_AF GPIO_AF_7 #define UART3_GPIO GPIOB #define UART3_TX_PINSOURCE GPIO_PinSource10 #define UART3_RX_PINSOURCE GPIO_PinSource11 @@ -270,20 +270,20 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) if (mode & MODE_TX) { GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN; - GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, GPIO_AF_7); + GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF); GPIO_Init(UART3_GPIO, &GPIO_InitStructure); } if (mode & MODE_RX) { GPIO_InitStructure.GPIO_Pin = UART3_RX_PIN; - GPIO_PinAFConfig(UART3_GPIO, UART3_RX_PINSOURCE, GPIO_AF_7); + GPIO_PinAFConfig(UART3_GPIO, UART3_RX_PINSOURCE, UART3_GPIO_AF); GPIO_Init(UART3_GPIO, &GPIO_InitStructure); } if (mode & MODE_BIDIR) { GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN; GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; - GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, GPIO_AF_7); + GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF); GPIO_Init(UART3_GPIO, &GPIO_InitStructure); } diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 5f4ce6693..ae1feff44 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -72,7 +72,7 @@ #ifndef UART3_GPIO #define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7) #define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7) -#define UART2_GPIO_AF GPIO_AF_7 +#define UART3_GPIO_AF GPIO_AF_7 #define UART3_GPIO GPIOB #define UART3_TX_PINSOURCE GPIO_PinSource10 #define UART3_RX_PINSOURCE GPIO_PinSource11 From 6e258a62acaf74ebcf73fff0f77bdf8144dc2f86 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 16 Feb 2015 22:45:47 +0000 Subject: [PATCH 06/11] Disable alt-hold debugging. --- src/main/flight/altitudehold.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 66195165a..16259b381 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -292,7 +292,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) vel += vel_acc; -#if 1 +#ifdef DEBUG_ALT_HOLD debug[1] = accSum[2] / accSumCount; // acceleration debug[2] = vel; // velocity debug[3] = accAlt; // height From 24ce82b28000f7e04eda2a18beefbc87d940171c Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 16 Feb 2015 22:50:39 +0000 Subject: [PATCH 07/11] Improve SBus compatibility by timing the entire frame instead of the gap between received bytes of data. Add support for the two SBus digital channels. --- src/main/rx/sbus.c | 84 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 64 insertions(+), 20 deletions(-) diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 6df723db0..b7ced34dc 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -35,10 +35,27 @@ #include "rx/rx.h" #include "rx/sbus.h" +/* + * Observations + * + * FrSky X8R + * time between frames: 6ms. + * time to send frame: 3ms. + */ + +#define SBUS_TIME_NEEDED_PER_FRAME 3000 + #define DEBUG_SBUS_PACKETS +#ifdef DEBUG_SBUS_PACKETS +static uint16_t sbusStateFlags = 0; -#define SBUS_MAX_CHANNEL 16 +#define SBUS_STATE_FAILSAFE (1 << 0) +#define SBUS_STATE_SIGNALLOSS (1 << 1) + +#endif + +#define SBUS_MAX_CHANNEL 18 #define SBUS_FRAME_SIZE 25 #define SBUS_FRAME_BEGIN_BYTE 0x0F @@ -46,6 +63,9 @@ #define SBUS_BAUDRATE 100000 +#define SBUS_DIGITAL_CHANNEL_MIN 173 +#define SBUS_DIGITAL_CHANNEL_MAX 1812 + static bool sbusFrameDone = false; static void sbusDataReceive(uint16_t c); static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); @@ -53,7 +73,7 @@ static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; static serialPort_t *sBusPort; -static uint32_t sbusSignalLostEventCount = 0; + void sbusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) { @@ -77,8 +97,8 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return sBusPort != NULL; } -#define SBUS_FLAG_RESERVED_1 (1 << 0) -#define SBUS_FLAG_RESERVED_2 (1 << 1) +#define SBUS_FLAG_CHANNEL_17 (1 << 0) +#define SBUS_FLAG_CHANNEL_18 (1 << 1) #define SBUS_FLAG_SIGNAL_LOSS (1 << 2) #define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3) @@ -115,23 +135,23 @@ static sbusFrame_t sbusFrame; // Receive ISR callback static void sbusDataReceive(uint16_t c) { -#ifdef DEBUG_SBUS_PACKETS - static uint8_t sbusUnusedFrameCount = 0; -#endif - static uint8_t sbusFramePosition = 0; - static uint32_t sbusTimeoutAt = 0; + static uint32_t sbusFrameStartAt = 0; uint32_t now = micros(); - if ((int32_t)(sbusTimeoutAt - now) < 0) { + int32_t sbusFrameTime = now - sbusFrameStartAt; + + if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) { sbusFramePosition = 0; } - sbusTimeoutAt = now + 2500; sbusFrame.bytes[sbusFramePosition] = (uint8_t)c; - if (sbusFramePosition == 0 && c != SBUS_FRAME_BEGIN_BYTE) { - return; + if (sbusFramePosition == 0) { + if (c != SBUS_FRAME_BEGIN_BYTE) { + return; + } + sbusFrameStartAt = now; } sbusFramePosition++; @@ -139,14 +159,11 @@ static void sbusDataReceive(uint16_t c) if (sbusFramePosition == SBUS_FRAME_SIZE) { if (sbusFrame.frame.endByte == SBUS_FRAME_END_BYTE) { sbusFrameDone = true; - } - sbusFramePosition = 0; - } else { #ifdef DEBUG_SBUS_PACKETS - if (sbusFrameDone) { - sbusUnusedFrameCount++; - } + debug[2] = sbusFrameTime; #endif + } + } else { sbusFrameDone = false; } } @@ -158,11 +175,22 @@ bool sbusFrameComplete(void) } sbusFrameDone = false; +#ifdef DEBUG_SBUS_PACKETS + debug[1] = sbusFrame.frame.flags; +#endif + if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) { // internal failsafe enabled and rx failsafe flag set - sbusSignalLostEventCount++; +#ifdef DEBUG_SBUS_PACKETS + sbusStateFlags |= SBUS_STATE_SIGNALLOSS; + debug[0] = sbusStateFlags; +#endif } if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) { +#ifdef DEBUG_SBUS_PACKETS + sbusStateFlags |= SBUS_STATE_FAILSAFE; + debug[0] = sbusStateFlags; +#endif // internal failsafe enabled and rx failsafe flag set return false; } @@ -183,6 +211,22 @@ bool sbusFrameComplete(void) sbusChannelData[13] = sbusFrame.frame.chan13; sbusChannelData[14] = sbusFrame.frame.chan14; sbusChannelData[15] = sbusFrame.frame.chan15; + + if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_17) { + sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX; + } else { + sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN; + } + + if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_18) { + sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX; + } else { + sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN; + } + +#ifdef DEBUG_SBUS_PACKETS + sbusStateFlags = 0; +#endif return true; } From c05b4c639ba1be4ad44ac0242eb48fc981a40de0 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 16 Feb 2015 23:02:14 +0000 Subject: [PATCH 08/11] Ensure sbus debug output is reset after failsafe/signal loss conditions. --- src/main/rx/sbus.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index b7ced34dc..b84fa47de 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -226,6 +226,7 @@ bool sbusFrameComplete(void) #ifdef DEBUG_SBUS_PACKETS sbusStateFlags = 0; + debug[0] = sbusStateFlags; #endif return true; } From 70fbeb22ba452307ab83bb68076d4c325d3c4000 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 16 Feb 2015 23:06:46 +0000 Subject: [PATCH 09/11] Disable SBUS debug on CJMCU to reduce code size. --- src/main/rx/sbus.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index b84fa47de..aeaf16f4d 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -45,7 +45,9 @@ #define SBUS_TIME_NEEDED_PER_FRAME 3000 +#ifndef CJMCU #define DEBUG_SBUS_PACKETS +#endif #ifdef DEBUG_SBUS_PACKETS static uint16_t sbusStateFlags = 0; @@ -183,7 +185,7 @@ bool sbusFrameComplete(void) // internal failsafe enabled and rx failsafe flag set #ifdef DEBUG_SBUS_PACKETS sbusStateFlags |= SBUS_STATE_SIGNALLOSS; - debug[0] = sbusStateFlags; + debug[0] |= SBUS_STATE_SIGNALLOSS; #endif } if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) { From 8c1d9c37d9db18fd050767076f91b0227f31202b Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 18 Feb 2015 00:36:05 +0000 Subject: [PATCH 10/11] Add comment regarding observations made on the Futaba SBus receivers. --- src/main/rx/sbus.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index aeaf16f4d..85dd83ebf 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -41,6 +41,10 @@ * FrSky X8R * time between frames: 6ms. * time to send frame: 3ms. +* + * Futaba R6208SB/R6303SB + * time between frames: 11ms. + * time to send frame: 3ms. */ #define SBUS_TIME_NEEDED_PER_FRAME 3000 From e554f73f5cef824830054c96bff1baeba6fac692 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 18 Feb 2015 19:29:47 +0000 Subject: [PATCH 11/11] Update the sensors page to show inclination, heading, estimated G and the value for small angle. The latter is to help get field reports to help with a problem where sometimes the aircraft won't arm because the FC thinks it's tilted over too much. --- src/main/flight/imu.h | 4 ++-- src/main/io/display.c | 39 +++++++++++++++++++++++++++++++++++---- 2 files changed, 37 insertions(+), 6 deletions(-) diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9d1c83110..9a632f570 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -21,11 +21,11 @@ extern int16_t throttleAngleCorrection; extern uint32_t accTimeSum; extern int accSumCount; extern float accVelScale; - +extern t_fp_vector EstG; extern int16_t accSmooth[XYZ_AXIS_COUNT]; extern int32_t accSum[XYZ_AXIS_COUNT]; extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT]; - +extern int16_t smallAngle; typedef struct rollAndPitchInclination_s { // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 diff --git a/src/main/io/display.c b/src/main/io/display.c index 849560f8d..0d054f4ad 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -35,6 +35,7 @@ #include "common/printf.h" #include "common/maths.h" #include "common/axis.h" +#include "common/typeconversion.h" #ifdef DISPLAY @@ -384,20 +385,20 @@ void showBatteryPage(void) void showSensorsPage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - static const char *format = "%c = %5d %5d %5d"; + static const char *format = "%s %5d %5d %5d"; i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(" X Y Z"); if (sensors(SENSOR_ACC)) { - tfp_sprintf(lineBuffer, format, 'A', accSmooth[X], accSmooth[Y], accSmooth[Z]); + tfp_sprintf(lineBuffer, format, "ACC", accSmooth[X], accSmooth[Y], accSmooth[Z]); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); } if (sensors(SENSOR_GYRO)) { - tfp_sprintf(lineBuffer, format, 'G', gyroADC[X], gyroADC[Y], gyroADC[Z]); + tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); @@ -405,12 +406,42 @@ void showSensorsPage(void) #ifdef MAG if (sensors(SENSOR_MAG)) { - tfp_sprintf(lineBuffer, format, 'M', magADC[X], magADC[Y], magADC[Z]); + tfp_sprintf(lineBuffer, format, "MAG", magADC[X], magADC[Y], magADC[Z]); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); } #endif + + tfp_sprintf(lineBuffer, format, "I&H", inclination.values.rollDeciDegrees, inclination.values.pitchDeciDegrees, heading); + padLineBuffer(); + i2c_OLED_set_line(rowIndex++); + i2c_OLED_send_string(lineBuffer); + + uint8_t length; + + ftoa(EstG.A[X], lineBuffer); + length = strlen(lineBuffer); + while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) { + lineBuffer[length++] = ' '; + lineBuffer[length+1] = 0; + } + ftoa(EstG.A[Y], lineBuffer + length); + padLineBuffer(); + i2c_OLED_set_line(rowIndex++); + i2c_OLED_send_string(lineBuffer); + + ftoa(EstG.A[Z], lineBuffer); + length = strlen(lineBuffer); + while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) { + lineBuffer[length++] = ' '; + lineBuffer[length+1] = 0; + } + ftoa(smallAngle, lineBuffer + length); + padLineBuffer(); + i2c_OLED_set_line(rowIndex++); + i2c_OLED_send_string(lineBuffer); + } #ifdef ENABLE_DEBUG_OLED_PAGE