From 6888de5192c7e585293a736e4c116e85f96ff10b Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Fri, 26 Dec 2014 15:13:59 +0100 Subject: [PATCH 001/182] Fix ADC clock speed to be within specification ADC clock speed may be slightly higher, but there is no benefit in current code --- src/main/drivers/adc_stm32f10x.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index ae6680c4ef..15b6049777 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -117,6 +117,9 @@ void adcInit(drv_adc_config_t *init) adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5; } + + RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); From e8e53ab7f19bc21d6d1b7deffcfa05f401ef629a Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sat, 27 Dec 2014 14:01:40 +0100 Subject: [PATCH 002/182] Added initial suport for Align JR01 DMSS (i.e. satellite receiver for JR systems) support. --- src/main/io/serial.c | 6 +-- src/main/rx/rx.c | 3 ++ src/main/rx/rx.h | 3 +- src/main/rx/xbus.c | 115 +++++++++++++++++++++++++++++++++++++------ 4 files changed, 109 insertions(+), 18 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 4f569b8aaa..2799cde91e 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -119,8 +119,8 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { }; static const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { - {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, - {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART1, 9600, 250000, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART2, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #if (SERIAL_PORT_COUNT > 2) {SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}, {SERIAL_PORT_SOFTSERIAL2, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE} @@ -134,7 +134,7 @@ const functionConstraint_t functionConstraints[] = { { FUNCTION_GPS, 9600, 115200, AUTOBAUD, SPF_NONE }, { FUNCTION_GPS_PASSTHROUGH, 9600, 115200, NO_AUTOBAUD, SPF_NONE }, { FUNCTION_MSP, 9600, 115200, NO_AUTOBAUD, SPF_NONE }, - { FUNCTION_SERIAL_RX, 9600, 115200, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK }, + { FUNCTION_SERIAL_RX, 9600, 250000, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK }, { FUNCTION_TELEMETRY, 9600, 19200, NO_AUTOBAUD, SPF_NONE }, { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600, NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE } }; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 60ebc97263..4c147aa684 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -102,6 +102,7 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo sumhUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; case SERIALRX_XBUS_MODE_B: + case SERIALRX_XBUS_MODE_B_JR01: xBusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; } @@ -158,6 +159,7 @@ void serialRxInit(rxConfig_t *rxConfig) enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_XBUS_MODE_B: + case SERIALRX_XBUS_MODE_B_JR01: enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; } @@ -182,6 +184,7 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig) case SERIALRX_SUMH: return sumhFrameComplete(); case SERIALRX_XBUS_MODE_B: + case SERIALRX_XBUS_MODE_B_JR01: return xBusFrameComplete(); } return false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 92ae2eb0f2..5de472a526 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -33,7 +33,8 @@ typedef enum { SERIALRX_SUMD = 3, SERIALRX_SUMH = 4, SERIALRX_XBUS_MODE_B = 5, - SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B + SERIALRX_XBUS_MODE_B_JR01 = 6, + SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B_JR01 } SerialRXType; #define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 9125151d77..11ec52075f 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -35,17 +35,19 @@ // #define XBUS_CHANNEL_COUNT 12 +#define XBUS_JR01_CHANNEL_COUNT 12 // Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27 #define XBUS_FRAME_SIZE 27 -#define XBUS_CRC_BYTE_1 25 -#define XBUS_CRC_BYTE_2 26 + +#define XBUS_JR01_FRAME_SIZE 33 #define XBUS_CRC_AND_VALUE 0x8000 #define XBUS_CRC_POLY 0x1021 #define XBUS_BAUDRATE 115200 -#define XBUS_MAX_FRAME_TIME 5000 +#define XBUS_JR01_BAUDRATE 250000 +#define XBUS_MAX_FRAME_TIME 8000 // NOTE! // This is actually based on ID+LENGTH (nibble each) @@ -68,9 +70,14 @@ static bool xBusFrameReceived = false; static bool xBusDataIncoming = false; static uint8_t xBusFramePosition; +static uint8_t xBusFrameLength; +static uint8_t xBusChannelCount; +static uint8_t xBusProvider; -static volatile uint8_t xBusFrame[XBUS_FRAME_SIZE]; -static uint16_t xBusChannelData[XBUS_CHANNEL_COUNT]; + +// Use max values for ram areas +static volatile uint8_t xBusFrame[XBUS_JR01_FRAME_SIZE]; +static uint16_t xBusChannelData[XBUS_JR01_CHANNEL_COUNT]; static void xBusDataReceive(uint16_t c); static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); @@ -80,25 +87,41 @@ static serialPort_t *xBusPort; void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) { functionConstraint->minBaudRate = XBUS_BAUDRATE; - functionConstraint->maxBaudRate = XBUS_BAUDRATE; + functionConstraint->maxBaudRate = XBUS_JR01_BAUDRATE; functionConstraint->requiredSerialPortFeatures = SPF_SUPPORTS_CALLBACK; } bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { + uint32_t baudRate; + switch (rxConfig->serialrx_provider) { case SERIALRX_XBUS_MODE_B: rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT; xBusFrameReceived = false; xBusDataIncoming = false; xBusFramePosition = 0; + baudRate = XBUS_BAUDRATE; + xBusFrameLength = XBUS_FRAME_SIZE; + xBusChannelCount = XBUS_CHANNEL_COUNT; + xBusProvider = SERIALRX_XBUS_MODE_B; + break; + case SERIALRX_XBUS_MODE_B_JR01: + rxRuntimeConfig->channelCount = XBUS_JR01_CHANNEL_COUNT; + xBusFrameReceived = false; + xBusDataIncoming = false; + xBusFramePosition = 0; + baudRate = XBUS_JR01_BAUDRATE; + xBusFrameLength = XBUS_JR01_FRAME_SIZE; + xBusChannelCount = XBUS_JR01_CHANNEL_COUNT; + xBusProvider = SERIALRX_XBUS_MODE_B_JR01; break; default: return false; break; } - xBusPort = openSerialPort(FUNCTION_SERIAL_RX, xBusDataReceive, XBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); + xBusPort = openSerialPort(FUNCTION_SERIAL_RX, xBusDataReceive, baudRate, MODE_RX, SERIAL_NOT_INVERTED); if (callback) { *callback = xBusReadRawRC; } @@ -123,7 +146,7 @@ static uint16_t xBusCRC16(uint16_t crc, uint8_t value) return crc; } -static void xBusUnpackFrame(void) +static void xBusUnpackModeBFrame(void) { // Calculate the CRC of the incoming frame uint16_t crc = 0; @@ -133,17 +156,17 @@ static void xBusUnpackFrame(void) uint8_t frameAddr; // Calculate on all bytes except the final two CRC bytes - for (i = 0; i < XBUS_FRAME_SIZE - 2; i++) { + for (i = 0; i < xBusFrameLength - 2; i++) { inCrc = xBusCRC16(inCrc, xBusFrame[i]); } // Get the received CRC - crc = ((uint16_t)xBusFrame[XBUS_CRC_BYTE_1]) << 8; - crc = crc + ((uint16_t)xBusFrame[XBUS_CRC_BYTE_2]); + crc = ((uint16_t)xBusFrame[xBusFrameLength-2]) << 8; + crc = crc + ((uint16_t)xBusFrame[xBusFrameLength-1]); if (crc == inCrc) { // Unpack the data, we have a valid frame - for (i = 0; i < XBUS_CHANNEL_COUNT; i++) { + for (i = 0; i < xBusChannelCount; i++) { frameAddr = 1 + i * 2; value = ((uint16_t)xBusFrame[frameAddr]) << 8; @@ -158,9 +181,68 @@ static void xBusUnpackFrame(void) } +static void xBusUnpackJr01Frame(void) +{ + // Calculate the CRC of the incoming frame + uint16_t crc = 0; + uint16_t inCrc = 0; + uint8_t i = 0; + uint16_t value; + uint8_t frameAddr; + + // When using the Align JR01 receiver with + // a MODE B setting in the radio (XG14 tested) + // the MODE_B -frame is packed within some + // at the moment unknown bytes before and after: + // 0xA1 __ __ 0xA1 12*(High + Low) CRC1 CRC2 + __ __ __ + // Compared to a standard MODE B frame that only + // contains the "middle" package + // Hence, at the moment, the unknown header and footer + // of the JR01 MODEB packages are discarded. This is + // ok as the CRC-checksum of the embedded package works + // out nicely. + + // Calculate CRC bytes of the "embedded MODE B frame" + for (i = 3; i < xBusFrameLength - 5; i++) { + inCrc = xBusCRC16(inCrc, xBusFrame[i]); + } + + // Get the received CRC (of the "embedded MODE B frame") + crc = ((uint16_t)xBusFrame[xBusFrameLength-5]) << 8; + crc = crc + ((uint16_t)xBusFrame[xBusFrameLength-4]); + + if (crc == inCrc) { + // Unpack the data, we have a valid frame + for (i = 0; i < xBusChannelCount; i++) { + + frameAddr = 4 + i * 2; + value = ((uint16_t)xBusFrame[frameAddr]) << 8; + value = value + ((uint16_t)xBusFrame[frameAddr + 1]); + + // Convert to internal format + xBusChannelData[i] = XBUS_CONVERT_TO_USEC(value); + } + + xBusFrameReceived = true; + } + +} + // Receive ISR callback static void xBusDataReceive(uint16_t c) { + uint32_t now; + static uint32_t xBusTimeLast, xBusTimeInterval; + + // Check if we shall reset frame position due to time + now = micros(); + xBusTimeInterval = now - xBusTimeLast; + xBusTimeLast = now; + if (xBusTimeInterval > XBUS_MAX_FRAME_TIME) { + xBusFramePosition = 0; + xBusDataIncoming = false; + } + // Check if we shall start a frame? if ((xBusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) { xBusDataIncoming = true; @@ -174,8 +256,13 @@ static void xBusDataReceive(uint16_t c) } // Done? - if (xBusFramePosition == XBUS_FRAME_SIZE) { - xBusUnpackFrame(); + if (xBusFramePosition == xBusFrameLength) { + switch (xBusProvider) { + case SERIALRX_XBUS_MODE_B: + xBusUnpackModeBFrame(); + case SERIALRX_XBUS_MODE_B_JR01: + xBusUnpackJr01Frame(); + } xBusDataIncoming = false; xBusFramePosition = 0; } From d3b21f8e36afeaf5974455088f0776eece91f573 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sat, 27 Dec 2014 14:52:06 +0100 Subject: [PATCH 003/182] Messed up the numbers, now changed to correct RJ01. --- src/main/rx/rx.c | 6 +++--- src/main/rx/rx.h | 4 ++-- src/main/rx/xbus.c | 34 +++++++++++++++++----------------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 4c147aa684..094766ab67 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -102,7 +102,7 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo sumhUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; case SERIALRX_XBUS_MODE_B: - case SERIALRX_XBUS_MODE_B_JR01: + case SERIALRX_XBUS_MODE_B_RJ01: xBusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; } @@ -159,7 +159,7 @@ void serialRxInit(rxConfig_t *rxConfig) enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_XBUS_MODE_B: - case SERIALRX_XBUS_MODE_B_JR01: + case SERIALRX_XBUS_MODE_B_RJ01: enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; } @@ -184,7 +184,7 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig) case SERIALRX_SUMH: return sumhFrameComplete(); case SERIALRX_XBUS_MODE_B: - case SERIALRX_XBUS_MODE_B_JR01: + case SERIALRX_XBUS_MODE_B_RJ01: return xBusFrameComplete(); } return false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 5de472a526..b88e0c5903 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -33,8 +33,8 @@ typedef enum { SERIALRX_SUMD = 3, SERIALRX_SUMH = 4, SERIALRX_XBUS_MODE_B = 5, - SERIALRX_XBUS_MODE_B_JR01 = 6, - SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B_JR01 + SERIALRX_XBUS_MODE_B_RJ01 = 6, + SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B_RJ01 } SerialRXType; #define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 11ec52075f..ed3015f6ac 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -35,18 +35,18 @@ // #define XBUS_CHANNEL_COUNT 12 -#define XBUS_JR01_CHANNEL_COUNT 12 +#define XBUS_RJ01_CHANNEL_COUNT 12 // Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27 #define XBUS_FRAME_SIZE 27 -#define XBUS_JR01_FRAME_SIZE 33 +#define XBUS_RJ01_FRAME_SIZE 33 #define XBUS_CRC_AND_VALUE 0x8000 #define XBUS_CRC_POLY 0x1021 #define XBUS_BAUDRATE 115200 -#define XBUS_JR01_BAUDRATE 250000 +#define XBUS_RJ01_BAUDRATE 250000 #define XBUS_MAX_FRAME_TIME 8000 // NOTE! @@ -76,8 +76,8 @@ static uint8_t xBusProvider; // Use max values for ram areas -static volatile uint8_t xBusFrame[XBUS_JR01_FRAME_SIZE]; -static uint16_t xBusChannelData[XBUS_JR01_CHANNEL_COUNT]; +static volatile uint8_t xBusFrame[XBUS_RJ01_FRAME_SIZE]; +static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT]; static void xBusDataReceive(uint16_t c); static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); @@ -87,7 +87,7 @@ static serialPort_t *xBusPort; void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) { functionConstraint->minBaudRate = XBUS_BAUDRATE; - functionConstraint->maxBaudRate = XBUS_JR01_BAUDRATE; + functionConstraint->maxBaudRate = XBUS_RJ01_BAUDRATE; functionConstraint->requiredSerialPortFeatures = SPF_SUPPORTS_CALLBACK; } @@ -106,15 +106,15 @@ bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa xBusChannelCount = XBUS_CHANNEL_COUNT; xBusProvider = SERIALRX_XBUS_MODE_B; break; - case SERIALRX_XBUS_MODE_B_JR01: - rxRuntimeConfig->channelCount = XBUS_JR01_CHANNEL_COUNT; + case SERIALRX_XBUS_MODE_B_RJ01: + rxRuntimeConfig->channelCount = XBUS_RJ01_CHANNEL_COUNT; xBusFrameReceived = false; xBusDataIncoming = false; xBusFramePosition = 0; - baudRate = XBUS_JR01_BAUDRATE; - xBusFrameLength = XBUS_JR01_FRAME_SIZE; - xBusChannelCount = XBUS_JR01_CHANNEL_COUNT; - xBusProvider = SERIALRX_XBUS_MODE_B_JR01; + baudRate = XBUS_RJ01_BAUDRATE; + xBusFrameLength = XBUS_RJ01_FRAME_SIZE; + xBusChannelCount = XBUS_RJ01_CHANNEL_COUNT; + xBusProvider = SERIALRX_XBUS_MODE_B_RJ01; break; default: return false; @@ -181,7 +181,7 @@ static void xBusUnpackModeBFrame(void) } -static void xBusUnpackJr01Frame(void) +static void xBusUnpackRJ01Frame(void) { // Calculate the CRC of the incoming frame uint16_t crc = 0; @@ -190,7 +190,7 @@ static void xBusUnpackJr01Frame(void) uint16_t value; uint8_t frameAddr; - // When using the Align JR01 receiver with + // When using the Align RJ01 receiver with // a MODE B setting in the radio (XG14 tested) // the MODE_B -frame is packed within some // at the moment unknown bytes before and after: @@ -198,7 +198,7 @@ static void xBusUnpackJr01Frame(void) // Compared to a standard MODE B frame that only // contains the "middle" package // Hence, at the moment, the unknown header and footer - // of the JR01 MODEB packages are discarded. This is + // of the RJ01 MODEB packages are discarded. This is // ok as the CRC-checksum of the embedded package works // out nicely. @@ -260,8 +260,8 @@ static void xBusDataReceive(uint16_t c) switch (xBusProvider) { case SERIALRX_XBUS_MODE_B: xBusUnpackModeBFrame(); - case SERIALRX_XBUS_MODE_B_JR01: - xBusUnpackJr01Frame(); + case SERIALRX_XBUS_MODE_B_RJ01: + xBusUnpackRJ01Frame(); } xBusDataIncoming = false; xBusFramePosition = 0; From 6b7c9facd3c511f6feaf36611072be856e842f55 Mon Sep 17 00:00:00 2001 From: Ben Hitchcock Date: Sun, 4 Jan 2015 22:46:01 +0800 Subject: [PATCH 004/182] First draft of better horizon mode --- src/main/config/config.c | 1 + src/main/flight/flight.c | 21 ++++++++++++++++++++- src/main/flight/flight.h | 1 + src/main/io/serial_cli.c | 1 + src/main/io/serial_msp.c | 4 ++-- 5 files changed, 25 insertions(+), 3 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 938613caaf..ad55a40972 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -159,6 +159,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D_f[YAW] = 0.05f; pidProfile->A_level = 5.0f; pidProfile->H_level = 3.0f; + pidProfile->H_sensitivity = 10.0f; } #ifdef GPS diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 22ee35fc93..b55ef9cd80 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -101,9 +101,28 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control float delta, deltaSum; float dT; int axis; + float horizonLevelStrength = 1; dT = (float)cycleTime * 0.000001f; + if (FLIGHT_MODE(HORIZON_MODE)) { + + if(abs(rcCommand[FD_ROLL]) > abs(rcCommand[FD_PITCH])){ + axis = FD_ROLL; + } + else { + axis = FD_PITCH; + } + + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - abs(rcCommand[axis])) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->H_sensitivity == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (10 / pidProfile->H_sensitivity)) + 1, 0, 1); + } + } + // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode @@ -134,7 +153,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRate to add a little auto-level feel - AngleRate += errorAngle * pidProfile->H_level; + AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; } } } diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index a6fadf55f6..6461ce21e8 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -42,6 +42,7 @@ typedef struct pidProfile_s { float D_f[3]; float A_level; float H_level; + float H_sensitivity; } pidProfile_t; typedef enum { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2628ba93a7..b1a3156300 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -392,6 +392,7 @@ const clivalue_t valueTable[] = { { "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 }, { "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 }, + { "sensitivity_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 }, { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 }, { "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b43409bf54..5c2d834eed 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -846,7 +846,7 @@ static bool processOutCommand(uint8_t cmdMSP) if (i == PIDLEVEL) { serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250)); - serialize8(0); + serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity * 10.0f), 0, 250)); } else { serialize8(currentProfile->pidProfile.P8[i]); serialize8(currentProfile->pidProfile.I8[i]); @@ -1165,7 +1165,7 @@ static bool processInCommand(void) if (i == PIDLEVEL) { currentProfile->pidProfile.A_level = (float)read8() / 10.0f; currentProfile->pidProfile.H_level = (float)read8() / 10.0f; - read8(); + currentProfile->pidProfile.H_sensitivity = (float)read8() / 10.0f; } else { currentProfile->pidProfile.P8[i] = read8(); currentProfile->pidProfile.I8[i] = read8(); From 69d94c81e141d69cdd24583d6562df2013acea39 Mon Sep 17 00:00:00 2001 From: Ben Hitchcock Date: Wed, 7 Jan 2015 21:54:13 +0800 Subject: [PATCH 005/182] Second draft of the tuneup. This uses ints for the sensitivity instead of mapping floats back and forth. Also the stick position is read directly, without the RC_Rate affecting this value. --- src/main/config/config.c | 2 +- src/main/flight/flight.c | 15 ++++++++++----- src/main/flight/flight.h | 2 +- src/main/io/rc_controls.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 4 ++-- src/main/mw.c | 5 +++++ src/main/mw.h | 2 ++ 8 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ad55a40972..75794acf54 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -159,7 +159,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D_f[YAW] = 0.05f; pidProfile->A_level = 5.0f; pidProfile->H_level = 3.0f; - pidProfile->H_sensitivity = 10.0f; + pidProfile->H_sensitivity = 75; } #ifdef GPS diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index b55ef9cd80..b9737f6e1d 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -96,6 +96,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control { float RateError, errorAngle, AngleRate, gyroRate; float ITerm,PTerm,DTerm; + int32_t stickPosAil, stickPosEle, mostDeflectedPos; static float lastGyroRate[3]; static float delta1[3], delta2[3]; float delta, deltaSum; @@ -107,19 +108,23 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control if (FLIGHT_MODE(HORIZON_MODE)) { - if(abs(rcCommand[FD_ROLL]) > abs(rcCommand[FD_PITCH])){ - axis = FD_ROLL; + // Figure out the raw stick positions + stickPosAil = getRcStickPosition(FD_ROLL); + stickPosEle = getRcStickPosition(FD_PITCH); + + if(abs(stickPosAil) > abs(stickPosEle)){ + mostDeflectedPos = abs(stickPosAil); } else { - axis = FD_PITCH; + mostDeflectedPos = abs(stickPosEle); } // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - abs(rcCommand[axis])) / 500; // 1 at centre stick, 0 = max stick deflection + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection if(pidProfile->H_sensitivity == 0){ horizonLevelStrength = 0; } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (10 / pidProfile->H_sensitivity)) + 1, 0, 1); + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1); } } diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 6461ce21e8..6e23355c97 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -42,7 +42,7 @@ typedef struct pidProfile_s { float D_f[3]; float A_level; float H_level; - float H_sensitivity; + uint8_t H_sensitivity; } pidProfile_t; typedef enum { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 745acd4682..54a8e8f772 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -140,7 +140,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); - +int32_t getRcStickPosition(int32_t axis); typedef enum { ADJUSTMENT_NONE = 0, diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b1a3156300..81eef513d6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -392,7 +392,7 @@ const clivalue_t valueTable[] = { { "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 }, { "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 }, - { "sensitivity_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 }, + { "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 }, { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 }, { "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5c2d834eed..e17a599ed3 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -846,7 +846,7 @@ static bool processOutCommand(uint8_t cmdMSP) if (i == PIDLEVEL) { serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250)); - serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity * 10.0f), 0, 250)); + serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 250)); } else { serialize8(currentProfile->pidProfile.P8[i]); serialize8(currentProfile->pidProfile.I8[i]); @@ -1165,7 +1165,7 @@ static bool processInCommand(void) if (i == PIDLEVEL) { currentProfile->pidProfile.A_level = (float)read8() / 10.0f; currentProfile->pidProfile.H_level = (float)read8() / 10.0f; - currentProfile->pidProfile.H_sensitivity = (float)read8() / 10.0f; + currentProfile->pidProfile.H_sensitivity = read8(); } else { currentProfile->pidProfile.P8[i] = read8(); currentProfile->pidProfile.I8[i] = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index 54defd191e..ef4b04fe94 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -338,6 +338,11 @@ void mwArm(void) } } +int32_t getRcStickPosition(int32_t axis) { + + return min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); +} + // Automatic ACC Offset Calibration bool AccInflightCalibrationArmed = false; bool AccInflightCalibrationMeasurementDone = false; diff --git a/src/main/mw.h b/src/main/mw.h index aebee40bee..941efe8b37 100644 --- a/src/main/mw.h +++ b/src/main/mw.h @@ -22,3 +22,5 @@ void handleInflightCalibrationStickPosition(); void mwDisarm(void); void mwArm(void); + +int32_t getRcStickPosition(int32_t axis); \ No newline at end of file From 22afc54228b23484910801ded620b277d34b5e6c Mon Sep 17 00:00:00 2001 From: Ben Hitchcock Date: Wed, 7 Jan 2015 22:04:21 +0800 Subject: [PATCH 006/182] Removing misplaced function definition --- src/main/io/rc_controls.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 54a8e8f772..745acd4682 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -140,7 +140,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); -int32_t getRcStickPosition(int32_t axis); + typedef enum { ADJUSTMENT_NONE = 0, From c8d4743ba04c1ff83b85096f44c430f009d36357 Mon Sep 17 00:00:00 2001 From: JBFUK Date: Fri, 9 Jan 2015 19:22:44 +0000 Subject: [PATCH 007/182] Update LedStrip.md --- docs/LedStrip.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/LedStrip.md b/docs/LedStrip.md index c0ea55ee43..0b6f28fd98 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -51,11 +51,11 @@ uses. e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip. It's also possible to pow from another BEC. Just ensure that the GROUND is the same for all BEC outputs and LEDs. -| Target | Pin | LED Strip | Signal | -| --------------------- | --- | --------- | -------| -| Naze/Olimexino | RC5 | Data In | PA6 | -| CC3D | ??? | Data In | PB4 | -| ChebuzzF3/F3Discovery | PB8 | Data In | PB8 | +| Target | Pin | LED Strip | Signal | +| --------------------- | ---- | --------- | -------| +| Naze/Olimexino | RC5 | Data In | PA6 | +| CC3D | RCO5 | Data In | PB4 | +| ChebuzzF3/F3Discovery | PB8 | Data In | PB8 | Since RC5 is also used for SoftSerial on the Naze/Olimexino it means that you cannot use SoftSerial and led strips at the same time. From 3e6646ec450b68e79ec8256de878df439a6c67bb Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 10 Jan 2015 22:54:04 +0000 Subject: [PATCH 008/182] STM32F3 - Fix LED Strip timer initialisation. --- src/main/drivers/light_ws2811strip_stm32f30x.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 49e8eae128..8d285b4675 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -68,7 +68,7 @@ void ws2811LedStripHardwareInit(void) TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(TIM16, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); + TIM_OC1PreloadConfig(TIM16, TIM_OCPreload_Enable); TIM_CtrlPWMOutputs(TIM16, ENABLE); From 5a07194d620dc15d623939be76c583519c189db4 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 10 Jan 2015 23:33:19 +0000 Subject: [PATCH 009/182] LedStrip - Fix missing GPIO Output type configuration. --- src/main/drivers/light_ws2811strip_stm32f30x.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8d285b4675..7288a22e6a 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -44,9 +44,10 @@ void ws2811LedStripHardwareInit(void) GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_1); - /* GPIOA Configuration: TIM16 Channel 1 as alternate function push-pull */ + /* Configuration alternate function push-pull */ GPIO_InitStructure.GPIO_Pin = WS2811_PIN; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(WS2811_GPIO, &GPIO_InitStructure); From 5d8e39f2d9d02f79f7e0232fbf5afcff2765fb4a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 10 Jan 2015 22:55:22 +0000 Subject: [PATCH 010/182] SPARKY - Add LED STRIP support. Update PWM mapping to avoid PWM/LedStrip clash on Motor output pin 5. --- .../drivers/light_ws2811strip_stm32f30x.c | 66 ++++++++++++------- src/main/drivers/pwm_mapping.c | 11 +++- src/main/drivers/serial_uart_stm32f30x.c | 2 + src/main/target/SPARKY/target.h | 30 +++++++++ 4 files changed, 82 insertions(+), 27 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 7288a22e6a..8908dda081 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -26,10 +26,16 @@ #include "common/color.h" #include "drivers/light_ws2811strip.h" -#define WS2811_GPIO GPIOB -#define WS2811_PIN Pin_8 // TIM16_CH1 -#define WS2811_PERIPHERAL RCC_AHBPeriph_GPIOB - +#ifndef WS2811_GPIO +#define WS2811_GPIO GPIOB +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB +#define WS2811_PIN Pin_8 // TIM16_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM16 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 +#define WS2811_DMA_CHANNEL DMA1_Channel3 +#define WS2811_IRQ DMA1_Channel3_IRQn +#endif void ws2811LedStripHardwareInit(void) { @@ -40,9 +46,9 @@ void ws2811LedStripHardwareInit(void) uint16_t prescalerValue; - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); + RCC_AHBPeriphClockCmd(WS2811_GPIO_AHB_PERIPHERAL, ENABLE); - GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_1); + GPIO_PinAFConfig(WS2811_GPIO, WS2811_PIN_SOURCE, GPIO_AF_1); /* Configuration alternate function push-pull */ GPIO_InitStructure.GPIO_Pin = WS2811_PIN; @@ -53,7 +59,7 @@ void ws2811LedStripHardwareInit(void) GPIO_Init(WS2811_GPIO, &GPIO_InitStructure); - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE); + RCC_APB2PeriphClockCmd(WS2811_TIMER_APB2_PERIPHERAL, ENABLE); /* Compute the prescaler value */ prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; /* Time base configuration */ @@ -61,27 +67,27 @@ void ws2811LedStripHardwareInit(void) TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM16, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure); - /* PWM1 Mode configuration: Channel1 */ + /* PWM1 Mode configuration */ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - TIM_OC1Init(TIM16, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(TIM16, TIM_OCPreload_Enable); + TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); - TIM_CtrlPWMOutputs(TIM16, ENABLE); + TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE); /* configure DMA */ /* DMA clock enable */ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); /* DMA1 Channel Config */ - DMA_DeInit(DMA1_Channel3); + DMA_DeInit(WS2811_DMA_CHANNEL); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM16->CCR1; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&WS2811_TIMER->CCR1; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; @@ -93,16 +99,15 @@ void ws2811LedStripHardwareInit(void) DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel3, &DMA_InitStructure); + DMA_Init(WS2811_DMA_CHANNEL, &DMA_InitStructure); - /* TIM16 CC1 DMA Request enable */ - TIM_DMACmd(TIM16, TIM_DMA_CC1, ENABLE); + TIM_DMACmd(WS2811_TIMER, TIM_DMA_CC1, ENABLE); - DMA_ITConfig(DMA1_Channel3, DMA_IT_TC, ENABLE); + DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE); NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel3_IRQn; + NVIC_InitStructure.NVIC_IRQChannel = WS2811_IRQ; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_WS2811_DMA); NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_WS2811_DMA); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; @@ -116,17 +121,28 @@ void DMA1_Channel3_IRQHandler(void) { if (DMA_GetFlagStatus(DMA1_FLAG_TC3)) { ws2811LedDataTransferInProgress = 0; - DMA_Cmd(DMA1_Channel3, DISABLE); // disable DMA channel 6 - DMA_ClearFlag(DMA1_FLAG_TC3); // clear DMA1 Channel 6 transfer complete flag + DMA_Cmd(DMA1_Channel3, DISABLE); // disable DMA channel + DMA_ClearFlag(DMA1_FLAG_TC3); // clear DMA1 Channel transfer complete flag } } +#if 0 +void DMA1_Channel7_IRQHandler(void) +{ + if (DMA_GetFlagStatus(DMA1_FLAG_TC7)) { + ws2811LedDataTransferInProgress = 0; + DMA_Cmd(DMA1_Channel7, DISABLE); // disable DMA channel + DMA_ClearFlag(DMA1_FLAG_TC7); // clear DMA1 Channel transfer complete flag + } +} +#endif + void ws2811LedStripDMAEnable(void) { - DMA_SetCurrDataCounter(DMA1_Channel3, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(TIM16, 0); - TIM_Cmd(TIM16, ENABLE); - DMA_Cmd(DMA1_Channel3, ENABLE); + DMA_SetCurrDataCounter(WS2811_DMA_CHANNEL, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(WS2811_TIMER, 0); + TIM_Cmd(WS2811_TIMER, ENABLE); + DMA_Cmd(WS2811_DMA_CHANNEL, ENABLE); } diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 85bbe09dbf..67a01b2b47 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -334,8 +334,15 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #ifdef LED_STRIP_TIMER // skip LED Strip output - if (init->useLEDStrip && timerHardwarePtr->tim == LED_STRIP_TIMER) - continue; + if (init->useLEDStrip) { + if (timerHardwarePtr->tim == LED_STRIP_TIMER) + continue; +#if defined(WS2811_GPIO) && defined(WS2811_PIN) + if (timerHardwarePtr->gpio == WS2811_GPIO && timerHardwarePtr->pin == WS2811_PIN) + continue; +#endif + } + #endif #ifdef STM32F10X diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index d32119d926..354ce06dd3 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -326,6 +326,7 @@ void DMA1_Channel4_IRQHandler(void) handleUsartTxDma(s); } +#ifdef USE_USART2_TX_DMA // USART2 Tx DMA Handler void DMA1_Channel7_IRQHandler(void) { @@ -334,6 +335,7 @@ void DMA1_Channel7_IRQHandler(void) DMA_Cmd(DMA1_Channel7, DISABLE); handleUsartTxDma(s); } +#endif // USART3 Tx DMA Handler void DMA1_Channel2_IRQHandler(void) diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 62e691a584..26cf2f709c 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -92,6 +92,36 @@ #define GPS #define DISPLAY +#define LED_STRIP +#if 1 +// LED strip configuration using PWM motor output pin 5. +#define LED_STRIP_TIMER TIM16 + +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_PIN Pin_6 // TIM16_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_TIMER TIM16 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 +#define WS2811_DMA_CHANNEL DMA1_Channel3 +#define WS2811_IRQ DMA1_Channel3_IRQn +#endif + +#if 0 +// Alternate LED strip pin - FIXME for some reason the DMA IRQ Transfer Complete is never called. +#define LED_STRIP_TIMER TIM17 + +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_PIN Pin_7 // TIM17_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource7 +#define WS2811_TIMER TIM17 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 +#define WS2811_DMA_CHANNEL DMA1_Channel7 +#define WS2811_IRQ DMA1_Channel7_IRQn +#endif + + #define SPEKTRUM_BIND // USART2, PA3 #define BIND_PORT GPIOA From a43ae266f5928787405249fd04df52942bdce9fc Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Sun, 11 Jan 2015 14:35:53 +1300 Subject: [PATCH 011/182] Transmit blackbox header slower on faster looptimes to avoid errors --- src/main/blackbox/blackbox.c | 195 ++++++++++++++++++++++------------- 1 file changed, 121 insertions(+), 74 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 43f9fba6fd..574b33d4dc 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -248,13 +248,23 @@ extern uint8_t motorCount; //From mw.c: extern uint32_t currentTime; -static const int SERIAL_CHUNK_SIZE = 16; +// How many bytes should we transmit per loop iteration? +static uint8_t serialChunkSize = 16; static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; -static uint32_t startTime; -static unsigned int headerXmitIndex; -static int fieldXmitIndex; +static struct { + uint32_t headerIndex; + + /* Since these fields are used during different blackbox states (never simultaneously) we can + * overlap them to save on RAM + */ + union { + int fieldIndex; + int serialBudget; + uint32_t startTime; + } u; +} xmitState; static uint32_t blackboxIteration; static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; @@ -633,17 +643,17 @@ static void blackboxSetState(BlackboxState newState) //Perform initial setup required for the new state switch (newState) { case BLACKBOX_STATE_SEND_HEADER: - startTime = millis(); - headerXmitIndex = 0; + xmitState.headerIndex = 0; + xmitState.u.startTime = millis(); break; case BLACKBOX_STATE_SEND_FIELDINFO: case BLACKBOX_STATE_SEND_GPS_G_HEADERS: case BLACKBOX_STATE_SEND_GPS_H_HEADERS: - headerXmitIndex = 0; - fieldXmitIndex = -1; + xmitState.headerIndex = 0; + xmitState.u.fieldIndex = -1; break; case BLACKBOX_STATE_SEND_SYSINFO: - headerXmitIndex = 0; + xmitState.headerIndex = 0; break; case BLACKBOX_STATE_RUNNING: blackboxIteration = 0; @@ -856,6 +866,15 @@ static void configureBlackboxPort(void) previousBaudRate = blackboxPort->baudRate; } } + + /* + * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If + * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should + * transmit with each write. + * + * 9 / 1250 = 7200 / 1000000 + */ + serialChunkSize = max((masterConfig.looptime * 9) / 1250, 4); } static void releaseBlackboxPort(void) @@ -983,7 +1002,7 @@ static void loadBlackboxState(void) * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition. * - * Set headerXmitIndex to 0 and fieldXmitIndex to -1 before calling for the first time. + * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time. * * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the * fieldDefinition and secondCondition arrays. @@ -1003,23 +1022,23 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit * the whole header. */ - if (fieldXmitIndex == -1) { - if (headerXmitIndex >= headerCount) + if (xmitState.u.fieldIndex == -1) { + if (xmitState.headerIndex >= headerCount) return false; //Someone probably called us again after we had already completed transmission charsWritten = blackboxPrint("H Field "); - charsWritten += blackboxPrint(headerNames[headerXmitIndex]); + charsWritten += blackboxPrint(headerNames[xmitState.headerIndex]); charsWritten += blackboxPrint(":"); - fieldXmitIndex++; + xmitState.u.fieldIndex++; needComma = false; } else charsWritten = 0; - for (; fieldXmitIndex < fieldCount && charsWritten < SERIAL_CHUNK_SIZE; fieldXmitIndex++) { - def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * fieldXmitIndex); + for (; xmitState.u.fieldIndex < fieldCount && charsWritten < serialChunkSize; xmitState.u.fieldIndex++) { + def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * xmitState.u.fieldIndex); - if (!conditions || testBlackboxCondition(conditions[conditionsStride * fieldXmitIndex])) { + if (!conditions || testBlackboxCondition(conditions[conditionsStride * xmitState.u.fieldIndex])) { if (needComma) { blackboxWrite(','); charsWritten++; @@ -1027,16 +1046,16 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he needComma = true; // The first header is a field name - if (headerXmitIndex == 0) { + if (xmitState.headerIndex == 0) { charsWritten += blackboxPrint(def->name); } else { //The other headers are integers - if (def->arr[headerXmitIndex - 1] >= 10) { - blackboxWrite(def->arr[headerXmitIndex - 1] / 10 + '0'); - blackboxWrite(def->arr[headerXmitIndex - 1] % 10 + '0'); + if (def->arr[xmitState.headerIndex - 1] >= 10) { + blackboxWrite(def->arr[xmitState.headerIndex - 1] / 10 + '0'); + blackboxWrite(def->arr[xmitState.headerIndex - 1] % 10 + '0'); charsWritten += 2; } else { - blackboxWrite(def->arr[headerXmitIndex - 1] + '0'); + blackboxWrite(def->arr[xmitState.headerIndex - 1] + '0'); charsWritten++; } } @@ -1044,85 +1063,115 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he } // Did we complete this line? - if (fieldXmitIndex == fieldCount) { + if (xmitState.u.fieldIndex == fieldCount) { blackboxWrite('\n'); - headerXmitIndex++; - fieldXmitIndex = -1; + xmitState.headerIndex++; + xmitState.u.fieldIndex = -1; } - return headerXmitIndex < headerCount; + return xmitState.headerIndex < headerCount; } /** - * Transmit a portion of the system information headers. Begin with a xmitIndex of 0. Returns the next xmitIndex to - * call with, or -1 if transmission is complete. + * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns + * true iff transmission is complete, otherwise call again later to continue transmission. */ -static int blackboxWriteSysinfo(int xmitIndex) +static bool blackboxWriteSysinfo() { union floatConvert_t { float f; uint32_t u; } floatConvert; - switch (xmitIndex) { + if (xmitState.headerIndex == 0) { + xmitState.u.serialBudget = 0; + xmitState.headerIndex = 1; + } + + // How many bytes can we afford to transmit this loop? + xmitState.u.serialBudget = min(xmitState.u.serialBudget + serialChunkSize, 64); + + // Most headers will consume at least 20 bytes so wait until we've built up that much link budget + if (xmitState.u.serialBudget < 20) { + return false; + } + + switch (xmitState.headerIndex) { case 0: - blackboxPrintf("H Firmware type:Cleanflight\n"); + //Shouldn't ever get here break; case 1: - // Pause to allow more time for previous to transmit (it exceeds our chunk size) + blackboxPrintf("H Firmware type:Cleanflight\n"); + + xmitState.u.serialBudget -= strlen("H Firmware type:Cleanflight\n"); break; case 2: blackboxPrintf("H Firmware revision:%s\n", shortGitRevision); + + /* Don't need to be super exact about the budget so don't mind the fact that we're including the length of + * the placeholder "%s" + */ + xmitState.u.serialBudget -= strlen("H Firmware revision:%s\n") + strlen(shortGitRevision); break; case 3: - // Pause to allow more time for previous to transmit + blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime); + + xmitState.u.serialBudget -= strlen("H Firmware date:%s %s\n") + strlen(buildDate) + strlen(buildTime); break; case 4: - blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime); + blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); + + xmitState.u.serialBudget -= strlen("H P interval:%d/%d\n"); + break; case 5: - // Pause to allow more time for previous to transmit + blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); + + xmitState.u.serialBudget -= strlen("H rcRate:%d\n"); break; case 6: - blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); + blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle); + + xmitState.u.serialBudget -= strlen("H minthrottle:%d\n"); break; case 7: - blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); + blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle); + + xmitState.u.serialBudget -= strlen("H maxthrottle:%d\n"); break; case 8: - blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle); - break; - case 9: - blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle); - break; - case 10: floatConvert.f = gyro.scale; blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u); + + xmitState.u.serialBudget -= strlen("H gyro.scale:0x%x\n") + 6; + break; + case 9: + blackboxPrintf("H acc_1G:%u\n", acc_1G); + + xmitState.u.serialBudget -= strlen("H acc_1G:%u\n"); + break; + case 10: + blackboxPrintf("H vbatscale:%u\n", masterConfig.batteryConfig.vbatscale); + + xmitState.u.serialBudget -= strlen("H vbatscale:%u\n"); break; case 11: - blackboxPrintf("H acc_1G:%u\n", acc_1G); - break; - case 12: - blackboxPrintf("H vbatscale:%u\n", masterConfig.batteryConfig.vbatscale); - break; - case 13: blackboxPrintf("H vbatcellvoltage:%u,%u,%u\n", masterConfig.batteryConfig.vbatmincellvoltage, masterConfig.batteryConfig.vbatwarningcellvoltage, masterConfig.batteryConfig.vbatmaxcellvoltage); + + xmitState.u.serialBudget -= strlen("H vbatcellvoltage:%u,%u,%u\n"); break; - case 14: - //Pause - break; - case 15: + case 12: blackboxPrintf("H vbatref:%u\n", vbatReference); - break; - case 16: - // One more pause for good luck + + xmitState.u.serialBudget -= strlen("H vbatref:%u\n"); break; default: - return -1; + return true; } - return xmitIndex + 1; + xmitState.headerIndex++; + return false; } // Beep the buzzer and write the current time to the log as a synchronization point @@ -1147,26 +1196,26 @@ static void blackboxPlaySyncBeep() void handleBlackbox(void) { - int i, result; + int i; switch (blackboxState) { case BLACKBOX_STATE_SEND_HEADER: - //On entry of this state, headerXmitIndex is 0 and startTime is intialised + //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised /* * Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit * buffer. */ - if (millis() > startTime + 100) { - for (i = 0; i < SERIAL_CHUNK_SIZE && blackboxHeader[headerXmitIndex] != '\0'; i++, headerXmitIndex++) - blackboxWrite(blackboxHeader[headerXmitIndex]); + if (millis() > xmitState.u.startTime + 100) { + for (i = 0; i < serialChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) + blackboxWrite(blackboxHeader[xmitState.headerIndex]); - if (blackboxHeader[headerXmitIndex] == '\0') + if (blackboxHeader[xmitState.headerIndex] == '\0') blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO); } break; case BLACKBOX_STATE_SEND_FIELDINFO: - //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1 + //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { #ifdef GPS @@ -1179,28 +1228,26 @@ void handleBlackbox(void) break; #ifdef GPS case BLACKBOX_STATE_SEND_GPS_H_HEADERS: - //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1 + //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, blackboxGpsHFields + 1, - ARRAY_LENGTH(blackboxGpsHFields), NULL, 0)) { + ARRAY_LENGTH(blackboxGpsHFields), NULL, NULL)) { blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS); } break; case BLACKBOX_STATE_SEND_GPS_G_HEADERS: - //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1 + //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, blackboxGpsGFields + 1, - ARRAY_LENGTH(blackboxGpsGFields), NULL, 0)) { + ARRAY_LENGTH(blackboxGpsGFields), NULL, NULL)) { blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); } break; #endif case BLACKBOX_STATE_SEND_SYSINFO: - //On entry of this state, headerXmitIndex is 0 - result = blackboxWriteSysinfo(headerXmitIndex); + //On entry of this state, xmitState.headerIndex is 0 - if (result == -1) + //Keep writing chunks of the system info headers until it returns true to signal completion + if (blackboxWriteSysinfo()) blackboxSetState(BLACKBOX_STATE_PRERUN); - else - headerXmitIndex = result; break; case BLACKBOX_STATE_PRERUN: blackboxPlaySyncBeep(); From efbffee51afd21cb5fb2ab2dd2e7be3afec3985d Mon Sep 17 00:00:00 2001 From: Akfreak Date: Sat, 10 Jan 2015 22:03:10 -0800 Subject: [PATCH 012/182] Update LedStrip.md Data added or the Spaky board, "| Sparky, pin PWM5, signal PA6 --- docs/LedStrip.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 0b6f28fd98..86c2076dc1 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -56,7 +56,7 @@ from another BEC. Just ensure that the GROUND is the same for all BEC outputs a | Naze/Olimexino | RC5 | Data In | PA6 | | CC3D | RCO5 | Data In | PB4 | | ChebuzzF3/F3Discovery | PB8 | Data In | PB8 | - +| Sparky | PWM5 | Data In | PA6 | Since RC5 is also used for SoftSerial on the Naze/Olimexino it means that you cannot use SoftSerial and led strips at the same time. Additionally, since RC5 is also used for Parallel PWM RC input on both the Naze, Chebuzz and STM32F3Discovery targets, led strips From 68be127f9c667352c163980d9718593bf6d9c3f8 Mon Sep 17 00:00:00 2001 From: eziosoft Date: Sun, 11 Jan 2015 21:32:24 +0100 Subject: [PATCH 013/182] Update Modes.md IDs corrected according to the source code --- docs/Modes.md | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/docs/Modes.md b/docs/Modes.md index 047649c963..df9d318e92 100644 --- a/docs/Modes.md +++ b/docs/Modes.md @@ -9,24 +9,24 @@ auxillary receiver channels and other events such as failsafe detection. | 1 | ANGLE | Legacy auto-level flight mode | | 2 | HORIZON | Auto-level flight mode | | 3 | BARO | Altitude hold mode (Requires barometer sensor) | -| 4 | MAG | Heading lock | -| 5 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | -| 6 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode | -| 7 | CAMSTAB | Camera Stabilisation | -| 8 | CAMTRIG | | -| 9 | GPSHOME | Autonomous flight to HOME position | -| 10 | GPSHOLD | Maintain the same longitude/lattitude | -| 11 | PASSTHRU | | -| 12 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | -| 13 | LEDMAX | | -| 14 | LEDLOW | | -| 15 | LLIGHTS | | -| 16 | CALIB | | -| 17 | GOV | | -| 18 | OSD | Enable/Disable On-Screen-Display (OSD) | -| 19 | TELEMETRY | Enable telemetry via switch | -| 20 | AUTOTUNE | Autotune Pitch/Roll PIDs | -| 21 | SONAR | Altitude hold mode (sonar sensor only) | +| 5 | MAG | Heading lock | +| 6 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | +| 7 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode | +| 8 | CAMSTAB | Camera Stabilisation | +| 9 | CAMTRIG | | +| 10 | GPSHOME | Autonomous flight to HOME position | +| 11 | GPSHOLD | Maintain the same longitude/lattitude | +| 12 | PASSTHRU | | +| 13 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | +| 14 | LEDMAX | | +| 15 | LEDLOW | | +| 16 | LLIGHTS | | +| 17 | CALIB | | +| 18 | GOV | | +| 19 | OSD | Enable/Disable On-Screen-Display (OSD) | +| 20 | TELEMETRY | Enable telemetry via switch | +| 21 | AUTOTUNE | Autotune Pitch/Roll PIDs | +| 22 | SONAR | Altitude hold mode (sonar sensor only) | ## Mode details From 32888b673f36584183b1b5c0034ca9976f85550f Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 11 Jan 2015 20:39:45 +0000 Subject: [PATCH 014/182] Rename ID column to MSP ID in Modes documenation. --- docs/Modes.md | 48 ++++++++++++++++++++++++------------------------ 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/docs/Modes.md b/docs/Modes.md index df9d318e92..145997bfb4 100644 --- a/docs/Modes.md +++ b/docs/Modes.md @@ -3,30 +3,30 @@ Cleanflight has various modes that can be toggled on or off. Modes can be enabled/disabled by stick positions, auxillary receiver channels and other events such as failsafe detection. -| ID | Short Name | Function | -| --- | ---------- | -------------------------------------------------------------------- | -| 0 | ARM | Enables motors and flight stabilisation | -| 1 | ANGLE | Legacy auto-level flight mode | -| 2 | HORIZON | Auto-level flight mode | -| 3 | BARO | Altitude hold mode (Requires barometer sensor) | -| 5 | MAG | Heading lock | -| 6 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | -| 7 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode | -| 8 | CAMSTAB | Camera Stabilisation | -| 9 | CAMTRIG | | -| 10 | GPSHOME | Autonomous flight to HOME position | -| 11 | GPSHOLD | Maintain the same longitude/lattitude | -| 12 | PASSTHRU | | -| 13 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | -| 14 | LEDMAX | | -| 15 | LEDLOW | | -| 16 | LLIGHTS | | -| 17 | CALIB | | -| 18 | GOV | | -| 19 | OSD | Enable/Disable On-Screen-Display (OSD) | -| 20 | TELEMETRY | Enable telemetry via switch | -| 21 | AUTOTUNE | Autotune Pitch/Roll PIDs | -| 22 | SONAR | Altitude hold mode (sonar sensor only) | +| MSP ID | Short Name | Function | +| ------- | ---------- | -------------------------------------------------------------------- | +| 0 | ARM | Enables motors and flight stabilisation | +| 1 | ANGLE | Legacy auto-level flight mode | +| 2 | HORIZON | Auto-level flight mode | +| 3 | BARO | Altitude hold mode (Requires barometer sensor) | +| 5 | MAG | Heading lock | +| 6 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | +| 7 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode | +| 8 | CAMSTAB | Camera Stabilisation | +| 9 | CAMTRIG | | +| 10 | GPSHOME | Autonomous flight to HOME position | +| 11 | GPSHOLD | Maintain the same longitude/lattitude | +| 12 | PASSTHRU | | +| 13 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | +| 14 | LEDMAX | | +| 15 | LEDLOW | | +| 16 | LLIGHTS | | +| 17 | CALIB | | +| 18 | GOV | | +| 19 | OSD | Enable/Disable On-Screen-Display (OSD) | +| 20 | TELEMETRY | Enable telemetry via switch | +| 21 | AUTOTUNE | Autotune Pitch/Roll PIDs | +| 22 | SONAR | Altitude hold mode (sonar sensor only) | ## Mode details From d7ee09c666e6fc51fd820eebb4165a936908e80f Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 12 Jan 2015 10:51:06 +0000 Subject: [PATCH 015/182] Update .gitignore to exclude generated files. --- .gitignore | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.gitignore b/.gitignore index cf98d4de42..34dd269dd7 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,8 @@ obj/ patches/ startup_stm32f10x_md_gcc.s + +# script-generated files +docs/Manual.pdf +README.pdf + From a89ed666cd51aa3b5495b02d61e88fffa5db1961 Mon Sep 17 00:00:00 2001 From: Pim van Pelt Date: Mon, 12 Jan 2015 22:04:45 +0100 Subject: [PATCH 016/182] Building on Ubuntu --- docs/development/Building in Ubuntu.md | 66 ++++++++++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100755 docs/development/Building in Ubuntu.md diff --git a/docs/development/Building in Ubuntu.md b/docs/development/Building in Ubuntu.md new file mode 100755 index 0000000000..b80dd8d2e8 --- /dev/null +++ b/docs/development/Building in Ubuntu.md @@ -0,0 +1,66 @@ +# Building in Ubuntu + +Building for Ubuntu platform is remarkably easy. The only trick to understand is that the Ubuntu toolchain, +which they are downstreaming from Debian, is not compatible with Cleanflight. We suggest that you take an +alternative PPA from Terry Guo, found here: + +https://launchpad.net/~terry.guo/+archive/ubuntu/gcc-arm-embedded + +## Setup GNU ARM Toolchain + +Note specifically the last paragraph of Terry's PPA documentation -- Ubuntu carries its own package for +`gcc-arm-none-eabi`, so you'll have to remove it, and then pin the one from the PPA. +For your release, you should first remove any older pacakges (from Debian or Ubuntu directly), introduce +Terry's PPA, and update: +``` +sudo apt-get remove binutils-arm-none-eabi gcc-arm-none-eabi +sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded +sudo apt-get update +``` + +For Ubuntu 14.04 (an LTS as of Q1'2015, called Trusty Tahr), you should pin: +``` +sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0trusty12 +``` + +For Ubuntu 14.10 (current release, called Utopic Unicorn), you should pin: +``` +sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0utopic12 +``` + +## Building on Ubuntu + +After the ARM toolchain from Terry is installed, you should be able to build from source. +``` +cd src +git clone git@github.com:cleanflight/cleanflight.git +cd cleanflight +make TARGET=NAZE +``` + +You'll see a set of files being compiled, and finally linked, yielding both an ELF and then a HEX: +``` +... +arm-none-eabi-size ./obj/main/cleanflight_NAZE.elf + text data bss dec hex filename + 97164 320 11080 108564 1a814 ./obj/main/cleanflight_NAZE.elf +arm-none-eabi-objcopy -O ihex --set-start 0x8000000 obj/main/cleanflight_NAZE.elf obj/cleanflight_NAZE.hex +$ ls -la obj/cleanflight_NAZE.hex +-rw-rw-r-- 1 pim pim 274258 Jan 12 21:45 obj/cleanflight_NAZE.hex +``` + +You can use the Cleanflight-Configurator to flash the ```obj/cleanflight_NAZE.hex``` file. + +## Updating and rebuilding + +Navigate to the local cleanflight repository and use the following steps to pull the latest changes and rebuild your version of cleanflight: + +``` +cd src/cleanflight +git reset --hard +git pull +make clean TARGET=NAZE +make +``` + +Credit goes to K.C. Budd for doing the long legwork that yielded this very short document. From 90a36aea77f978afde76b44e087e60e03bbe2b11 Mon Sep 17 00:00:00 2001 From: Pim van Pelt Date: Mon, 12 Jan 2015 22:28:31 +0100 Subject: [PATCH 017/182] Add notes on success/fail TARGETs. Add Penguin to the list. --- docs/development/Building in Ubuntu.md | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/docs/development/Building in Ubuntu.md b/docs/development/Building in Ubuntu.md index b80dd8d2e8..4dbd3586da 100755 --- a/docs/development/Building in Ubuntu.md +++ b/docs/development/Building in Ubuntu.md @@ -18,14 +18,19 @@ sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded sudo apt-get update ``` +For Ubuntu 14.10 (current release, called Utopic Unicorn), you should pin: +``` +sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0utopic12 +``` + For Ubuntu 14.04 (an LTS as of Q1'2015, called Trusty Tahr), you should pin: ``` sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0trusty12 ``` -For Ubuntu 14.10 (current release, called Utopic Unicorn), you should pin: +For Ubuntu 12.04 (previous LTS, called Precise Penguin), you should pin: ``` -sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0utopic12 +sudo apt-get install gcc-arm-none-eabi=4.9.3.2014q4-0precise12 ``` ## Building on Ubuntu @@ -63,4 +68,7 @@ make clean TARGET=NAZE make ``` +## Notes +There are compiler issues with at least the Sparky target. + Credit goes to K.C. Budd for doing the long legwork that yielded this very short document. From c12e1fe0872396845ec9fe3ddd59e91592e867c1 Mon Sep 17 00:00:00 2001 From: Pim van Pelt Date: Mon, 12 Jan 2015 23:00:09 +0100 Subject: [PATCH 018/182] Various tests/build for 4.8 on SPARKY, tested by AKfreak, add attributions --- docs/development/Building in Ubuntu.md | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/docs/development/Building in Ubuntu.md b/docs/development/Building in Ubuntu.md index 4dbd3586da..d52e87c7d0 100755 --- a/docs/development/Building in Ubuntu.md +++ b/docs/development/Building in Ubuntu.md @@ -3,9 +3,13 @@ Building for Ubuntu platform is remarkably easy. The only trick to understand is that the Ubuntu toolchain, which they are downstreaming from Debian, is not compatible with Cleanflight. We suggest that you take an alternative PPA from Terry Guo, found here: - https://launchpad.net/~terry.guo/+archive/ubuntu/gcc-arm-embedded +This PPA has several compiler versions and platforms available. For many hardware platforms (notably Naze) +the 4.9.3 compiler will work fine. For some, older compiler 4.8 (notably Sparky) is more appropriate. We +suggest you build with 4.9.3 first, and try to see if you can connect to the CLI or run the Configurator. +If you cannot, please see the section below for further hints on what you might do. + ## Setup GNU ARM Toolchain Note specifically the last paragraph of Terry's PPA documentation -- Ubuntu carries its own package for @@ -56,6 +60,20 @@ $ ls -la obj/cleanflight_NAZE.hex You can use the Cleanflight-Configurator to flash the ```obj/cleanflight_NAZE.hex``` file. +## Bricked/Bad build? + +Users have reported that the 4.9.3 compiler for ARM produces bad builds, for example on the Sparky hardware platform. +It is very likely that using an older compiler would be fine -- Terry happens to have also a 4.8 2014q2 build in his +PPA - to install this, you can fetch the `.deb` directly: +http://ppa.launchpad.net/terry.guo/gcc-arm-embedded/ubuntu/pool/main/g/gcc-arm-none-eabi/ + +and use `dpkg` to install: +``` +sudo dpkg -i gcc-arm-none-eabi_4-8-2014q2-0saucy9_amd64.deb +``` + +Make sure to remove `obj/` and `make clean`, before building again. + ## Updating and rebuilding Navigate to the local cleanflight repository and use the following steps to pull the latest changes and rebuild your version of cleanflight: @@ -68,7 +86,4 @@ make clean TARGET=NAZE make ``` -## Notes -There are compiler issues with at least the Sparky target. - -Credit goes to K.C. Budd for doing the long legwork that yielded this very short document. +Credit goes to K.C. Budd, AKfreak for testing, and pulsar for doing the long legwork that yielded this very short document. From b6ac9204d388e3025db02da4534d286aa08ff3f0 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 12 Jan 2015 22:01:45 +0000 Subject: [PATCH 019/182] STM32F3 - Fix LED Strip hardware initialisation. --- .../drivers/light_ws2811strip_stm32f10x.c | 6 +++++ .../drivers/light_ws2811strip_stm32f30x.c | 26 ++++++++++++++++--- src/main/drivers/pwm_mapping.c | 4 +-- src/main/drivers/serial_uart_stm32f30x.c | 3 +++ src/main/target/CHEBUZZF3/target.h | 16 ++++++++++++ src/main/target/SPARKY/target.h | 11 +++++--- 6 files changed, 58 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 1462114324..2b0de69bdb 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -35,13 +35,16 @@ void ws2811LedStripHardwareInit(void) #ifdef CC3D RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); #else RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + /* GPIOA Configuration: TIM3 Channel 1 as alternate function push-pull */ + GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; @@ -52,6 +55,7 @@ void ws2811LedStripHardwareInit(void) /* Compute the prescaler value */ prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; /* Time base configuration */ + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; @@ -59,6 +63,7 @@ void ws2811LedStripHardwareInit(void) TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ + TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 0; @@ -75,6 +80,7 @@ void ws2811LedStripHardwareInit(void) /* DMA1 Channel6 Config */ DMA_DeInit(DMA1_Channel6); + DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR1; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8908dda081..7404f8e539 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -27,9 +27,11 @@ #include "drivers/light_ws2811strip.h" #ifndef WS2811_GPIO +#define USE_LED_STRIP_ON_DMA1_CHANNEL3 #define WS2811_GPIO GPIOB #define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_PIN Pin_8 // TIM16_CH1 +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 #define WS2811_PIN_SOURCE GPIO_PinSource8 #define WS2811_TIMER TIM16 #define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 @@ -48,9 +50,10 @@ void ws2811LedStripHardwareInit(void) RCC_AHBPeriphClockCmd(WS2811_GPIO_AHB_PERIPHERAL, ENABLE); - GPIO_PinAFConfig(WS2811_GPIO, WS2811_PIN_SOURCE, GPIO_AF_1); + GPIO_PinAFConfig(WS2811_GPIO, WS2811_PIN_SOURCE, WS2811_GPIO_AF); /* Configuration alternate function push-pull */ + GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = WS2811_PIN; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; @@ -60,9 +63,11 @@ void ws2811LedStripHardwareInit(void) RCC_APB2PeriphClockCmd(WS2811_TIMER_APB2_PERIPHERAL, ENABLE); + /* Compute the prescaler value */ prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; /* Time base configuration */ + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; @@ -70,6 +75,7 @@ void ws2811LedStripHardwareInit(void) TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure); /* PWM1 Mode configuration */ + TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 0; @@ -87,6 +93,7 @@ void ws2811LedStripHardwareInit(void) /* DMA1 Channel Config */ DMA_DeInit(WS2811_DMA_CHANNEL); + DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&WS2811_TIMER->CCR1; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; @@ -117,6 +124,7 @@ void ws2811LedStripHardwareInit(void) ws2811UpdateStrip(); } +#ifdef USE_LED_STRIP_ON_DMA1_CHANNEL3 void DMA1_Channel3_IRQHandler(void) { if (DMA_GetFlagStatus(DMA1_FLAG_TC3)) { @@ -125,8 +133,20 @@ void DMA1_Channel3_IRQHandler(void) DMA_ClearFlag(DMA1_FLAG_TC3); // clear DMA1 Channel transfer complete flag } } +#endif -#if 0 +#ifdef USE_LED_STRIP_ON_DMA1_CHANNEL2 +void DMA1_Channel2_IRQHandler(void) +{ + if (DMA_GetFlagStatus(DMA1_FLAG_TC2)) { + ws2811LedDataTransferInProgress = 0; + DMA_Cmd(DMA1_Channel2, DISABLE); // disable DMA channel + DMA_ClearFlag(DMA1_FLAG_TC2); // clear DMA1 Channel transfer complete flag + } +} +#endif + +#ifdef USE_LED_STRIP_ON_DMA1_CHANNEL7 void DMA1_Channel7_IRQHandler(void) { if (DMA_GetFlagStatus(DMA1_FLAG_TC7)) { diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 67a01b2b47..9130d14841 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -337,8 +337,8 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (init->useLEDStrip) { if (timerHardwarePtr->tim == LED_STRIP_TIMER) continue; -#if defined(WS2811_GPIO) && defined(WS2811_PIN) - if (timerHardwarePtr->gpio == WS2811_GPIO && timerHardwarePtr->pin == WS2811_PIN) +#if defined(STM32F303xC) && defined(WS2811_GPIO) && defined(WS2811_PIN_SOURCE) + if (timerHardwarePtr->gpio == WS2811_GPIO && timerHardwarePtr->gpioPinSource == WS2811_PIN_SOURCE) continue; #endif } diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 354ce06dd3..3d8ee674cb 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -338,6 +338,7 @@ void DMA1_Channel7_IRQHandler(void) #endif // USART3 Tx DMA Handler +#ifdef USE_USART2_TX_DMA void DMA1_Channel2_IRQHandler(void) { uartPort_t *s = &uartPort3; @@ -345,6 +346,8 @@ void DMA1_Channel2_IRQHandler(void) DMA_Cmd(DMA1_Channel2, DISABLE); handleUsartTxDma(s); } +#endif + void usartIrqHandler(uartPort_t *s) { diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index f4bc202a54..de560a7fc5 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -69,7 +69,23 @@ #define GPS #define LED_STRIP +#if 1 #define LED_STRIP_TIMER TIM16 +#else +// alternative LED strip configuration, tested working. +#define LED_STRIP_TIMER TIM1 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_6 +#define WS2811_PIN GPIO_Pin_8 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM1 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#endif #define BLACKBOX #define TELEMETRY diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 26cf2f709c..d020296653 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -97,9 +97,11 @@ // LED strip configuration using PWM motor output pin 5. #define LED_STRIP_TIMER TIM16 +#define USE_LED_STRIP_ON_DMA1_CHANNEL3 #define WS2811_GPIO GPIOA #define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_PIN Pin_6 // TIM16_CH1 +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 #define WS2811_PIN_SOURCE GPIO_PinSource6 #define WS2811_TIMER TIM16 #define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 @@ -108,12 +110,15 @@ #endif #if 0 -// Alternate LED strip pin - FIXME for some reason the DMA IRQ Transfer Complete is never called. +// Alternate LED strip pin +// FIXME DMA IRQ Transfer Complete is never called because the TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1 #define LED_STRIP_TIMER TIM17 +#define USE_LED_STRIP_ON_DMA1_CHANNEL7 #define WS2811_GPIO GPIOA #define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_PIN Pin_7 // TIM17_CH1 +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_7 // TIM17_CH1 #define WS2811_PIN_SOURCE GPIO_PinSource7 #define WS2811_TIMER TIM17 #define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 From ec09f0d7fba6b6b38bfbf15c930257799c3b6e0c Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 12 Jan 2015 23:27:48 +0000 Subject: [PATCH 020/182] Adding a Futuba SBus RX to the list of known working SBus receivers. --- docs/Rx.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/Rx.md b/docs/Rx.md index 1d99c394fb..98b2cee6bf 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -61,6 +61,10 @@ http://www.frsky-rc.com/product/pro.php?pro_id=135 FrSky X8R 8/16ch Telemetry Receiver http://www.frsky-rc.com/product/pro.php?pro_id=105 +Futaba R2008SB 2.4GHz S-FHSS +http://www.futaba-rc.com/systems/futk8100-8j/ + + #### OpenTX S.BUS configuration If using OpenTX set the transmitter module to D16 mode and select CH1-16 on the transmitter before binding to allow reception From b8fa926b3d3d77aedf94c27f20fc2d645b7d6b5a Mon Sep 17 00:00:00 2001 From: curtisbyers Date: Mon, 12 Jan 2015 17:50:55 -0600 Subject: [PATCH 021/182] Fix broken link in Buzzer.md. --- docs/Buzzer.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Buzzer.md b/docs/Buzzer.md index 73b759ff01..376235d579 100644 --- a/docs/Buzzer.md +++ b/docs/Buzzer.md @@ -35,4 +35,4 @@ Buzzer support on the CC3D requires that a buzzer circuit be created to which th PA15 is unused and not connected according to the CC3D Revision A schematic. Connecting to PA15 requires careful soldering. -See the [CC3D - buzzer circuir.pdf](Wiring/CC3D - buzzer circuir.pdf) for details. +See the [CC3D - buzzer circuit.pdf](Wiring/CC3D - buzzer circuit.pdf) for details. From f7a9a628b6ebb370641e451846733cb7f6c89562 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Wed, 14 Jan 2015 11:46:44 +1300 Subject: [PATCH 022/182] Add logging for GPS ground course (heading) --- src/main/blackbox/blackbox.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 574b33d4dc..2365c39138 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -215,7 +215,8 @@ static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = { {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)}, {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)}, {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)} + {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)} }; // GPS home frame @@ -942,6 +943,7 @@ static void writeGPSFrame() writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); writeUnsignedVB(GPS_altitude); writeUnsignedVB(GPS_speed); + writeUnsignedVB(GPS_ground_course); gpsHistory.GPS_numSat = GPS_numSat; gpsHistory.GPS_coord[0] = GPS_coord[0]; From 2b1e8c12fc5776ba3582b1dc7481e46a0c833276 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Wed, 14 Jan 2015 15:14:25 +1300 Subject: [PATCH 023/182] Add Blackbox logging for autotune events --- src/main/blackbox/blackbox.c | 51 ++++++++-- src/main/blackbox/blackbox.h | 7 +- src/main/blackbox/blackbox_fielddefs.h | 35 +++++++ src/main/flight/autotune.c | 130 +++++++++++++++++++------ 4 files changed, 183 insertions(+), 40 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 43f9fba6fd..49406c4b7f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -74,7 +74,6 @@ #include "config/config_profile.h" #include "config/config_master.h" -#include "blackbox_fielddefs.h" #include "blackbox.h" #define BLACKBOX_BAUDRATE 115200 @@ -1125,10 +1124,49 @@ static int blackboxWriteSysinfo(int xmitIndex) return xmitIndex + 1; } +/** + * Write the given event to the log immediately + */ +void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) +{ + if (blackboxState != BLACKBOX_STATE_RUNNING) + return; + + //Shared header for event frames + blackboxWrite('E'); + blackboxWrite(event); + + //Now serialize the data for this specific frame type + switch (event) { + case FLIGHT_LOG_EVENT_SYNC_BEEP: + writeUnsignedVB(data->syncBeep.time); + break; + case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: + blackboxWrite(data->autotuneCycleStart.phase); + blackboxWrite(data->autotuneCycleStart.cycle); + blackboxWrite(data->autotuneCycleStart.p); + blackboxWrite(data->autotuneCycleStart.i); + blackboxWrite(data->autotuneCycleStart.d); + break; + case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT: + blackboxWrite(data->autotuneCycleResult.overshot); + blackboxWrite(data->autotuneCycleStart.p); + blackboxWrite(data->autotuneCycleStart.i); + blackboxWrite(data->autotuneCycleStart.d); + break; + case FLIGHT_LOG_EVENT_LOG_END: + blackboxPrint("That's all folks!"); + blackboxWrite(0); + break; + } +} + // Beep the buzzer and write the current time to the log as a synchronization point static void blackboxPlaySyncBeep() { - uint32_t now = micros(); + flightLogEvent_syncBeep_t eventData; + + eventData.time = micros(); /* * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later. @@ -1139,10 +1177,7 @@ static void blackboxPlaySyncBeep() // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive queueConfirmationBeep(1); - blackboxWrite('E'); - blackboxWrite(FLIGHT_LOG_EVENT_SYNC_BEEP); - - writeUnsignedVB(now); + blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData); } void handleBlackbox(void) @@ -1203,9 +1238,9 @@ void handleBlackbox(void) headerXmitIndex = result; break; case BLACKBOX_STATE_PRERUN: - blackboxPlaySyncBeep(); - blackboxSetState(BLACKBOX_STATE_RUNNING); + + blackboxPlaySyncBeep(); break; case BLACKBOX_STATE_RUNNING: // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 5d30671bbf..b971feec59 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -17,9 +17,12 @@ #pragma once -#include "common/axis.h" #include +#include "common/axis.h" +#include "flight/mixer.h" +#include "blackbox/blackbox_fielddefs.h" + typedef struct blackboxValues_t { uint32_t time; @@ -41,6 +44,8 @@ typedef struct blackboxValues_t { #endif } blackboxValues_t; +void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); + void initBlackbox(void); void handleBlackbox(void); void startBlackbox(void); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 87db8ecbbb..458c87caf6 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -96,5 +96,40 @@ typedef enum FlightLogFieldSign { typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, + FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, + FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; + +typedef struct flightLogEvent_syncBeep_t { + uint32_t time; +} flightLogEvent_syncBeep_t; + +typedef struct flightLogEvent_autotuneCycleStart_t { + uint8_t phase; + uint8_t cycle; + uint8_t p; + uint8_t i; + uint8_t d; +} flightLogEvent_autotuneCycleStart_t; + +typedef struct flightLogEvent_autotuneCycleResult_t { + uint8_t overshot; + uint8_t p; + uint8_t i; + uint8_t d; +} flightLogEvent_autotuneCycleResult_t; + +typedef union flightLogEventData_t +{ + flightLogEvent_syncBeep_t syncBeep; + flightLogEvent_autotuneCycleStart_t autotuneCycleStart; + flightLogEvent_autotuneCycleResult_t autotuneCycleResult; + +} flightLogEventData_t; + +typedef struct flightLogEvent_t +{ + FlightLogEvent event; + flightLogEventData_t data; +} flightLogEvent_t; diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 5ba9664f66..7ecf5a80bf 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -31,6 +31,9 @@ #include "flight/flight.h" +#include "config/config.h" +#include "blackbox/blackbox.h" + extern int16_t debug[4]; /* @@ -97,6 +100,12 @@ typedef enum { PHASE_SAVE_OR_RESTORE_PIDS, } autotunePhase_e; +typedef enum { + CYCLE_TUNE_I = 0, + CYCLE_TUNE_PD, + CYCLE_TUNE_PD2 +} autotuneCycle_e; + static const pidIndex_e angleIndexToPidIndexMap[] = { PIDROLL, PIDPITCH @@ -112,7 +121,7 @@ static pidProfile_t pidBackup; static uint8_t pidController; static uint8_t pidIndex; static bool rising; -static uint8_t cycleCount; // TODO can we replace this with an enum to improve readability. +static autotuneCycle_e cycle; static uint32_t timeoutAt; static angle_index_t autoTuneAngleIndex; static autotunePhase_e phase = PHASE_IDLE; @@ -140,10 +149,33 @@ bool isAutotuneIdle(void) return phase == PHASE_IDLE; } +#ifdef BLACKBOX + +static void autotuneLogCycleStart() +{ + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneCycleStart_t eventData; + + eventData.phase = phase; + eventData.cycle = cycle; + eventData.p = pid.p * MULTIWII_P_MULTIPLIER; + eventData.i = pid.i * MULTIWII_I_MULTIPLIER; + eventData.d = pid.d; + + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData); + } +} + +#endif + static void startNewCycle(void) { rising = !rising; secondPeakAngle = firstPeakAngle = 0; + +#ifdef BLACKBOX + autotuneLogCycleStart(); +#endif } static void updatePidIndex(void) @@ -155,8 +187,7 @@ static void updateTargetAngle(void) { if (rising) { targetAngle = AUTOTUNE_TARGET_ANGLE; - } - else { + } else { targetAngle = -AUTOTUNE_TARGET_ANGLE; } @@ -210,30 +241,48 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle); } else if (secondPeakAngle > 0) { - if (cycleCount == 0) { - // when checking the I value, we would like to overshoot the target position by half of the max oscillation. - if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) { - pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; - } else { - pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; - if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { - pid.i = AUTOTUNE_MINIMUM_I_VALUE; + switch (cycle) { + case CYCLE_TUNE_I: + // when checking the I value, we would like to overshoot the target position by half of the max oscillation. + if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) { + pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; + } else { + pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; + if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { + pid.i = AUTOTUNE_MINIMUM_I_VALUE; + } } - } - // go back to checking P and D - cycleCount = 1; - pidProfile->I8[pidIndex] = 0; - startNewCycle(); - } else { - // we are checking P and D values - // set up to look for the 2nd peak - firstPeakAngle = currentAngle; - timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS; +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneCycleResult_t eventData; + + eventData.overshot = currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 ? 0 : 1; + eventData.p = pidProfile->P8[pidIndex]; + eventData.i = pidProfile->I8[pidIndex]; + eventData.d = pidProfile->D8[pidIndex]; + + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData); + } +#endif + + // go back to checking P and D + cycle = CYCLE_TUNE_PD; + pidProfile->I8[pidIndex] = 0; + startNewCycle(); + break; + + case CYCLE_TUNE_PD: + case CYCLE_TUNE_PD2: + // we are checking P and D values + // set up to look for the 2nd peak + firstPeakAngle = currentAngle; + timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS; + break; } } } else { - // we saw the first peak. looking for the second + // We saw the first peak while tuning PD, looking for the second if (currentAngle < firstPeakAngle) { firstPeakAngle = currentAngle; @@ -266,8 +315,8 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; } #else - pid.p *= AUTOTUNE_DECREASE_MULTIPLIER; - pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; + pid.p *= AUTOTUNE_DECREASE_MULTIPLIER; + pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; #endif } else { // undershot @@ -286,15 +335,30 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER; pidProfile->D8[pidIndex] = pid.d; - // switch to the other direction and start a new cycle - startNewCycle(); +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneCycleResult_t eventData; - if (++cycleCount == 3) { - // switch to testing I value - cycleCount = 0; + eventData.overshot = secondPeakAngle > targetAngleAtPeak ? 1 : 0; + eventData.p = pidProfile->P8[pidIndex]; + eventData.i = pidProfile->I8[pidIndex]; + eventData.d = pidProfile->D8[pidIndex]; - pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER; + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData); } +#endif + + if (cycle == CYCLE_TUNE_PD2) { + // switch to testing I value + cycle = CYCLE_TUNE_I; + + pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER; + } else { + cycle = CYCLE_TUNE_PD2; + } + + // switch to the other direction for the new cycle + startNewCycle(); } } @@ -344,7 +408,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle } rising = true; - cycleCount = 1; + cycle = CYCLE_TUNE_PD; secondPeakAngle = firstPeakAngle = 0; pidProfile = pidProfileToTune; @@ -360,6 +424,10 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle pidProfile->D8[pidIndex] = pid.d; pidProfile->I8[pidIndex] = 0; + +#ifdef BLACKBOX + autotuneLogCycleStart(); +#endif } void autotuneEndPhase(void) From b893e457f138880eb48a026512caaad2515fb41a Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Wed, 14 Jan 2015 15:42:38 +1300 Subject: [PATCH 024/182] Don't break the blackbox stream when PIDs change in-flight --- src/main/blackbox/blackbox.c | 39 ++++++++++++++++++-------- src/main/blackbox/blackbox_fielddefs.h | 13 ++++----- 2 files changed, 33 insertions(+), 19 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2365c39138..028f54c903 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -267,6 +267,8 @@ static struct { } u; } xmitState; +static uint32_t blackboxConditionCache; + static uint32_t blackboxIteration; static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; @@ -582,7 +584,7 @@ static void writeTag8_8SVB(int32_t *values, int valueCount) } } -static bool testBlackboxCondition(FlightLogFieldCondition condition) +static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) { switch (condition) { case FLIGHT_LOG_FIELD_CONDITION_ALWAYS: @@ -597,19 +599,10 @@ static bool testBlackboxCondition(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8: return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1; + case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER: return masterConfig.mixerMode == MIXER_TRI; - case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0: - case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1: - case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2: - return currentProfile->pidProfile.P8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0] != 0; - - case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0: - case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1: - case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2: - return currentProfile->pidProfile.I8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0] != 0; - case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2: @@ -639,6 +632,23 @@ static bool testBlackboxCondition(FlightLogFieldCondition condition) } } +static void blackboxBuildConditionCache() +{ + FlightLogFieldCondition cond; + + blackboxConditionCache = 0; + + for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { + if (testBlackboxConditionUncached(cond)) + blackboxConditionCache |= 1 << cond; + } +} + +static bool testBlackboxCondition(FlightLogFieldCondition condition) +{ + return (blackboxConditionCache & (1 << condition)) != 0; +} + static void blackboxSetState(BlackboxState newState) { //Perform initial setup required for the new state @@ -908,6 +918,13 @@ void startBlackbox(void) //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it + /* + * We use conditional tests to decide whether or not certain fields should be logged. Since our headers + * must always agree with the logged data, the results of these tests must not change during logging. So + * cache those now. + */ + blackboxBuildConditionCache(); + blackboxSetState(BLACKBOX_STATE_SEND_HEADER); } } diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 87db8ecbbb..1763561092 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -29,21 +29,18 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8, FLIGHT_LOG_FIELD_CONDITION_TRICOPTER, - FLIGHT_LOG_FIELD_CONDITION_MAG = 20, + FLIGHT_LOG_FIELD_CONDITION_MAG, FLIGHT_LOG_FIELD_CONDITION_BARO, FLIGHT_LOG_FIELD_CONDITION_VBAT, - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0 = 40, - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1, - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2, - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0, - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1, - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2, - FLIGHT_LOG_FIELD_CONDITION_NEVER = 255, + FLIGHT_LOG_FIELD_CONDITION_NEVER, + + FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS, + FLIGHT_LOG_FIELD_CONDITION_LAST = FLIGHT_LOG_FIELD_CONDITION_NEVER } FlightLogFieldCondition; typedef enum FlightLogFieldPredictor { From bf886968471a519413143dabeae8722f32242d74 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 14 Jan 2015 14:24:20 +0000 Subject: [PATCH 025/182] Update unit tests to correctly use C/C++ where appropriate. Cleanup makefile by using additional whitespace. --- src/test/Makefile | 161 ++++++++++++++++++----- src/test/unit/flight_imu_unittest.cc | 2 + src/test/unit/gps_conversion_unittest.cc | 5 +- src/test/unit/ledstrip_unittest.cc | 53 +++++--- src/test/unit/ws2811_unittest.cc | 12 +- 5 files changed, 173 insertions(+), 60 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index 4dd0cf1ab0..37685498be 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -87,118 +87,209 @@ $(OBJECT_DIR)/gtest_main.a : $(OBJECT_DIR)/gtest-all.o $(OBJECT_DIR)/gtest_main. # includes in test dir must override includes in user dir TEST_INCLUDE_DIRS := $(TEST_DIR) \ $(USER_INCLUDE_DIR) - TEST_CFLAGS = $(addprefix -I,$(TEST_INCLUDE_DIRS)) +$(OBJECT_DIR)/common/maths.o : \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/common/maths.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@ + $(OBJECT_DIR)/sensors/battery.o : $(USER_DIR)/sensors/battery.c $(USER_DIR)/sensors/battery.h $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/battery.c -o $@ -$(OBJECT_DIR)/battery_unittest.o : $(TEST_DIR)/battery_unittest.cc \ - $(USER_DIR)/sensors/battery.h $(GTEST_HEADERS) +$(OBJECT_DIR)/battery_unittest.o : \ + $(TEST_DIR)/battery_unittest.cc \ + $(USER_DIR)/sensors/battery.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/battery_unittest.cc -o $@ -battery_unittest : $(OBJECT_DIR)/sensors/battery.o $(OBJECT_DIR)/battery_unittest.o $(OBJECT_DIR)/gtest_main.a +battery_unittest : \ + $(OBJECT_DIR)/sensors/battery.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/battery_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/flight/imu.o : \ + $(USER_DIR)/flight/imu.c \ + $(USER_DIR)/flight/imu.h \ + $(GTEST_HEADERS) - -$(OBJECT_DIR)/flight/imu.o : $(USER_DIR)/flight/imu.c $(USER_DIR)/flight/imu.h $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/imu.c -o $@ -$(OBJECT_DIR)/flight_imu_unittest.o : $(TEST_DIR)/flight_imu_unittest.cc \ - $(USER_DIR)/flight/imu.h $(GTEST_HEADERS) +$(OBJECT_DIR)/flight_imu_unittest.o : \ + $(TEST_DIR)/flight_imu_unittest.cc \ + $(USER_DIR)/flight/imu.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -o $@ -flight_imu_unittest : $(OBJECT_DIR)/flight/imu.o $(OBJECT_DIR)/flight/altitudehold.o $(OBJECT_DIR)/flight_imu_unittest.o $(OBJECT_DIR)/gtest_main.a +flight_imu_unittest : \ + $(OBJECT_DIR)/flight/imu.o \ + $(OBJECT_DIR)/flight/altitudehold.o \ + $(OBJECT_DIR)/flight_imu_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/flight/altitudehold.o : $(USER_DIR)/flight/altitudehold.c $(USER_DIR)/flight/altitudehold.h $(GTEST_HEADERS) +$(OBJECT_DIR)/flight/altitudehold.o : \ + $(USER_DIR)/flight/altitudehold.c \ + $(USER_DIR)/flight/altitudehold.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@ -$(OBJECT_DIR)/altitude_hold_unittest.o : $(TEST_DIR)/altitude_hold_unittest.cc \ - $(USER_DIR)/flight/altitudehold.h $(GTEST_HEADERS) +$(OBJECT_DIR)/altitude_hold_unittest.o : \ + $(TEST_DIR)/altitude_hold_unittest.cc \ + $(USER_DIR)/flight/altitudehold.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/altitude_hold_unittest.cc -o $@ -altitude_hold_unittest : $(OBJECT_DIR)/flight/altitudehold.o $(OBJECT_DIR)/altitude_hold_unittest.o $(OBJECT_DIR)/gtest_main.a +altitude_hold_unittest : \ + $(OBJECT_DIR)/flight/altitudehold.o \ + $(OBJECT_DIR)/altitude_hold_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/flight/gps_conversion.o : \ + $(USER_DIR)/flight/gps_conversion.c \ + $(USER_DIR)/flight/gps_conversion.h \ + $(GTEST_HEADERS) -$(OBJECT_DIR)/flight/gps_conversion.o : $(USER_DIR)/flight/gps_conversion.c $(USER_DIR)/flight/gps_conversion.h $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@ -$(OBJECT_DIR)/gps_conversion_unittest.o : $(TEST_DIR)/gps_conversion_unittest.cc \ - $(USER_DIR)/flight/gps_conversion.h $(GTEST_HEADERS) +$(OBJECT_DIR)/gps_conversion_unittest.o : \ + $(TEST_DIR)/gps_conversion_unittest.cc \ + $(USER_DIR)/flight/gps_conversion.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@ -gps_conversion_unittest : $(OBJECT_DIR)/flight/gps_conversion.o $(OBJECT_DIR)/gps_conversion_unittest.o $(OBJECT_DIR)/gtest_main.a +gps_conversion_unittest : \ + $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/gps_conversion_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/telemetry/hott.o : $(USER_DIR)/telemetry/hott.c $(USER_DIR)/telemetry/hott.h $(GTEST_HEADERS) +$(OBJECT_DIR)/telemetry/hott.o : \ + $(USER_DIR)/telemetry/hott.c \ + $(USER_DIR)/telemetry/hott.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/hott.c -o $@ -$(OBJECT_DIR)/telemetry_hott_unittest.o : $(TEST_DIR)/telemetry_hott_unittest.cc \ - $(USER_DIR)/telemetry/hott.h $(GTEST_HEADERS) +$(OBJECT_DIR)/telemetry_hott_unittest.o : \ + $(TEST_DIR)/telemetry_hott_unittest.cc \ + $(USER_DIR)/telemetry/hott.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_hott_unittest.cc -o $@ -telemetry_hott_unittest :$(OBJECT_DIR)/telemetry/hott.o $(OBJECT_DIR)/telemetry_hott_unittest.o $(OBJECT_DIR)/flight/gps_conversion.o $(OBJECT_DIR)/gtest_main.a +telemetry_hott_unittest : \ + $(OBJECT_DIR)/telemetry/hott.o \ + $(OBJECT_DIR)/telemetry_hott_unittest.o \ + $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/io/rc_controls.o : $(USER_DIR)/io/rc_controls.c $(USER_DIR)/io/rc_controls.h $(GTEST_HEADERS) +$(OBJECT_DIR)/io/rc_controls.o : \ + $(USER_DIR)/io/rc_controls.c \ + $(USER_DIR)/io/rc_controls.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/rc_controls.c -o $@ -$(OBJECT_DIR)/rc_controls_unittest.o : $(TEST_DIR)/rc_controls_unittest.cc \ - $(USER_DIR)/io/rc_controls.h $(GTEST_HEADERS) +$(OBJECT_DIR)/rc_controls_unittest.o : \ + $(TEST_DIR)/rc_controls_unittest.cc \ + $(USER_DIR)/io/rc_controls.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@ -rc_controls_unittest :$(OBJECT_DIR)/io/rc_controls.o $(OBJECT_DIR)/rc_controls_unittest.o $(OBJECT_DIR)/gtest_main.a +rc_controls_unittest : \ + $(OBJECT_DIR)/io/rc_controls.o \ + $(OBJECT_DIR)/rc_controls_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/io/ledstrip.o : $(USER_DIR)/io/ledstrip.c $(USER_DIR)/io/ledstrip.h $(GTEST_HEADERS) +$(OBJECT_DIR)/io/ledstrip.o : \ + $(USER_DIR)/io/ledstrip.c \ + $(USER_DIR)/io/ledstrip.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@ + $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@ + +$(OBJECT_DIR)/ledstrip_unittest.o : \ + $(TEST_DIR)/ledstrip_unittest.cc \ + $(USER_DIR)/io/ledstrip.h \ + $(GTEST_HEADERS) -$(OBJECT_DIR)/ledstrip_unittest.o : $(TEST_DIR)/ledstrip_unittest.cc \ - $(USER_DIR)/io/ledstrip.h $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@ + $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@ + +ledstrip_unittest : \ + $(OBJECT_DIR)/io/ledstrip.o \ + $(OBJECT_DIR)/ledstrip_unittest.o \ + $(OBJECT_DIR)/gtest_main.a -ledstrip_unittest :$(OBJECT_DIR)/io/ledstrip.o $(OBJECT_DIR)/ledstrip_unittest.o $(OBJECT_DIR)/gtest_main.a $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/drivers/light_ws2811strip.o : $(USER_DIR)/drivers/light_ws2811strip.c $(USER_DIR)/drivers/light_ws2811strip.h $(GTEST_HEADERS) +$(OBJECT_DIR)/drivers/light_ws2811strip.o : \ + $(USER_DIR)/drivers/light_ws2811strip.c \ + $(USER_DIR)/drivers/light_ws2811strip.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/light_ws2811strip.c -o $@ -$(OBJECT_DIR)/ws2811_unittest.o : $(TEST_DIR)/ws2811_unittest.cc \ - $(USER_DIR)/drivers/light_ws2811strip.h $(GTEST_HEADERS) +$(OBJECT_DIR)/ws2811_unittest.o : \ + $(TEST_DIR)/ws2811_unittest.cc \ + $(USER_DIR)/drivers/light_ws2811strip.h \ + $(GTEST_HEADERS) + @mkdir -p $(dir $@) $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ws2811_unittest.cc -o $@ -ws2811_unittest :$(OBJECT_DIR)/drivers/light_ws2811strip.o $(OBJECT_DIR)/ws2811_unittest.o $(OBJECT_DIR)/gtest_main.a +ws2811_unittest : \ + $(OBJECT_DIR)/drivers/light_ws2811strip.o \ + $(OBJECT_DIR)/ws2811_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 83ac36b331..67425aa63d 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -27,7 +27,9 @@ extern "C" { #include "flight/flight.h" #include "sensors/sensors.h" + #include "drivers/sensor.h" #include "drivers/accgyro.h" + #include "drivers/compass.h" #include "sensors/gyro.h" #include "sensors/compass.h" #include "sensors/acceleration.h" diff --git a/src/test/unit/gps_conversion_unittest.cc b/src/test/unit/gps_conversion_unittest.cc index 15c04d912c..f75085b5b3 100644 --- a/src/test/unit/gps_conversion_unittest.cc +++ b/src/test/unit/gps_conversion_unittest.cc @@ -18,7 +18,10 @@ #include #include -#include "flight/gps_conversion.h" + +extern "C" { + #include "flight/gps_conversion.h" +} #include "unittest_macros.h" #include "gtest/gtest.h" diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 082d7ffdc6..067b8983df 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -19,37 +19,40 @@ #include -#include "build_config.h" +extern "C" { + #include "build_config.h" -#include "common/color.h" -#include "common/axis.h" -#include "flight/flight.h" + #include "common/color.h" + #include "common/axis.h" + #include "flight/flight.h" -#include "sensors/battery.h" -#include "config/runtime_config.h" -#include "config/config.h" + #include "sensors/battery.h" + #include "config/runtime_config.h" + #include "config/config.h" -#include "rx/rx.h" - -#include "drivers/light_ws2811strip.h" -#include "io/ledstrip.h" + #include "rx/rx.h" + #include "drivers/light_ws2811strip.h" + #include "io/ledstrip.h" +} #include "unittest_macros.h" #include "gtest/gtest.h" -extern ledConfig_t *ledConfigs; -extern uint8_t highestYValueForNorth; -extern uint8_t lowestYValueForSouth; -extern uint8_t highestXValueForWest; -extern uint8_t lowestXValueForEast; -extern uint8_t ledGridWidth; -extern uint8_t ledGridHeight; +extern "C" { + extern ledConfig_t *ledConfigs; + extern uint8_t highestYValueForNorth; + extern uint8_t lowestYValueForSouth; + extern uint8_t highestXValueForWest; + extern uint8_t lowestXValueForEast; + extern uint8_t ledGridWidth; + extern uint8_t ledGridHeight; -void determineLedStripDimensions(void); -void determineOrientationLimits(void); + void determineLedStripDimensions(void); + void determineOrientationLimits(void); -ledConfig_t systemLedConfigs[MAX_LED_STRIP_LENGTH]; + ledConfig_t systemLedConfigs[MAX_LED_STRIP_LENGTH]; +} TEST(LedStripTest, parseLedStripConfig) { @@ -336,12 +339,18 @@ TEST(ColorTest, parseColor) } } +extern "C" { + uint8_t armingFlags = 0; uint16_t flightModeFlags = 0; int16_t rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +batteryState_e calculateBatteryState(void) { + return BATTERY_OK; +} +void ws2811LedStripInit(void) {} void ws2811UpdateStrip(void) {} void setLedValue(uint16_t index, const uint8_t value) { @@ -399,3 +408,5 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) { return 0; } + +} diff --git a/src/test/unit/ws2811_unittest.cc b/src/test/unit/ws2811_unittest.cc index 5606155a24..da13b9d8ef 100644 --- a/src/test/unit/ws2811_unittest.cc +++ b/src/test/unit/ws2811_unittest.cc @@ -19,19 +19,23 @@ #include -#include "build_config.h" +extern "C" { + #include "build_config.h" -#include "common/color.h" + #include "common/color.h" -#include "drivers/light_ws2811strip.h" + #include "drivers/light_ws2811strip.h" +} #include "unittest_macros.h" #include "gtest/gtest.h" +extern "C" { STATIC_UNIT_TESTED extern uint16_t dmaBufferOffset; STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color); STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue); +} TEST(WS2812, updateDMABuffer) { // given @@ -86,6 +90,7 @@ TEST(WS2812, updateDMABuffer) { byteIndex++; } +extern "C" { rgbColor24bpp_t* hsvToRgb24(const hsvColor_t *c) { UNUSED(c); return NULL; @@ -93,3 +98,4 @@ rgbColor24bpp_t* hsvToRgb24(const hsvColor_t *c) { void ws2811LedStripHardwareInit(void) {} void ws2811LedStripDMAEnable(void) {} +} From 30b928c0bcda86c9ce279d31e919c601e2bd5dee Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 14 Jan 2015 15:09:00 +0000 Subject: [PATCH 026/182] Cleanup unit test compiler flags. Fix compiler warnings in unit test and related code. --- src/main/io/ledstrip.c | 6 +- src/main/io/ledstrip.h | 2 +- src/test/Makefile | 74 ++++++++++++++---------- src/test/unit/gps_conversion_unittest.cc | 2 +- src/test/unit/ledstrip_unittest.cc | 2 +- src/test/unit/platform.h | 1 + src/test/unit/ws2811_unittest.cc | 2 +- 7 files changed, 51 insertions(+), 38 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 5c6b3880e0..07c3f1518d 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -185,6 +185,7 @@ static const modeColorIndexes_t angleModeColors = { COLOR_ORANGE }; +#ifdef MAG static const modeColorIndexes_t magModeColors = { COLOR_MINT_GREEN, COLOR_DARK_VIOLET, @@ -193,6 +194,7 @@ static const modeColorIndexes_t magModeColors = { COLOR_BLUE, COLOR_ORANGE }; +#endif static const modeColorIndexes_t baroModeColors = { COLOR_LIGHT_BLUE, @@ -788,9 +790,9 @@ void updateLedStrip(void) ws2811UpdateStrip(); } -bool parseColor(uint8_t index, char *colorConfig) +bool parseColor(uint8_t index, const char *colorConfig) { - char *remainingCharacters = colorConfig; + const char *remainingCharacters = colorConfig; hsvColor_t *color = &colors[index]; diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index c7fad4f1fa..83776b7c21 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -69,7 +69,7 @@ void updateLedStrip(void); void applyDefaultLedStripConfig(ledConfig_t *ledConfig); void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); -bool parseColor(uint8_t index, char *colorConfig); +bool parseColor(uint8_t index, const char *colorConfig); void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); void ledStripEnable(void); diff --git a/src/test/Makefile b/src/test/Makefile index 37685498be..8e376ea51b 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -23,13 +23,23 @@ USER_INCLUDE_DIR = $(USER_DIR) OBJECT_DIR = ../../obj/test -# Flags passed to the preprocessor. -# Set Google Test's header directory as a system directory, such that -# the compiler doesn't generate warnings in Google Test headers. -CPPFLAGS += -isystem $(GTEST_DIR)/inc +COMMON_FLAGS = \ + -g \ + -Wall \ + -Wextra \ + -pthread \ + -ggdb3 \ + -O0 \ + -DUNIT_TEST \ + -isystem $(GTEST_DIR)/inc + +# Flags passed to the C compiler. +C_FLAGS = $(COMMON_FLAGS) \ + -std=gnu99 # Flags passed to the C++ compiler. -CXXFLAGS += -g -Wall -Wextra -pthread -ggdb3 -O0 -DUNIT_TEST +CXX_FLAGS = $(COMMON_FLAGS) \ + -std=gnu++98 # All tests produced by this Makefile. Remember to add new tests you # created to the list. @@ -66,12 +76,12 @@ GTEST_SRCS_ = $(GTEST_DIR)/src/*.cc $(GTEST_DIR)/inc/gtest/*.h $(GTEST_HEADERS) # compiles fast and for ordinary users its source rarely changes. $(OBJECT_DIR)/gtest-all.o : $(GTEST_SRCS_) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) -I$(GTEST_DIR) $(CXXFLAGS) -c \ + $(CXX) $(CXX_FLAGS) -I$(GTEST_DIR) -c \ $(GTEST_DIR)/src/gtest-all.cc -o $@ $(OBJECT_DIR)/gtest_main.o : $(GTEST_SRCS_) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) -I$(GTEST_DIR) $(CXXFLAGS) -c \ + $(CXX) $(CXX_FLAGS) -I$(GTEST_DIR) -c \ $(GTEST_DIR)/src/gtest_main.cc -o $@ $(OBJECT_DIR)/gtest.a : $(OBJECT_DIR)/gtest-all.o @@ -96,12 +106,12 @@ $(OBJECT_DIR)/common/maths.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@ $(OBJECT_DIR)/sensors/battery.o : $(USER_DIR)/sensors/battery.c $(USER_DIR)/sensors/battery.h $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/battery.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/battery.c -o $@ $(OBJECT_DIR)/battery_unittest.o : \ $(TEST_DIR)/battery_unittest.cc \ @@ -109,7 +119,7 @@ $(OBJECT_DIR)/battery_unittest.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/battery_unittest.cc -o $@ + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/battery_unittest.cc -o $@ battery_unittest : \ $(OBJECT_DIR)/sensors/battery.o \ @@ -117,7 +127,7 @@ battery_unittest : \ $(OBJECT_DIR)/battery_unittest.o \ $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ $(OBJECT_DIR)/flight/imu.o : \ $(USER_DIR)/flight/imu.c \ @@ -125,7 +135,7 @@ $(OBJECT_DIR)/flight/imu.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/imu.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/imu.c -o $@ $(OBJECT_DIR)/flight_imu_unittest.o : \ $(TEST_DIR)/flight_imu_unittest.cc \ @@ -133,7 +143,7 @@ $(OBJECT_DIR)/flight_imu_unittest.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -o $@ + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -o $@ flight_imu_unittest : \ $(OBJECT_DIR)/flight/imu.o \ @@ -141,7 +151,7 @@ flight_imu_unittest : \ $(OBJECT_DIR)/flight_imu_unittest.o \ $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ @@ -151,7 +161,7 @@ $(OBJECT_DIR)/flight/altitudehold.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@ $(OBJECT_DIR)/altitude_hold_unittest.o : \ $(TEST_DIR)/altitude_hold_unittest.cc \ @@ -159,14 +169,14 @@ $(OBJECT_DIR)/altitude_hold_unittest.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/altitude_hold_unittest.cc -o $@ + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/altitude_hold_unittest.cc -o $@ altitude_hold_unittest : \ $(OBJECT_DIR)/flight/altitudehold.o \ $(OBJECT_DIR)/altitude_hold_unittest.o \ $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ $(OBJECT_DIR)/flight/gps_conversion.o : \ @@ -175,7 +185,7 @@ $(OBJECT_DIR)/flight/gps_conversion.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@ $(OBJECT_DIR)/gps_conversion_unittest.o : \ $(TEST_DIR)/gps_conversion_unittest.cc \ @@ -183,14 +193,14 @@ $(OBJECT_DIR)/gps_conversion_unittest.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@ + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@ gps_conversion_unittest : \ $(OBJECT_DIR)/flight/gps_conversion.o \ $(OBJECT_DIR)/gps_conversion_unittest.o \ $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ @@ -200,7 +210,7 @@ $(OBJECT_DIR)/telemetry/hott.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/hott.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/hott.c -o $@ $(OBJECT_DIR)/telemetry_hott_unittest.o : \ $(TEST_DIR)/telemetry_hott_unittest.cc \ @@ -208,7 +218,7 @@ $(OBJECT_DIR)/telemetry_hott_unittest.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_hott_unittest.cc -o $@ + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_hott_unittest.cc -o $@ telemetry_hott_unittest : \ $(OBJECT_DIR)/telemetry/hott.o \ @@ -216,7 +226,7 @@ telemetry_hott_unittest : \ $(OBJECT_DIR)/flight/gps_conversion.o \ $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ @@ -226,7 +236,7 @@ $(OBJECT_DIR)/io/rc_controls.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/rc_controls.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/rc_controls.c -o $@ $(OBJECT_DIR)/rc_controls_unittest.o : \ $(TEST_DIR)/rc_controls_unittest.cc \ @@ -234,14 +244,14 @@ $(OBJECT_DIR)/rc_controls_unittest.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@ + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@ rc_controls_unittest : \ $(OBJECT_DIR)/io/rc_controls.o \ $(OBJECT_DIR)/rc_controls_unittest.o \ $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ $(OBJECT_DIR)/io/ledstrip.o : \ @@ -250,7 +260,7 @@ $(OBJECT_DIR)/io/ledstrip.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@ $(OBJECT_DIR)/ledstrip_unittest.o : \ $(TEST_DIR)/ledstrip_unittest.cc \ @@ -258,14 +268,14 @@ $(OBJECT_DIR)/ledstrip_unittest.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@ + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@ ledstrip_unittest : \ $(OBJECT_DIR)/io/ledstrip.o \ $(OBJECT_DIR)/ledstrip_unittest.o \ $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ @@ -275,7 +285,7 @@ $(OBJECT_DIR)/drivers/light_ws2811strip.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/light_ws2811strip.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/light_ws2811strip.c -o $@ $(OBJECT_DIR)/ws2811_unittest.o : \ $(TEST_DIR)/ws2811_unittest.cc \ @@ -283,13 +293,13 @@ $(OBJECT_DIR)/ws2811_unittest.o : \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ws2811_unittest.cc -o $@ + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ws2811_unittest.cc -o $@ ws2811_unittest : \ $(OBJECT_DIR)/drivers/light_ws2811strip.o \ $(OBJECT_DIR)/ws2811_unittest.o \ $(OBJECT_DIR)/gtest_main.a - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ diff --git a/src/test/unit/gps_conversion_unittest.cc b/src/test/unit/gps_conversion_unittest.cc index f75085b5b3..8f0c20e25d 100644 --- a/src/test/unit/gps_conversion_unittest.cc +++ b/src/test/unit/gps_conversion_unittest.cc @@ -36,7 +36,7 @@ TEST(GpsConversionTest, GPSCoordToDegrees_BadString) } typedef struct gpsConversionExpectation_s { - char *coord; + const char *coord; uint32_t degrees; } gpsConversionExpectation_t; diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 067b8983df..04c2a53869 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -315,7 +315,7 @@ TEST(ColorTest, parseColor) { 333, 22, 1 } }; - char *testColors[TEST_COLOR_COUNT] = { + const char *testColors[TEST_COLOR_COUNT] = { "0,0,0", "1,1,1", "359,255,255", diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 2f72a23e2e..39c8093167 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -17,6 +17,7 @@ #pragma once +#define MAG #define BARO #define GPS #define TELEMETRY diff --git a/src/test/unit/ws2811_unittest.cc b/src/test/unit/ws2811_unittest.cc index da13b9d8ef..59a6dad759 100644 --- a/src/test/unit/ws2811_unittest.cc +++ b/src/test/unit/ws2811_unittest.cc @@ -39,7 +39,7 @@ STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue); TEST(WS2812, updateDMABuffer) { // given - rgbColor24bpp_t color1 = {0xFF,0xAA,0x55}; + rgbColor24bpp_t color1 = { .raw = {0xFF,0xAA,0x55} }; // and dmaBufferOffset = 0; From 138fd963a784a8509104d1da98b6dc6a7a41864e Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 15 Jan 2015 10:38:48 +0000 Subject: [PATCH 027/182] Removing known issues and todo list, the issue tracker is now used instead. --- docs/development/Development.md | 21 ++------------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/docs/development/Development.md b/docs/development/Development.md index fddd58020c..a450c35789 100755 --- a/docs/development/Development.md +++ b/docs/development/Development.md @@ -16,6 +16,8 @@ This project could really do with some functional tests which test the behaviour All pull requests to add/improve the testability of the code or testing methods are highly sought! +Note: Tests are written in C++ and linked with with firmware's C code. + ##General principals 1. Name everything well. @@ -48,22 +50,3 @@ You can run them on the command line to execute the tests and to see the test re You can also step-debug the tests in eclipse and you can use the GoogleTest test runner to make building and re-running the tests simple. The tests are currently always compiled with debugging information enabled, there may be additional warnings, if you see any warnings please attempt to fix them and submit pull requests with the fixes. - - -##TODO - -* Test OpenLRSNG's RSSI PWM on AUX5-8. -* Add support for UART3/4 on STM32F3. -* Cleanup validateAndFixConfig and pwm_mapping.c to use some kind of feature/timer/io pin mapping to remove #ifdef -* Split RX config into RC config and RX config. -* Enabling/disabling features should not take effect until reboot since. Main loop executes and uses new flags as they are set in the cli but -appropriate init methods will not have been called which results in undefined behaviour and could damage connected devices - this is a legacy -problem from baseflight. -* Solve all the naze rev4/5 HSE_VALUE == 8000000/1200000 checking, the checks should only apply to the naze32 target. See system_stm32f10x.c/SetSysClock(). - -##Known Issues - -* Softserial RX on STM32F3 does not work. TX is fine. -* Dynamic throttle PID does not work with new pid controller. -* Autotune does not work yet with with new pid controller. - From 947bb0d9184dadff2dc0023c650b0ceda67f7836 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 15 Jan 2015 10:48:54 +0000 Subject: [PATCH 028/182] Minor code size reduction. Adding some const correctness. --- src/main/common/printf.c | 6 +++--- src/main/common/printf.h | 6 +++--- src/main/common/typeconversion.c | 2 +- src/main/common/typeconversion.h | 2 +- src/main/io/display.c | 10 +++++++--- 5 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/main/common/printf.c b/src/main/common/printf.c index e558ff8620..46e5b3902c 100644 --- a/src/main/common/printf.c +++ b/src/main/common/printf.c @@ -69,7 +69,7 @@ static void putchw(void *putp, putcf putf, int n, char z, char *bf) putf(putp, ch); } -void tfp_format(void *putp, putcf putf, char *fmt, va_list va) +void tfp_format(void *putp, putcf putf, const char *fmt, va_list va) { char bf[12]; @@ -154,7 +154,7 @@ void init_printf(void *putp, void (*putf) (void *, char)) stdout_putp = putp; } -void tfp_printf(char *fmt, ...) +void tfp_printf(const char *fmt, ...) { va_list va; va_start(va, fmt); @@ -168,7 +168,7 @@ static void putcp(void *p, char c) *(*((char **) p))++ = c; } -void tfp_sprintf(char *s, char *fmt, ...) +void tfp_sprintf(char *s, const char *fmt, ...) { va_list va; diff --git a/src/main/common/printf.h b/src/main/common/printf.h index 410c867a52..bcb78bcbb7 100644 --- a/src/main/common/printf.h +++ b/src/main/common/printf.h @@ -107,10 +107,10 @@ regs Kusti, 23.10.2004 void init_printf(void *putp, void (*putf) (void *, char)); -void tfp_printf(char *fmt, ...); -void tfp_sprintf(char *s, char *fmt, ...); +void tfp_printf(const char *fmt, ...); +void tfp_sprintf(char *s, const char *fmt, ...); -void tfp_format(void *putp, void (*putf) (void *, char), char *fmt, va_list va); +void tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list va); #define printf tfp_printf #define sprintf tfp_sprintf diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 2b94ef5fe3..e3e788d35d 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -90,7 +90,7 @@ int a2d(char ch) return -1; } -char a2i(char ch, char **src, int base, int *nump) +char a2i(char ch, const char **src, int base, int *nump) { char *p = *src; int num = 0; diff --git a/src/main/common/typeconversion.h b/src/main/common/typeconversion.h index 659c8b8341..62b437875b 100644 --- a/src/main/common/typeconversion.h +++ b/src/main/common/typeconversion.h @@ -20,7 +20,7 @@ void uli2a(unsigned long int num, unsigned int base, int uc, char *bf); void li2a(long num, char *bf); void ui2a(unsigned int num, unsigned int base, int uc, char *bf); void i2a(int num, char *bf); -char a2i(char ch, char **src, int base, int *nump); +char a2i(char ch, const char **src, int base, int *nump); char *ftoa(float x, char *floatString); float fastA2F(const char *p); diff --git a/src/main/io/display.c b/src/main/io/display.c index 691dc546f8..001615e874 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -378,24 +378,28 @@ void showBatteryPage(void) void showSensorsPage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; + static const char *format = "%c = %5d %5d %5d"; i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(" X Y Z"); + if (sensors(SENSOR_ACC)) { - tfp_sprintf(lineBuffer, "A = %5d %5d %5d", accSmooth[X], accSmooth[Y], accSmooth[Z]); + tfp_sprintf(lineBuffer, format, 'A', accSmooth[X], accSmooth[Y], accSmooth[Z]); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); } + if (sensors(SENSOR_GYRO)) { - tfp_sprintf(lineBuffer, "G = %5d %5d %5d", gyroADC[X], gyroADC[Y], gyroADC[Z]); + tfp_sprintf(lineBuffer, format, 'G', gyroADC[X], gyroADC[Y], gyroADC[Z]); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); } + #ifdef MAG if (sensors(SENSOR_MAG)) { - tfp_sprintf(lineBuffer, "M = %5d %5d %5d", magADC[X], magADC[Y], magADC[Z]); + tfp_sprintf(lineBuffer, format, 'M', magADC[X], magADC[Y], magADC[Z]); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); From ad65722f0e311b2b94b357e19357e3eb6454f647 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 15 Jan 2015 16:53:14 +0000 Subject: [PATCH 029/182] Code size reduction when led animation is disabled. --- src/main/io/ledstrip.c | 47 ++++++++++++++++++++++++++---------------- 1 file changed, 29 insertions(+), 18 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 07c3f1518d..75eadc25da 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -50,14 +50,26 @@ static bool ledStripInitialised = false; static failsafe_t* failsafe; +//#define USE_LED_ANIMATION + +// timers +#ifdef USE_LED_ANIMATION +static uint32_t nextAnimationUpdateAt = 0; +#endif + +static uint32_t nextIndicatorFlashAt = 0; +static uint32_t nextWarningFlashAt = 0; + +#define LED_STRIP_20HZ ((1000 * 1000) / 20) +#define LED_STRIP_10HZ ((1000 * 1000) / 10) +#define LED_STRIP_5HZ ((1000 * 1000) / 5) + #if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH #error "Led strip length must match driver" #endif hsvColor_t *colors; -//#define USE_LED_ANIMATION - // H S V #define LED_BLACK { 0, 0, 0} #define LED_WHITE { 0, 255, 255} @@ -438,15 +450,6 @@ void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSiz sprintf(ledConfigBuffer, "%u,%u:%s:%s", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions); } -// timers -uint32_t nextAnimationUpdateAt = 0; -uint32_t nextIndicatorFlashAt = 0; -uint32_t nextWarningFlashAt = 0; - -#define LED_STRIP_20HZ ((1000 * 1000) / 20) -#define LED_STRIP_10HZ ((1000 * 1000) / 10) -#define LED_STRIP_5HZ ((1000 * 1000) / 5) - void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors) { // apply up/down colors regardless of quadrant. @@ -688,6 +691,7 @@ void applyLedThrottleLayer() } } +#ifdef USE_LED_ANIMATION static uint8_t frameCounter = 0; static uint8_t previousRow; @@ -705,7 +709,6 @@ static void updateLedAnimationState(void) frameCounter = (frameCounter + 1) % animationFrames; } -#ifdef USE_LED_ANIMATION static void applyLedAnimationLayer(void) { const ledConfig_t *ledConfig; @@ -740,11 +743,18 @@ void updateLedStrip(void) uint32_t now = micros(); - bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; - bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; - - if (!(warningFlashNow || indicatorFlashNow || animationUpdateNow)) { + bool indicatorFlashNow = indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; + bool warningFlashNow =warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; +#ifdef USE_LED_ANIMATION + bool animationUpdateNow = animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; +#endif + if (!( + indicatorFlashNow || + warningFlashNow +#ifdef USE_LED_ANIMATION + || animationUpdateNow +#endif + )) { return; } @@ -779,14 +789,15 @@ void updateLedStrip(void) applyLedIndicatorLayer(indicatorFlashState); +#ifdef USE_LED_ANIMATION if (animationUpdateNow) { nextAnimationUpdateAt = now + LED_STRIP_20HZ; updateLedAnimationState(); } -#ifdef USE_LED_ANIMATION applyLedAnimationLayer(); #endif + ws2811UpdateStrip(); } From a0969755fdfdabefcab52f3b2e7972c91ebcb392 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 15 Jan 2015 16:55:03 +0000 Subject: [PATCH 030/182] Move gui screenshot to new Screenshots folder. --- docs/Configuration.md | 2 +- docs/{ => Screenshots}/cleanflight-gui.png | Bin 2 files changed, 1 insertion(+), 1 deletion(-) rename docs/{ => Screenshots}/cleanflight-gui.png (100%) diff --git a/docs/Configuration.md b/docs/Configuration.md index 66ad1c39df..1fdda5de42 100644 --- a/docs/Configuration.md +++ b/docs/Configuration.md @@ -15,7 +15,7 @@ __Due to ongoing development, the fact that the GUI cannot yet backup all your s ## GUI -![Cleanflight Gui](cleanflight-gui.png) +![Cleanflight Gui](Screenshots/cleanflight-gui.png) The GUI tool is the preferred way of configuration. The GUI tool also includes a terminal which can be used to interact with the CLI. diff --git a/docs/cleanflight-gui.png b/docs/Screenshots/cleanflight-gui.png similarity index 100% rename from docs/cleanflight-gui.png rename to docs/Screenshots/cleanflight-gui.png From d2e6742917a7a1b9b2e913d124286551f65eea19 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 15 Jan 2015 16:55:37 +0000 Subject: [PATCH 031/182] Fix typo in Configuration.md --- docs/Configuration.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Configuration.md b/docs/Configuration.md index 1fdda5de42..9e187766ca 100644 --- a/docs/Configuration.md +++ b/docs/Configuration.md @@ -1,6 +1,6 @@ # Configuration -Cleanflight is configured primarilty using the Cleanflight Configurator GUI. +Cleanflight is configured primarily using the Cleanflight Configurator GUI. Both the command line interface and GUI are accessible by connecting to a serial port on the target, be it a USB virtual serial port, physical hardware UART port or a SoftSerial port. From 51dd9a43678f8ec886001c7730a7a8397c0bd5b0 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Thu, 15 Jan 2015 20:27:00 +0100 Subject: [PATCH 032/182] Added documentation for Align DMSS RJ01 receiver support. --- docs/Rx.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/docs/Rx.md b/docs/Rx.md index 98b2cee6bf..0a4285e075 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -86,6 +86,11 @@ These receivers are reported working: XG14 14ch DMSS System w/RG731BX XBus Receiver http://www.jramericas.com/233794/JRP00631/ +There exist a remote receiver made for small BNF-models like the Align T-Rex 150 helicopter. The code also supports using the Align DMSS RJ01 receiver directly with the cleanflight software. +To use this receiver you must power it with 3V from the hardware, and then connect the serial line as other serial RX receivers. +In order for this receiver to work, you need to specify the XBUS_MODE_B_RJ01 for serialrx_provider. Note that you need to set your radio mode for XBUS "MODE B" also for this receiver to work. +Receiver name: Align DMSS RJ01 (HER15001) + ### SUMD 16 channels via serial currently supported. @@ -135,7 +140,7 @@ For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` CLI setting as | SUMD | 3 | | SUMH | 4 | | XBUS_MODE_B | 5 | - +| XBUS_MODE_B_RJ01 | 6 | ### PPM/PWM input filtering. From a61f7eeddfe57595bb4f83dfba47c0b36c4a2810 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Fri, 16 Jan 2015 10:45:01 +1300 Subject: [PATCH 033/182] Clean shutdown of blackbox (allows us to write "log completed" event) --- src/main/blackbox/blackbox.c | 104 +++++++++++++++++++++---- src/main/blackbox/blackbox.h | 7 +- src/main/blackbox/blackbox_fielddefs.h | 35 +++++++++ src/main/mw.c | 3 - 4 files changed, 130 insertions(+), 19 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 028f54c903..447f791f19 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -74,7 +74,6 @@ #include "config/config_profile.h" #include "config/config_master.h" -#include "blackbox_fielddefs.h" #include "blackbox.h" #define BLACKBOX_BAUDRATE 115200 @@ -83,6 +82,9 @@ #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) +#define STATIC_ASSERT(condition, name ) \ + typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] + // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter: #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) @@ -235,7 +237,8 @@ typedef enum BlackboxState { BLACKBOX_STATE_SEND_GPS_G_HEADERS, BLACKBOX_STATE_SEND_SYSINFO, BLACKBOX_STATE_PRERUN, - BLACKBOX_STATE_RUNNING + BLACKBOX_STATE_RUNNING, + BLACKBOX_STATE_SHUTTING_DOWN } BlackboxState; typedef struct gpsState_t { @@ -267,8 +270,11 @@ static struct { } u; } xmitState; +// Cache for FLIGHT_LOG_FIELD_CONDITION_* test results: static uint32_t blackboxConditionCache; +STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER, too_many_flight_log_conditions); + static uint32_t blackboxIteration; static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; @@ -671,6 +677,9 @@ static void blackboxSetState(BlackboxState newState) blackboxPFrameIndex = 0; blackboxIFrameIndex = 0; break; + case BLACKBOX_STATE_SHUTTING_DOWN: + xmitState.u.startTime = millis(); + break; default: ; } @@ -894,6 +903,14 @@ static void releaseBlackboxPort(void) serialSetBaudRate(blackboxPort, previousBaudRate); endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + + /* + * Normally this would be handled by mw.c, but since we take an unknown amount + * of time to shut down asynchronously, we're the only ones that know when to call it. + */ + if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { + mspAllocateSerialPorts(&masterConfig.serialConfig); + } } void startBlackbox(void) @@ -931,10 +948,18 @@ void startBlackbox(void) void finishBlackbox(void) { - if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) { - blackboxSetState(BLACKBOX_STATE_STOPPED); - + if (blackboxState == BLACKBOX_STATE_RUNNING) { + blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL); + + blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN); + } else if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED + && blackboxState != BLACKBOX_STATE_SHUTTING_DOWN) { + /* + * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event. + * Just give the port back and stop immediately. + */ releaseBlackboxPort(); + blackboxSetState(BLACKBOX_STATE_STOPPED); } } @@ -1141,7 +1166,6 @@ static bool blackboxWriteSysinfo() blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); xmitState.u.serialBudget -= strlen("H P interval:%d/%d\n"); - break; case 5: blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); @@ -1163,10 +1187,10 @@ static bool blackboxWriteSysinfo() blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u); xmitState.u.serialBudget -= strlen("H gyro.scale:0x%x\n") + 6; - break; + break; case 9: blackboxPrintf("H acc_1G:%u\n", acc_1G); - + xmitState.u.serialBudget -= strlen("H acc_1G:%u\n"); break; case 10: @@ -1193,10 +1217,49 @@ static bool blackboxWriteSysinfo() return false; } +/** + * Write the given event to the log immediately + */ +void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) +{ + if (blackboxState != BLACKBOX_STATE_RUNNING) + return; + + //Shared header for event frames + blackboxWrite('E'); + blackboxWrite(event); + + //Now serialize the data for this specific frame type + switch (event) { + case FLIGHT_LOG_EVENT_SYNC_BEEP: + writeUnsignedVB(data->syncBeep.time); + break; + case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: + blackboxWrite(data->autotuneCycleStart.phase); + blackboxWrite(data->autotuneCycleStart.cycle); + blackboxWrite(data->autotuneCycleStart.p); + blackboxWrite(data->autotuneCycleStart.i); + blackboxWrite(data->autotuneCycleStart.d); + break; + case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT: + blackboxWrite(data->autotuneCycleResult.overshot); + blackboxWrite(data->autotuneCycleStart.p); + blackboxWrite(data->autotuneCycleStart.i); + blackboxWrite(data->autotuneCycleStart.d); + break; + case FLIGHT_LOG_EVENT_LOG_END: + blackboxPrint("End of log"); + blackboxWrite(0); + break; + } +} + // Beep the buzzer and write the current time to the log as a synchronization point static void blackboxPlaySyncBeep() { - uint32_t now = micros(); + flightLogEvent_syncBeep_t eventData; + + eventData.time = micros(); /* * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later. @@ -1207,10 +1270,7 @@ static void blackboxPlaySyncBeep() // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive queueConfirmationBeep(1); - blackboxWrite('E'); - blackboxWrite(FLIGHT_LOG_EVENT_SYNC_BEEP); - - writeUnsignedVB(now); + blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData); } void handleBlackbox(void) @@ -1269,9 +1329,9 @@ void handleBlackbox(void) blackboxSetState(BLACKBOX_STATE_PRERUN); break; case BLACKBOX_STATE_PRERUN: - blackboxPlaySyncBeep(); - blackboxSetState(BLACKBOX_STATE_RUNNING); + + blackboxPlaySyncBeep(); break; case BLACKBOX_STATE_RUNNING: // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 @@ -1317,6 +1377,20 @@ void handleBlackbox(void) blackboxIFrameIndex++; } break; + case BLACKBOX_STATE_SHUTTING_DOWN: + //On entry of this state, startTime is set + + /* + * Wait for the log we've transmitted to make its way to the logger before we release the serial port, + * since releasing the port clears the Tx buffer. + * + * Don't wait longer than it could possibly take if something funky happens. + */ + if (millis() > xmitState.u.startTime + 200 || isSerialTransmitBufferEmpty(blackboxPort)) { + releaseBlackboxPort(); + blackboxSetState(BLACKBOX_STATE_STOPPED); + } + break; default: break; } diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 5d30671bbf..b971feec59 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -17,9 +17,12 @@ #pragma once -#include "common/axis.h" #include +#include "common/axis.h" +#include "flight/mixer.h" +#include "blackbox/blackbox_fielddefs.h" + typedef struct blackboxValues_t { uint32_t time; @@ -41,6 +44,8 @@ typedef struct blackboxValues_t { #endif } blackboxValues_t; +void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); + void initBlackbox(void); void handleBlackbox(void); void startBlackbox(void); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 1763561092..84f0ee1195 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -93,5 +93,40 @@ typedef enum FlightLogFieldSign { typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, + FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, + FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; + +typedef struct flightLogEvent_syncBeep_t { + uint32_t time; +} flightLogEvent_syncBeep_t; + +typedef struct flightLogEvent_autotuneCycleStart_t { + uint8_t phase; + uint8_t cycle; + uint8_t p; + uint8_t i; + uint8_t d; +} flightLogEvent_autotuneCycleStart_t; + +typedef struct flightLogEvent_autotuneCycleResult_t { + uint8_t overshot; + uint8_t p; + uint8_t i; + uint8_t d; +} flightLogEvent_autotuneCycleResult_t; + +typedef union flightLogEventData_t +{ + flightLogEvent_syncBeep_t syncBeep; + flightLogEvent_autotuneCycleStart_t autotuneCycleStart; + flightLogEvent_autotuneCycleResult_t autotuneCycleResult; + +} flightLogEventData_t; + +typedef struct flightLogEvent_t +{ + FlightLogEvent event; + flightLogEventData_t data; +} flightLogEvent_t; diff --git a/src/main/mw.c b/src/main/mw.c index fff9470b8d..c3b221b3b1 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -311,9 +311,6 @@ void mwDisarm(void) #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { finishBlackbox(); - if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { - mspAllocateSerialPorts(&masterConfig.serialConfig); - } } #endif } From 5de6fee787c1a7afe183e6de7b7c1d71139c0b87 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 15 Jan 2015 23:48:27 +0000 Subject: [PATCH 034/182] Minor const correctness. --- src/main/common/typeconversion.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index e3e788d35d..7df5bb1370 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -92,7 +92,7 @@ int a2d(char ch) char a2i(char ch, const char **src, int base, int *nump) { - char *p = *src; + const char *p = *src; int num = 0; int digit; while ((digit = a2d(ch)) >= 0) { From 8caff86006c7a2ff9b2b7456f565790581999d2b Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 16 Jan 2015 00:09:02 +0000 Subject: [PATCH 035/182] Update MSP_SET_LED_STRIP_CONFIG. Each LED must be sent one at a time since sending 32 leds needs a packet larger than the MSP receiver buffer allows. --- src/main/io/serial_msp.c | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 8943c4453d..3860c85c6a 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1442,28 +1442,26 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { - uint8_t ledCount = currentPort->dataSize / 6; - if (ledCount != MAX_LED_STRIP_LENGTH) { + i = read8(); + if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != 7) { headSerialError(0); break; } - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = read16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; + ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + uint16_t mask; + // currently we're storing directions and functions in a uint16 (flags) + // the msp uses 2 x uint16_t to cater for future expansion + mask = read16(); + ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; - mask = read16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; + mask = read16(); + ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; - mask = read8(); - ledConfig->xy = CALCULATE_LED_X(mask); + mask = read8(); + ledConfig->xy = CALCULATE_LED_X(mask); - mask = read8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); - } + mask = read8(); + ledConfig->xy |= CALCULATE_LED_Y(mask); } break; #endif From d8fa66244670cc2b40da8eeb98074ebb5939ae85 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 16 Jan 2015 00:42:15 +0000 Subject: [PATCH 036/182] Shorten SBus frame timeout. Fixes #361. See 5401092afaabf3b19391c3da8d3fb3fc211df889. --- src/main/rx/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 71c543c4ab..6df723db07 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -121,7 +121,7 @@ static void sbusDataReceive(uint16_t c) static uint8_t sbusFramePosition = 0; static uint32_t sbusTimeoutAt = 0; - uint32_t now = millis(); + uint32_t now = micros(); if ((int32_t)(sbusTimeoutAt - now) < 0) { sbusFramePosition = 0; From b64c71264cd14b0229cafdaf5f05bac3786aabaf Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 16 Jan 2015 01:00:40 +0000 Subject: [PATCH 037/182] Renaming getRcStickPosition to getRcStickDeflection and moving to rc_controls.c. --- src/main/flight/flight.c | 18 +++++++++++------- src/main/io/rc_controls.c | 4 ++++ src/main/io/rc_controls.h | 2 ++ src/main/mw.c | 11 ++++------- src/main/mw.h | 2 -- 5 files changed, 21 insertions(+), 16 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 18577217ea..dbc049b3d3 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -57,10 +57,10 @@ static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static int32_t errorAngleI[2] = { 0, 0 }; static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii @@ -97,7 +97,7 @@ bool shouldAutotune(void) #endif static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { float RateError, errorAngle, AngleRate, gyroRate; float ITerm,PTerm,DTerm; @@ -114,8 +114,8 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions - stickPosAil = getRcStickPosition(FD_ROLL); - stickPosEle = getRcStickPosition(FD_PITCH); + stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); + stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); if(abs(stickPosAil) > abs(stickPosEle)){ mostDeflectedPos = abs(stickPosAil); @@ -205,8 +205,10 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control } static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { + UNUSED(rxConfig); + int axis, prop; int32_t error, errorAngle; int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; @@ -288,8 +290,10 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa #define GYRO_I_MAX 256 static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, - rollAndPitchTrims_t *angleTrim) + rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { + UNUSED(rxConfig); + int32_t errorAngle; int axis; int32_t delta, deltaSum; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 66db651275..9bd4560085 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -517,6 +517,10 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) } } +int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { + return min(abs(rcData[axis] - midrc), 500); +} + void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) { uint8_t index; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 745acd4682..8441ea90f1 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -214,3 +214,5 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); bool isUsingSticksForArming(void); + +int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); diff --git a/src/main/mw.c b/src/main/mw.c index fbc9cfd7e6..9b2a45e0a6 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -97,7 +97,8 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m extern uint8_t dynP8[3], dynI8[3], dynD8[3]; extern failsafe_t *failsafe; -typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims); +typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype extern pidControllerFuncPtr pid_controller; @@ -355,11 +356,6 @@ void mwArm(void) } } -int32_t getRcStickPosition(int32_t axis) { - - return min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); -} - // Automatic ACC Offset Calibration bool AccInflightCalibrationArmed = false; bool AccInflightCalibrationMeasurementDone = false; @@ -714,7 +710,8 @@ void loop(void) ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, - ¤tProfile->accelerometerTrims + ¤tProfile->accelerometerTrims, + &masterConfig.rxConfig ); mixTable(); diff --git a/src/main/mw.h b/src/main/mw.h index 941efe8b37..aebee40bee 100644 --- a/src/main/mw.h +++ b/src/main/mw.h @@ -22,5 +22,3 @@ void handleInflightCalibrationStickPosition(); void mwDisarm(void); void mwArm(void); - -int32_t getRcStickPosition(int32_t axis); \ No newline at end of file From 999f0ce002995aef00f24696415e89ba68815d48 Mon Sep 17 00:00:00 2001 From: tracernz Date: Fri, 16 Jan 2015 22:00:45 +1300 Subject: [PATCH 038/182] Add virtual current sensor support Virtual current sensor calculates an estimate of current based on throttle position, current_meter_scale, and current_meter_offset. Documentation to follow later. --- src/main/config/config.c | 2 +- src/main/io/serial_cli.c | 2 +- src/main/sensors/battery.c | 31 ++++++++++++++++++++++++++----- src/main/sensors/battery.h | 10 ++++++++++ 4 files changed, 38 insertions(+), 7 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index f4b0dca420..457bf30212 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -108,7 +108,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 88; +static const uint8_t EEPROM_CONF_VERSION = 89; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 428496c159..7a952e7ff0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -305,7 +305,7 @@ const clivalue_t valueTable[] = { { "current_meter_scale", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, 1, 10000 }, { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, 0, 1650 }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, 0, 1 }, - + { "current_meter_type", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterType, 0, CURRENT_SENSOR_MAX }, { "align_gyro", VAR_UINT8 | MASTER_VALUE, &masterConfig.sensorAlignmentConfig.gyro_align, 0, 8 }, { "align_acc", VAR_UINT8 | MASTER_VALUE, &masterConfig.sensorAlignmentConfig.acc_align, 0, 8 }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 9766d40130..f3c3e80288 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -20,9 +20,14 @@ #include "common/maths.h" +#include "config/runtime_config.h" + #include "drivers/adc.h" #include "drivers/system.h" +#include "rx/rx.h" +#include "io/rc_controls.h" + #include "sensors/battery.h" // Battery monitoring stuff @@ -112,13 +117,29 @@ void updateCurrentMeter(int32_t lastUpdateAt) { static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; + int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; + int32_t throttleFactor = 0; - amperageRaw -= amperageRaw / 8; - amperageRaw += adcGetChannel(ADC_CURRENT); - amperage = currentSensorToCentiamps(amperageRaw / 8); + switch(batteryConfig->currentMeterType) { + case CURRENT_SENSOR_ADC: + amperageRaw -= amperageRaw / 8; + amperageRaw += adcGetChannel(ADC_CURRENT); + amperage = currentSensorToCentiamps(amperageRaw / 8); + break; + case CURRENT_SENSOR_VIRTUAL: + amperage = (int32_t)batteryConfig->currentMeterOffset; + if(ARMING_FLAG(ARMED)) { + throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); + amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + } + break; + case CURRENT_SENSOR_NONE: + amperage = 0; + break; + } - mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; - mAhDrawn = mAhdrawnRaw / (3600 * 100); + mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; + mAhDrawn = mAhdrawnRaw / (3600 * 100); } uint8_t calculateBatteryPercentage(void) diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index f90395f5fc..eaf989347f 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -20,6 +20,15 @@ #define VBAT_SCALE_DEFAULT 110 #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 +#define VIRTUAL_CURRENT_MIN 0 +#define VIRTUAL_CURRENT_MAX 0xffff + +typedef enum { + CURRENT_SENSOR_NONE = 0, + CURRENT_SENSOR_ADC, + CURRENT_SENSOR_VIRTUAL, + CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL +} currentSensor_e; typedef struct batteryConfig_s { uint8_t vbatscale; // adjust this to match battery voltage to reported value @@ -29,6 +38,7 @@ typedef struct batteryConfig_s { uint16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps + currentSensor_e currentMeterType; // type of current meter used, either ADC or virtual // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code. uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp From a81fad9cc2bdd148cf6a8377b3bb5bfafde87222 Mon Sep 17 00:00:00 2001 From: tracernz Date: Fri, 16 Jan 2015 22:47:10 +1300 Subject: [PATCH 039/182] Add virtual current sensor documentation ..and remove a couple of defines I didn't use. --- docs/Battery.md | 32 +++++++++++++++++++++++++++++++- src/main/sensors/battery.h | 2 -- 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/docs/Battery.md b/docs/Battery.md index c233c11947..aac57415d4 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -74,8 +74,19 @@ Enable current monitoring using the CLI command feature CURRENT_METER ``` +Configure the current meter type using the `current_meter_type` settings as per the following table. + +| Value | Sensor Type | +| ----- | ---------------------- | +| 0 | None | +| 1 | ADC/hardware sensor | +| 2 | Virtual sensor | + Configure capacity using the `battery_capacity` setting, which takes a value in mAh. +If you're using an OSD that expects the multiwii current meter output value, then set `multiwii_current_meter_output` to `1` (this multiplies amperage sent to MSP by 10). + +### ADC Sensor The current meter may need to be configured so that the value read at the ADC input matches actual current draw. Just like you need a voltmeter to correctly calibrate your voltage reading you also need an ammeter to calibrate your current sensor. Use the following settings to adjust calibration. @@ -83,4 +94,23 @@ Use the following settings to adjust calibration. `current_meter_scale` `current_meter_offset` -If you're using an OSD that expects the multiwii current meter output value, then set `multiwii_current_meter_output` to `1`. +### Virtual Sensor +The virtual sensor uses the throttle position to calculate as estimated current value. This is useful when a real sensor is not available. The following settings adjust the calibration. + +| Setting | Description | +| ----------------------------- | -------------------------------------------------------- | +| `current_meter_scale` | The throttle scaling factor [centiamps, i.e. 1/100th A] | +| `current_meter_offset` | The current at zero throttle [centiamps, i.e. 1/100th A] | + +If you know your current at zero throttle (Imin) and maximum throttle(Imax) you can calculate the scaling factors using the following formulas where Tmax is maximum throttle offset (i.e. for `max_throttle` = 1850, Tmax = 1850 - 1000 = 850): +``` +current_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50)) +current_meter_offset = Imin * 100 +``` +e.g. For a maximum current of 34.2 A and minimum current of 2.8 A with `max_throttle` = 1850 +``` +current_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50)) + = (34.2 - 2.8) * 100000 / (850 + (850 * 850 / 50)) + = 205 +current_meter_offset = Imin * 100 = 280 +``` diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index eaf989347f..38d2dd17d7 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -20,8 +20,6 @@ #define VBAT_SCALE_DEFAULT 110 #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 -#define VIRTUAL_CURRENT_MIN 0 -#define VIRTUAL_CURRENT_MAX 0xffff typedef enum { CURRENT_SENSOR_NONE = 0, From 4945446579678a61ef0f6aa68c0e7cb57821c0fd Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 16 Jan 2015 11:32:59 +0000 Subject: [PATCH 040/182] Updating documentation. --- CONTRIBUTING.md | 37 +++++++++++++++++++++++++++++++++++++ README.md | 2 +- build_docs.sh | 2 ++ docs/Boards.md | 32 ++++++++++++++++++++++++++++++++ docs/Failsafe.md | 2 +- docs/Introduction.md | 33 +++++++++++++++++++++++++++++++++ docs/Rx.md | 6 +++--- 7 files changed, 109 insertions(+), 5 deletions(-) create mode 100644 CONTRIBUTING.md create mode 100644 docs/Boards.md create mode 100644 docs/Introduction.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000000..2ff00122e4 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,37 @@ +# Contributing + +Please see the Contributing section of the README.md + +Please see the docs/developers folder for other notes. + +Ensure you understand the github workflow: https://guides.github.com/introduction/flow/index.html + +Please keep pull requests focused on one thing only, since this makes it easier to merge and test in a timely manner. + +If you need help with pull requests there are guides on github here: + +https://help.github.com/articles/creating-a-pull-request/ + +The main flow for a contributing is as follows: + +1. Login to github, goto the cleanflight repository and press `fork`. +2. `git clone ` +3. `cd cleanflight` +4. `git checkout master` +5. `git checkout -b my-new-code` +6. Make changes +7. `git add ` +8. `git commit` +9. `git push origin my-new-code` +10. Create pull request using github UI to merge your changes from your new branch into `cleanflight/master` +11. Repeat from step 4 for new other changes. + +The primary thing to remember is that separate pull requests should be created for separate branches. Never create a pull request from your `master` branch. + +Later, you can get the changes from the cleanflight repo into your `master` branch by adding cleanflight as a git remote and merging from it as follows: + +1. `git add remote cleanflight https://github.com/cleanflight/cleanflight.git` +2. `git checkout master` +3. `git fetch cleanflight` +4. `git merge cleanflight/master` + diff --git a/README.md b/README.md index 7f36dd25da..9ad931ae0a 100644 --- a/README.md +++ b/README.md @@ -83,7 +83,7 @@ https://github.com/cleanflight/cleanflight-configurator ## Contributing -Contributions are welcome and encouraged. You can contibute in many ways: +Contributions are welcome and encouraged. You can contribute in many ways: * Documentation updates and corrections. * How-To guides - received help? help others! diff --git a/build_docs.sh b/build_docs.sh index 396e276d12..464dbe5f6e 100755 --- a/build_docs.sh +++ b/build_docs.sh @@ -2,6 +2,7 @@ filename=Manual doc_files=( + 'Introduction.md' 'Installation.md' 'Configuration.md' 'Cli.md' @@ -24,6 +25,7 @@ doc_files=( 'Autotune.md' 'Blackbox.md' 'Migrating from baseflight.md' + 'Boards.md' 'Board - AlienWii32.md' 'Board - CC3D.md' 'Board - CJMCU.md' diff --git a/docs/Boards.md b/docs/Boards.md new file mode 100644 index 0000000000..e86484a8df --- /dev/null +++ b/docs/Boards.md @@ -0,0 +1,32 @@ +# Flight controller hardware + +The current focus is geared towards flight controller hardware that use the STM32F103 and STM32F303 series processors. The core logic is separated from the hardware drivers, porting to other processors is possible. + +The core set of supported flyable boards are: + +* CC3D +* CJMCU +* Flip32+ +* Naze32 +* Sparky +* AlienWii32 + +Cleanflight also runs on the following developer boards: + +* STM32F3Discovery +* Port103R +* EUSTM32F103RB + +There is also limited support for the following boards which may be removed due to lack of users or commercial availability. + +* Olimexino +* Naze32Pro +* STM32F3Discovery with Chebuzz F3 shield. + +Each board has it's pros and cons, before purchasing hardware the main thing to check is if the board offers enough serial ports and input/output pins for the hardware you want to use with it and that you can use them at the same time. On some boards some features are mutually exclusive. + +Please see the board-specific chapters in the manual for wiring details. + +There are off-shoots (forks) of the project that support the STM32F4 processors as found on the Revo and Quanton boards. + +Where applicable the chapters also provide links to other hardware that is known to work with Cleanflight, such as receivers, buzzers, etc. diff --git a/docs/Failsafe.md b/docs/Failsafe.md index 358bbcbb38..2f930b269a 100644 --- a/docs/Failsafe.md +++ b/docs/Failsafe.md @@ -28,7 +28,7 @@ a) no valid channel data from the RX is received via Serial RX. b) the first 4 Parallel PWM/PPM channels do not have valid signals. -And: +And when: c) the failsafe guard time specified by `failsafe_delay` has elapsed. diff --git a/docs/Introduction.md b/docs/Introduction.md new file mode 100644 index 0000000000..ad1a9c623a --- /dev/null +++ b/docs/Introduction.md @@ -0,0 +1,33 @@ +# Cleanflight + +Welcome to CleanFlight! + +Cleanflight is an community project which attempts to deliver flight controller firmware and related tools. + +## Primary Goals + +* Community driven. +* Friendly project atmosphere. +* Focus on the needs of users. +* Great flight performance. +* Understandable and maintainable code. + +## Hardware + +See the flight controller hardware chapter for details. + +## Software + +There are two primary components, the firmware and the configuration tool. The firmware is the code that runs on the flight controller board. The GUI configuration tool (configurator) is used to configure the flight controller, it runs on Windows, OSX and Linux. + +## Feedback & Contributing + +We welcome all feedback. If you love it we want to hear from you, if you have problems please tell us how we could improve things so we can make it better for everyone. + +If you want to contribute please see the notes here: + +https://github.com/cleanflight/cleanflight#contributing + +Developers should read this: + +https://github.com/cleanflight/cleanflight/blob/master/CONTRIBUTING.md diff --git a/docs/Rx.md b/docs/Rx.md index 98b2cee6bf..add5907afd 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -4,9 +4,9 @@ A receiver is used to receive radio control signals from your transmitter and co There are 3 basic types of receivers: -Parallel PWM Receivers -PPM Receivers -Serial Receivers +1. Parallel PWM Receivers +2. PPM Receivers +3. Serial Receivers ## Parallel PWM Receivers From d72983e1500fb5e99a3ac944b7e290f30b523f87 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 16 Jan 2015 12:38:44 +0000 Subject: [PATCH 041/182] Bump version numbers for release. --- src/main/io/serial_msp.c | 2 +- src/main/version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b422f393d1..e7b1d1a43b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -121,7 +121,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 2 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 3 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 diff --git a/src/main/version.h b/src/main/version.h index 59a5afa312..a31fe051fc 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define MW_VERSION 231 From 82161882bb6c652e30f6e02be9b519b2abe06c2e Mon Sep 17 00:00:00 2001 From: Ben Hitchcock Date: Sat, 17 Jan 2015 22:29:06 +0800 Subject: [PATCH 042/182] Bugfix for situations where Rx mapping is being used, and failsafe is kicking in unecessarily. An example is when a GPS is being used on UART2 (Rx pin 4), with the mapping of AET4R123. This code prevents failsafe being triggered by GPS data on Rx pin 4. --- src/main/rx/rx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 8e4bda1e85..8ab48f534b 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -289,7 +289,7 @@ void processRxChannels(void) uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel); if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) { - failsafe->vTable->checkPulse(rawChannel, sample); + failsafe->vTable->checkPulse(chan, sample); } // validate the range From 01fb028fe4d2bf5ba257403a3485ee368318b4e3 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 17 Jan 2015 19:51:11 +0000 Subject: [PATCH 043/182] Update HoTT connection details --- docs/Telemetry.md | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 1358f25a06..27e5010555 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -64,17 +64,18 @@ Only Electric Air Modules and GPS Modules are emulated, remember to enable them Serial ports use two wires but HoTT uses a single wire so some electronics are required so that the signals don't get mixed up. Connect as follows: -``` -HoTT TX/RX -> Serial RX (connect directly) -Serial TX -> 1N4148 Diode -(| )-> HoTT TX/RX (connect via diode) -``` + +* HoTT TX/RX `T` -> Serial RX (connect directly) +* HoTT TX/RX `T` -> Diode `-( |)-` > Serial TX (connect via diode) The diode should be arranged to allow the data signals to flow the right way ``` --(| )- == Diode, | indicates cathode marker. +-( |)- == Diode, | indicates cathode marker. ``` +1N4148 diodes have been tested and work with the GR-24. + As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which has 5V tolerant digital pins. Note: The SoftSerial ports are not listed as 5V tolerant in the STM32F103xx data sheet pinouts and pin description section. Verify if you require a 5v/3.3v level shifters. From ce3c0a583423ed3d8737f2789eec121e1135f912 Mon Sep 17 00:00:00 2001 From: rosek86 Date: Sat, 17 Jan 2015 23:51:00 +0100 Subject: [PATCH 044/182] Update Building in Windows.md typo --- docs/development/Building in Windows.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/Building in Windows.md b/docs/development/Building in Windows.md index 99b5bcd478..bb0f144f8e 100755 --- a/docs/development/Building in Windows.md +++ b/docs/development/Building in Windows.md @@ -38,7 +38,7 @@ Continue with the Installation and accept all autodetected dependencies. ---------- -versions do matter, 4.8-2014-q2 is known to work well. Download this version from htps://launchpad.net/gcc-arm-embedded/+download - preferrebly as a ZIP-File. +versions do matter, 4.8-2014-q2 is known to work well. Download this version from https://launchpad.net/gcc-arm-embedded/+download - preferrebly as a ZIP-File. Extract the contents of this archive to any folder of your choice, for instance ```C:\dev\gcc-arm-none-eabi-4_8-2014q2```. From cb84394348a72bba63d211f3f78596afad61f2f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krzysztof=20Rosi=C5=84ski?= Date: Sun, 18 Jan 2015 00:08:34 +0100 Subject: [PATCH 045/182] Update Development.md --- docs/development/Development.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/Development.md b/docs/development/Development.md index a450c35789..1ae4d9dd24 100755 --- a/docs/development/Development.md +++ b/docs/development/Development.md @@ -28,7 +28,7 @@ Note: Tests are written in C++ and linked with with firmware's C code. 6. Keep methods short - it makes it easier to test. 7. Don't be afraid of moving code to a new file - it helps to reduce test dependencies. 8. Avoid noise-words in variable names, like 'data' or 'info'. Think about what you're naming and name it well. Don't be afraid to rename anything. -9. Avoid comments taht describe what the code is doing, the code should describe itself. Comments are useful however for big-picture purposes and to document content of variables. +9. Avoid comments that describe what the code is doing, the code should describe itself. Comments are useful however for big-picture purposes and to document content of variables. 10. If you need to document a variable do it at the declarion, don't copy the comment to the `extern` usage since it will lead to comment rot. 11. Seek advice from other developers - know you can always learn more. 12. Be professional - attempts at humor or slating existing code in the codebase itself is not helpful when you have to change/fix it. From 6ef0f2ad63499a907879dd412c8bbe923007ca26 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 19 Jan 2015 17:28:48 +0100 Subject: [PATCH 046/182] Fixing typo. Fixes #380. --- docs/Gps.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Gps.md b/docs/Gps.md index 247df2427a..d2772f696d 100644 --- a/docs/Gps.md +++ b/docs/Gps.md @@ -58,7 +58,7 @@ UBlox GPS units can either be configured using the FC or manually. ### UBlox GPS manual configuration -Use UBox U-Center and connect your GPS to your computer. The CLI `gpspassthough` command may be of use if you do not have a spare USART to USB adapter. +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. Display the Packet Console (so you can see what messages your receiver is sending to your computer). From 48bd7c1e19feb18fa41f32b859070af9881d46fa Mon Sep 17 00:00:00 2001 From: chris-penny Date: Mon, 19 Jan 2015 22:53:02 +0000 Subject: [PATCH 047/182] I hope this is along the right lines... I have it running on NAZE32 ACRO it is using the BOXLLIGHTS at the moment but I'll update it to use BOXLEDLOW asap --- src/main/io/ledstrip.c | 11 +++++++++++ src/main/io/ledstrip.h | 1 + 2 files changed, 12 insertions(+) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 75eadc25da..7e2061cbaa 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -741,6 +741,11 @@ void updateLedStrip(void) return; } + if ( IS_RC_MODE_ACTIVE(BOXLLIGHTS)){ + ledStripDisable(); + return; + } + uint32_t now = micros(); bool indicatorFlashNow = indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; @@ -883,4 +888,10 @@ void ledStripEnable(void) ws2811LedStripInit(); } + +void ledStripDisable(void) +{ + setStripColor(&hsv_black); + ws2811UpdateStrip(); +} #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 83776b7c21..e3d6271afd 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -73,4 +73,5 @@ bool parseColor(uint8_t index, const char *colorConfig); void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); void ledStripEnable(void); +void ledStripDisable(void); From 0da650b5bd2b834d05871ef622189ac2b2417fef Mon Sep 17 00:00:00 2001 From: Chris Penny Date: Tue, 20 Jan 2015 10:45:10 +0000 Subject: [PATCH 048/182] changed rc mode to BOXLEDLOW --- src/main/io/ledstrip.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 7e2061cbaa..1eb4ab2ba3 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -741,7 +741,7 @@ void updateLedStrip(void) return; } - if ( IS_RC_MODE_ACTIVE(BOXLLIGHTS)){ + if ( IS_RC_MODE_ACTIVE(BOXLEDLOW)){ ledStripDisable(); return; } From aede4037cfc386e6c56505a7601f59284798eaa5 Mon Sep 17 00:00:00 2001 From: Chris Penny Date: Tue, 20 Jan 2015 19:51:07 +0000 Subject: [PATCH 049/182] LED on/off add via LEDLOW mode --- src/main/io/ledstrip.c | 8 +++++++- src/main/io/serial_msp.c | 9 +++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 1eb4ab2ba3..aaaf228bd4 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -48,6 +48,7 @@ #include "io/ledstrip.h" static bool ledStripInitialised = false; +static bool ledStripIsOnFlag = true; static failsafe_t* failsafe; //#define USE_LED_ANIMATION @@ -742,8 +743,13 @@ void updateLedStrip(void) } if ( IS_RC_MODE_ACTIVE(BOXLEDLOW)){ + if (ledStripIsOnFlag){ ledStripDisable(); - return; + ledStripIsOnFlag = false; + } + return; + }else{ + ledStripIsOnFlag = true; } uint32_t now = micros(); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index e7b1d1a43b..9d25c6415e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -617,6 +617,14 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; +#ifdef LED_STRIP + if (feature(FEATURE_LED_STRIP)) { + //activeBoxIds[activeBoxIdCount++] = BOXLEDMAX; + activeBoxIds[activeBoxIdCount++] = BOXLEDLOW; + //activeBoxIds[activeBoxIdCount++] = BOXLLIGHTS; + } +#endif + if (feature(FEATURE_INFLIGHT_ACC_CAL)) activeBoxIds[activeBoxIdCount++] = BOXCALIB; @@ -746,6 +754,7 @@ static bool processOutCommand(uint8_t cmdMSP) IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | From bfdb4d756455cc8739435067a19509a733de4621 Mon Sep 17 00:00:00 2001 From: Chris Penny Date: Tue, 20 Jan 2015 20:27:37 +0000 Subject: [PATCH 050/182] removed commented out code --- src/main/io/serial_msp.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9d25c6415e..5f2064f92f 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -619,9 +619,7 @@ void mspInit(serialConfig_t *serialConfig) #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { - //activeBoxIds[activeBoxIdCount++] = BOXLEDMAX; activeBoxIds[activeBoxIdCount++] = BOXLEDLOW; - //activeBoxIds[activeBoxIdCount++] = BOXLLIGHTS; } #endif From 4bf1d9cf51ffceb4ab520f535d9734cd53a72713 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 20 Jan 2015 21:53:18 +0100 Subject: [PATCH 051/182] Cleanup warnings in serial_usb_vcp.c --- src/main/drivers/serial_usb_vcp.c | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 6fbfd723e9..2aab5ad9a7 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -17,17 +17,17 @@ #include #include +#include +#include #include "platform.h" +#include "build_config.h" + #include "usb_core.h" #include "usb_init.h" #include "hw_config.h" -#include -#include - - #include "drivers/system.h" #include "serial.h" @@ -40,26 +40,37 @@ static vcpPort_t vcpPort; void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate) { + UNUSED(instance); + UNUSED(baudRate); + // TODO implement } void usbVcpSetMode(serialPort_t *instance, portMode_t mode) { + UNUSED(instance); + UNUSED(mode); + // TODO implement } bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance) { + UNUSED(instance); return true; } uint8_t usbVcpAvailable(serialPort_t *instance) { + UNUSED(instance); + return receiveLength & 0xFF; // FIXME use uint32_t return type everywhere } uint8_t usbVcpRead(serialPort_t *instance) { + UNUSED(instance); + uint8_t buf[1]; uint32_t rxed = 0; @@ -73,6 +84,8 @@ uint8_t usbVcpRead(serialPort_t *instance) void usbVcpWrite(serialPort_t *instance, uint8_t c) { + UNUSED(instance); + uint32_t txed; uint32_t start = millis(); From 42aac00b87bf80ff35576af43904b3d441f8f4b2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 20 Jan 2015 21:58:37 +0100 Subject: [PATCH 052/182] Cleanup warnings in sound_beeper_stm32f30x.c when no beeper is available. --- src/main/drivers/sound_beeper_stm32f30x.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/sound_beeper_stm32f30x.c b/src/main/drivers/sound_beeper_stm32f30x.c index 34f6536c90..d1c81b6d62 100644 --- a/src/main/drivers/sound_beeper_stm32f30x.c +++ b/src/main/drivers/sound_beeper_stm32f30x.c @@ -21,13 +21,17 @@ #include "platform.h" +#include "build_config.h" + #include "gpio.h" #include "sound_beeper.h" void initBeeperHardware(beeperConfig_t *config) { -#ifdef BEEPER +#ifndef BEEPER + UNUSED(config); +#else gpio_config_t gpioConfig = { config->gpioPin, config->gpioMode, From f825f15a9f6aeafbf32563ad250d10440448a52b Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 20 Jan 2015 22:01:14 +0100 Subject: [PATCH 053/182] Cleanup warnings in accgyro_l3gd20. This file needs some cleanup since it also does SPI initialisation and that should be moved out. --- src/main/drivers/accgyro_l3gd20.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index ebc223f681..8671ce8be2 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -20,6 +20,8 @@ #include "platform.h" +#include "build_config.h" + #include "common/maths.h" #include "system.h" @@ -110,7 +112,7 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); - SPI_I2S_DeInit(SPI1); + SPI_I2S_DeInit(SPIx); SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; SPI_InitStructure.SPI_Mode = SPI_Mode_Master; @@ -122,11 +124,11 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; SPI_InitStructure.SPI_CRCPolynomial = 7; - SPI_Init(SPI1, &SPI_InitStructure); + SPI_Init(SPIx, &SPI_InitStructure); - SPI_RxFIFOThresholdConfig(L3GD20_SPI, SPI_RxFIFOThreshold_QF); + SPI_RxFIFOThresholdConfig(SPIx, SPI_RxFIFOThreshold_QF); - SPI_Cmd(SPI1, ENABLE); + SPI_Cmd(SPIx, ENABLE); } void l3gd20GyroInit(void) @@ -194,6 +196,8 @@ static void l3gd20GyroRead(int16_t *gyroData) bool l3gd20Detect(gyro_t *gyro, uint16_t lpf) { + UNUSED(lpf); + gyro->init = l3gd20GyroInit; gyro->read = l3gd20GyroRead; From 24b1e9f49ee133cb914bfc8922eed69c833263a6 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 20 Jan 2015 23:28:42 +0100 Subject: [PATCH 054/182] Add LSM303DLHC compass support using HMC5883L driver --- src/main/target/STM32F3DISCOVERY/target.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 561a2f0581..2d8af45793 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -42,6 +42,9 @@ #define ACC #define USE_ACC_LSM303DLHC +#define MAG +#define USE_MAG_HMC5883 + #define BEEPER #define LED0 #define LED1 @@ -54,7 +57,7 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) -#define SENSORS_SET (SENSOR_ACC) +#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG) #define BLACKBOX #define GPS From 650389afb67098a65a0c58559c7fd3e0f25a8d10 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 20 Jan 2015 23:33:03 +0100 Subject: [PATCH 055/182] Disable ADC initialisation on CJMCU. Replace MASSIVEF3 with SPRACINGF3. Conditional VCP code inclusion. Other minor F1/F3 cleanups. --- Makefile | 86 ++++++++++++------- src/main/drivers/accgyro_mpu6050.c | 14 ++- src/main/drivers/accgyro_mpu6050.h | 5 ++ src/main/drivers/compass_hmc5883l.c | 8 +- src/main/drivers/compass_hmc5883l.h | 5 ++ src/main/drivers/pwm_mapping.c | 57 +++++++++++- src/main/drivers/pwm_mapping.h | 6 +- src/main/drivers/timer.c | 31 +++++++ src/main/drivers/timer.h | 12 --- src/main/io/serial.c | 4 + src/main/main.c | 2 +- src/main/sensors/initialisation.c | 20 +++++ src/main/target/CC3D/target.h | 4 + src/main/target/CHEBUZZF3/target.h | 4 + src/main/target/EUSTM32F103RC/target.h | 2 + src/main/target/MASSIVEF3/target.h | 63 -------------- src/main/target/NAZE/target.h | 1 + src/main/target/OLIMEXINO/target.h | 2 + src/main/target/PORT103R/target.h | 2 + src/main/target/SPARKY/target.h | 2 + .../system_stm32f30x.c | 4 +- .../system_stm32f30x.h | 0 src/main/target/SPRACINGF3/target.h | 86 +++++++++++++++++++ src/main/target/STM32F3DISCOVERY/target.h | 2 + 24 files changed, 305 insertions(+), 117 deletions(-) delete mode 100644 src/main/target/MASSIVEF3/target.h rename src/main/target/{MASSIVEF3 => SPRACINGF3}/system_stm32f30x.c (99%) rename src/main/target/{MASSIVEF3 => SPRACINGF3}/system_stm32f30x.h (100%) create mode 100644 src/main/target/SPRACINGF3/target.h diff --git a/Makefile b/Makefile index cda4d0421b..10c62d3574 100644 --- a/Makefile +++ b/Makefile @@ -35,7 +35,7 @@ SERIAL_DEVICE ?= /dev/ttyUSB0 FORKNAME = cleanflight -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC MASSIVEF3 PORT103R SPARKY ALIENWIIF1 +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENWIIF1 # Valid targets for OP BootLoader support OPBL_VALID_TARGETS = CC3D @@ -54,7 +54,7 @@ LINKER_DIR = $(ROOT)/src/main/target # Search path for sources VPATH := $(SRC_DIR):$(SRC_DIR)/startup -ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO MASSIVEF3 SPARKY)) +ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY)) STDPERIPH_DIR = $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver @@ -67,21 +67,31 @@ EXCLUDES = stm32f30x_crc.c \ STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) -DEVICE_STDPERIPH_SRC = $(USBPERIPH_SRC) \ +DEVICE_STDPERIPH_SRC = \ $(STDPERIPH_SRC) -VPATH := $(VPATH):$(CMSIS_DIR)/CM1/CoreSupport:$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x:$(USBFS_DIR)/src +VPATH := $(VPATH):$(CMSIS_DIR)/CM1/CoreSupport:$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM1/CoreSupport/*.c \ $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x/*.c)) INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(STDPERIPH_DIR)/inc \ - $(USBFS_DIR)/inc \ $(CMSIS_DIR)/CM1/CoreSupport \ - $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x \ + $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x + +ifneq ($(TARGET),SPRACINGF3) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(USBFS_DIR)/inc \ $(ROOT)/src/main/vcp +VPATH := $(VPATH):$(USBFS_DIR)/src + +DEVICE_STDPERIPH_SRC := $(DEVICE_STDPERIPH_SRC)\ + $(USBPERIPH_SRC) + +endif + LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_256k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion @@ -92,12 +102,6 @@ ifeq ($(TARGET),CHEBUZZF3) TARGET_FLAGS := $(TARGET_FLAGS) -DSTM32F3DISCOVERY endif -ifeq ($(TARGET),MASSIVEF3) -# MASSIVEF3 is a VARIANT of STM32F3DISCOVERY -TARGET_FLAGS := $(TARGET_FLAGS) -DSTM32F3DISCOVERY -endif - - else ifeq ($(TARGET),$(filter $(TARGET),EUSTM32F103RC PORT103R)) @@ -350,7 +354,8 @@ $(error OPBL specified with a unsupported target) endif endif -CJMCU_SRC = startup_stm32f10x_md_gcc.S \ +CJMCU_SRC = \ + startup_stm32f10x_md_gcc.S \ drivers/adc.c \ drivers/adc_stm32f10x.c \ drivers/accgyro_mpu6050.c \ @@ -371,7 +376,8 @@ CJMCU_SRC = startup_stm32f10x_md_gcc.S \ hardware_revision.c \ $(COMMON_SRC) -CC3D_SRC = startup_stm32f10x_md_gcc.S \ +CC3D_SRC = \ + startup_stm32f10x_md_gcc.S \ drivers/accgyro_spi_mpu6000.c \ drivers/adc.c \ drivers/adc_stm32f10x.c \ @@ -394,7 +400,8 @@ CC3D_SRC = startup_stm32f10x_md_gcc.S \ $(HIGHEND_SRC) \ $(COMMON_SRC) -STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S \ +STM32F30x_COMMON_SRC = \ + startup_stm32f30x_md_gcc.S \ drivers/adc.c \ drivers/adc_stm32f30x.c \ drivers/bus_i2c_stm32f30x.c \ @@ -408,29 +415,36 @@ STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S \ drivers/pwm_rx.c \ drivers/serial_uart.c \ drivers/serial_uart_stm32f30x.c \ - drivers/serial_usb_vcp.c \ drivers/sound_beeper_stm32f30x.c \ drivers/system_stm32f30x.c \ drivers/timer.c \ - drivers/timer_stm32f30x.c \ + drivers/timer_stm32f30x.c + +VCP_SRC = \ vcp/hw_config.c \ vcp/stm32_it.c \ vcp/usb_desc.c \ vcp/usb_endp.c \ vcp/usb_istr.c \ vcp/usb_prop.c \ - vcp/usb_pwr.c + vcp/usb_pwr.c \ + drivers/serial_usb_vcp.c -NAZE32PRO_SRC = $(STM32F30x_COMMON_SRC) \ +NAZE32PRO_SRC = \ + $(STM32F30x_COMMON_SRC) \ $(HIGHEND_SRC) \ - $(COMMON_SRC) + $(COMMON_SRC) \ + $(VCP_SRC) -STM32F3DISCOVERY_COMMON_SRC = $(STM32F30x_COMMON_SRC) \ +STM32F3DISCOVERY_COMMON_SRC = \ + $(STM32F30x_COMMON_SRC) \ drivers/accgyro_l3gd20.c \ drivers/accgyro_l3gd20.c \ - drivers/accgyro_lsm303dlhc.c + drivers/accgyro_lsm303dlhc.c \ + $(VCP_SRC) -STM32F3DISCOVERY_SRC = $(STM32F3DISCOVERY_COMMON_SRC) \ +STM32F3DISCOVERY_SRC = \ + $(STM32F3DISCOVERY_COMMON_SRC) \ drivers/accgyro_adxl345.c \ drivers/accgyro_bma280.c \ drivers/accgyro_mma845x.c \ @@ -442,25 +456,33 @@ STM32F3DISCOVERY_SRC = $(STM32F3DISCOVERY_COMMON_SRC) \ $(HIGHEND_SRC) \ $(COMMON_SRC) -CHEBUZZF3_SRC = $(STM32F3DISCOVERY_SRC) \ +CHEBUZZF3_SRC = \ + $(STM32F3DISCOVERY_SRC) \ $(HIGHEND_SRC) \ $(COMMON_SRC) -MASSIVEF3_SRC = $(STM32F3DISCOVERY_SRC) \ - $(HIGHEND_SRC) \ - $(COMMON_SRC) - -SPARKY_SRC = $(STM32F30x_COMMON_SRC) \ +SPARKY_SRC = \ + $(STM32F30x_COMMON_SRC) \ drivers/display_ug2864hsweg01.c \ drivers/accgyro_mpu6050.c \ drivers/barometer_ms5611.c \ drivers/compass_ak8975.c \ + drivers/serial_usb_vcp.c \ + $(HIGHEND_SRC) \ + $(COMMON_SRC) \ + $(VCP_SRC) + +SPRACINGF3_SRC = \ + $(STM32F30x_COMMON_SRC) \ + drivers/accgyro_mpu6050.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) -ifeq ($(TARGET),MASSIVEF3) -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_128k.ld -endif +#ifeq ($(TARGET),SPRACINGF3) +#LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_128k.ld +#endif # Search path and source files for the ST stdperiph library VPATH := $(VPATH):$(STDPERIPH_DIR)/src diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index e5044afc25..eacea46b0b 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -178,9 +178,17 @@ static const mpu6050Config_t *mpu6050Config = NULL; void mpu6050GpioInit(void) { gpio_config_t gpio; - if (mpu6050Config->gpioAPB2Peripherals) { - RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE); - } +#ifdef STM32F303 + if (mpu6050Config->gpioAHBPeripherals) { + RCC_AHBPeriphClockCmd(mpu6050Config->gpioAHBPeripherals, ENABLE); + } +#endif +#ifdef STM32F10X + if (mpu6050Config->gpioAPB2Peripherals) { + RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE); + } +#endif + gpio.pin = mpu6050Config->gpioPin; gpio.speed = Speed_2MHz; diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h index b05a611563..e52c013fff 100644 --- a/src/main/drivers/accgyro_mpu6050.h +++ b/src/main/drivers/accgyro_mpu6050.h @@ -18,7 +18,12 @@ #pragma once typedef struct mpu6050Config_s { +#ifdef STM32F303 + uint32_t gpioAHBPeripherals; +#endif +#ifdef STM32F10X uint32_t gpioAPB2Peripherals; +#endif uint16_t gpioPin; GPIO_TypeDef *gpioPort; } mpu6050Config_t; diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 98788f18b6..63d0566c90 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -142,10 +142,16 @@ void hmc5883lInit(void) gpio_config_t gpio; if (hmc5883Config) { +#ifdef STM32F303 + if (hmc5883Config->gpioAHBPeripherals) { + RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE); + } +#endif +#ifdef STM32F10X if (hmc5883Config->gpioAPB2Peripherals) { RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE); } - +#endif gpio.pin = hmc5883Config->gpioPin; gpio.speed = Speed_2MHz; gpio.mode = Mode_IN_FLOATING; diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index df07d21adf..bc6aabc311 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -18,7 +18,12 @@ #pragma once typedef struct hmc5883Config_s { +#ifdef STM32F303 + uint32_t gpioAHBPeripherals; +#endif +#ifdef STM32F10X uint32_t gpioAPB2Peripherals; +#endif uint16_t gpioPin; GPIO_TypeDef *gpioPort; } hmc5883Config_t; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 9130d14841..ac0a5caff6 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -69,7 +69,7 @@ enum { MAP_TO_SERVO_OUTPUT, }; -#if defined(NAZE) || defined(OLIMEXINO) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(EUSTM32F103RC) || defined(MASSIVEF3) || defined(PORT103R) +#if defined(NAZE) || defined(OLIMEXINO) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(EUSTM32F103RC) || defined(PORT103R) static const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed @@ -267,15 +267,64 @@ static const uint16_t multiPWM[] = { }; static const uint16_t airPPM[] = { + // TODO 0xFFFF }; static const uint16_t airPWM[] = { + // TODO 0xFFFF }; #endif + +#ifdef SPRACINGF3 +static const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +static const uint16_t airPPM[] = { + // TODO + 0xFFFF +}; + +static const uint16_t airPWM[] = { + // TODO + 0xFFFF +}; +#endif + static const uint16_t * const hardwareMaps[] = { multiPWM, multiPPM, @@ -387,6 +436,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) type = MAP_TO_SERVO_OUTPUT; #endif +#if defined(SPRACINGF3) + // remap PWM15+16 as servos + if ((timerIndex == PWM15 || timerIndex == PWM16) && timerHardwarePtr->tim == TIM15) + type = MAP_TO_SERVO_OUTPUT; +#endif + #if defined(NAZE32PRO) || (defined(STM32F3DISCOVERY) && !defined(CHEBUZZF3)) // remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer if (init->useSoftSerial) { diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index b11ac21b47..a78a6e4fa1 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -65,7 +65,7 @@ typedef struct pwmOutputConfiguration_s { uint8_t motorCount; } pwmOutputConfiguration_t; -// This indexes into the read-only hardware definition structure, timerHardware_t, as well as into pwmPorts structure with dynamic data. +// This indexes into the read-only hardware definition structure, timerHardware_t enum { PWM1 = 0, PWM2, @@ -80,5 +80,7 @@ enum { PWM11, PWM12, PWM13, - PWM14 + PWM14, + PWM15, + PWM16 }; diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index d3a86778d2..5c10e1674c 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -220,6 +220,37 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif +#ifdef SPRACINGF3 +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1}, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1}, // RC_CH3 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1}, // RC_CH4 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2}, // RC_CH5 - PB4 - *TIM3_CH2 + { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // RC_CH6 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // RC_CH7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, GPIOA, Pin_11, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_10}, // PWM3 - PA11 + { TIM4, GPIOA, Pin_12, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource12, GPIO_AF_10}, // PWM4 - PA12 + { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2}, // PWM5 - PB8 + { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2}, // PWM6 - PB9 + { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9}, // PWM7 - PA2 + { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // PWM8 - PA3 + + { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // GPIO_TIMER / LED_STRIP +}; + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) + +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + +#endif + #define USED_TIMER_COUNT BITCOUNT(USED_TIMERS) #define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 1a32699c53..d8d78a4471 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -17,18 +17,6 @@ #pragma once -#ifdef SPARKY -#define USABLE_TIMER_CHANNEL_COUNT 11 -#endif - -#ifdef CHEBUZZF3 -#define USABLE_TIMER_CHANNEL_COUNT 18 -#endif - -#ifdef CC3D -#define USABLE_TIMER_CHANNEL_COUNT 12 -#endif - #if !defined(USABLE_TIMER_CHANNEL_COUNT) #define USABLE_TIMER_CHANNEL_COUNT 14 #endif diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 185ecf51c5..7493f51d2e 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -73,7 +73,9 @@ static serialPort_t *serialPorts[SERIAL_PORT_COUNT]; #ifdef STM32F303xC static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { +#ifdef USE_VCP {SERIAL_PORT_USB_VCP, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, +#endif {SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, {SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, #if (SERIAL_PORT_COUNT > 3) @@ -85,7 +87,9 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { }; const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { +#ifdef USE_VCP {SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE }, +#endif {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #if (SERIAL_PORT_COUNT > 3) diff --git a/src/main/main.c b/src/main/main.c index 4a9656b8df..d2584fdba4 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -216,7 +216,7 @@ void init(void) #endif #endif -#if !defined(SPARKY) +#ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index d13711ec66..f4cbe90e5f 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -94,6 +94,15 @@ const mpu6050Config_t *selectMPU6050Config(void) return &nazeRev5MPU6050Config; } #endif + +#ifdef SPRACINGF3 + static const mpu6050Config_t spRacingF3MPU6050Config = { + .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, + .gpioPort = GPIOC, + .gpioPin = Pin_13 + }; + return &spRacingF3MPU6050Config; +#endif return NULL; } @@ -406,6 +415,17 @@ static void detectMag(uint8_t magHardwareToUse) hmc5883Config = &nazeHmc5883Config; #endif + +#ifdef SPRACINGF3 + hmc5883Config_t spRacingF3Hmc5883Config = { + .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, + .gpioPin = Pin_14, + .gpioPort = GPIOC + }; + + hmc5883Config = &spRacingF3Hmc5883Config; +#endif + #endif retry: diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 3ae21223b3..9db2089ae2 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -35,6 +35,8 @@ #define MPU6000_CS_PIN GPIO_Pin_4 #define MPU6000_SPI_INSTANCE SPI1 +#define USABLE_TIMER_CHANNEL_COUNT 12 + #define GYRO #define USE_GYRO_SPI_MPU6000 @@ -71,6 +73,8 @@ #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 +#define USE_ADC + #define SENSORS_SET (SENSOR_ACC) #define GPS diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index de560a7fc5..78bf6af63a 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -33,6 +33,8 @@ #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOE #define BEEPER_INVERTED +#define USABLE_TIMER_CHANNEL_COUNT 18 + #define GYRO #define USE_GYRO_L3GD20 #define USE_GYRO_MPU6050 @@ -65,6 +67,8 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) +#define USE_ADC + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 8ef66c2ccc..7063f10c6b 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -98,6 +98,8 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 +#define USE_ADC + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/MASSIVEF3/target.h b/src/main/target/MASSIVEF3/target.h deleted file mode 100644 index 1484ec94a9..0000000000 --- a/src/main/target/MASSIVEF3/target.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight 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. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "MF3A" - -#define LED0_GPIO GPIOE -#define LED0_PIN Pin_8|Pin_12 // Blue LEDs - PE8/PE12 -#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOE -#define LED0_INVERTED -#define LED1_GPIO GPIOE -#define LED1_PIN Pin_10|Pin_14 // Orange LEDs - PE10/PE14 -#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOE -#define LED1_INVERTED - -#define BEEP_GPIO GPIOE -#define BEEP_PIN Pin_9|Pin_13 // Red LEDs - PE9/PE13 -#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOE -#define BEEPER_INVERTED - - -#define BEEPER_INVERTED - -#define GYRO -#define ACC - -#define BARO -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 - -#define BEEPER -#define LED0 -#define LED1 - -#define USE_VCP -#define USE_USART1 -#define USE_USART2 -#define SERIAL_PORT_COUNT 3 - -#define USE_I2C -#define I2C_DEVICE (I2CDEV_1) - -#define SENSORS_SET (SENSOR_ACC) - -#define GPS -#define TELEMETRY -#define SERIAL_RX -#define AUTOTUNE diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 1686d523c9..d2f37240f1 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -117,6 +117,7 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 +#define USE_ADC #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 935ff691c5..7dd042e571 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -85,6 +85,8 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 +#define USE_ADC + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 0f7805213f..ddd82cf072 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -95,6 +95,8 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 +#define USE_ADC + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define LED0 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index d020296653..aee9257cd1 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -26,6 +26,8 @@ #define LED1_PIN Pin_5 // Green LEDs - PB5 #define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB +#define USABLE_TIMER_CHANNEL_COUNT 11 + // MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component. #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/MASSIVEF3/system_stm32f30x.c b/src/main/target/SPRACINGF3/system_stm32f30x.c similarity index 99% rename from src/main/target/MASSIVEF3/system_stm32f30x.c rename to src/main/target/SPRACINGF3/system_stm32f30x.c index 8166961185..fca6969825 100644 --- a/src/main/target/MASSIVEF3/system_stm32f30x.c +++ b/src/main/target/SPRACINGF3/system_stm32f30x.c @@ -55,9 +55,9 @@ *----------------------------------------------------------------------------- * APB1 Prescaler | 2 *----------------------------------------------------------------------------- - * HSE Frequency(Hz) | 12000000 + * HSE Frequency(Hz) | 8000000 *---------------------------------------------------------------------------- - * PLLMUL | 6 + * PLLMUL | 9 *----------------------------------------------------------------------------- * PREDIV | 1 *----------------------------------------------------------------------------- diff --git a/src/main/target/MASSIVEF3/system_stm32f30x.h b/src/main/target/SPRACINGF3/system_stm32f30x.h similarity index 100% rename from src/main/target/MASSIVEF3/system_stm32f30x.h rename to src/main/target/SPRACINGF3/system_stm32f30x.h diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h new file mode 100644 index 0000000000..af35c8a182 --- /dev/null +++ b/src/main/target/SPRACINGF3/target.h @@ -0,0 +1,86 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SRF3" + +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_3 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB + +#define BEEP_GPIO GPIOC +#define BEEP_PIN Pin_15 +#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC + +#define USABLE_TIMER_CHANNEL_COUNT 17 + +#define GYRO +#define USE_GYRO_MPU6050 + +#define ACC +#define USE_ACC_MPU6050 + +#define BARO +#define USE_BARO_MS5611 + +#define MAG +#define USE_MAG_HMC5883 + +#define BEEPER +#define LED0 + +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define SERIAL_PORT_COUNT 3 + +#define UART2_TX_PIN GPIO_Pin_14 // PA14 / SWCLK +#define UART2_RX_PIN GPIO_Pin_15 // PA15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource14 +#define UART2_RX_PINSOURCE GPIO_PinSource15 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +//#define USE_SPI +//#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define LED_STRIP +#define LED_STRIP_TIMER TIM1 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_6 +#define WS2811_PIN GPIO_Pin_8 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM1 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn + +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) + +#define GPS +#define BLACKBOX +#define TELEMETRY +#define SERIAL_RX +#define AUTOTUNE + diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 561a2f0581..ccad4b6e5a 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -54,6 +54,8 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) +#define USE_ADC + #define SENSORS_SET (SENSOR_ACC) #define BLACKBOX From bfa79ab0104a0a56dfe6cc9ee04cdae82bbc182b Mon Sep 17 00:00:00 2001 From: Chris Penny Date: Tue, 20 Jan 2015 22:43:32 +0000 Subject: [PATCH 056/182] changed ledStripIsOnFlag to ledStripEnabled and ledStripEnabled fail check --- src/main/io/ledstrip.c | 21 +++++++++++++++------ src/main/io/ledstrip.h | 2 +- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index aaaf228bd4..3555417a93 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -48,9 +48,12 @@ #include "io/ledstrip.h" static bool ledStripInitialised = false; -static bool ledStripIsOnFlag = true; + static failsafe_t* failsafe; +static bool ledStripEnabled = true; +static void ledStripDisable(void); + //#define USE_LED_ANIMATION // timers @@ -743,14 +746,20 @@ void updateLedStrip(void) } if ( IS_RC_MODE_ACTIVE(BOXLEDLOW)){ - if (ledStripIsOnFlag){ + if (ledStripEnabled){ ledStripDisable(); - ledStripIsOnFlag = false; + ledStripEnabled = false; } - return; }else{ - ledStripIsOnFlag = true; + ledStripEnabled = true; } + + if (!ledStripEnabled){ + return; + } + + + uint32_t now = micros(); @@ -895,7 +904,7 @@ void ledStripEnable(void) ws2811LedStripInit(); } -void ledStripDisable(void) +static void ledStripDisable(void) { setStripColor(&hsv_black); ws2811UpdateStrip(); diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index e3d6271afd..4e434b4003 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -73,5 +73,5 @@ bool parseColor(uint8_t index, const char *colorConfig); void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); void ledStripEnable(void); -void ledStripDisable(void); + From b7850270c473eb5a9d160e24b9e24c0815e2244c Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 21 Jan 2015 01:02:33 +0100 Subject: [PATCH 057/182] Cleanup CC3D/NAZE/OLIMEXINO ADC initialisation and PWM mapping. Fixes #297. --- src/main/drivers/adc_stm32f10x.c | 54 ++++++++++---------------- src/main/drivers/pwm_mapping.c | 20 ++++------ src/main/target/CC3D/target.h | 17 ++++++-- src/main/target/EUSTM32F103RC/target.h | 16 ++++++++ src/main/target/NAZE/target.h | 16 ++++++++ src/main/target/OLIMEXINO/target.h | 17 ++++++++ src/main/target/PORT103R/target.h | 16 ++++++++ 7 files changed, 107 insertions(+), 49 deletions(-) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 15b6049777..5f591ed0a7 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -48,7 +48,7 @@ extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; void adcInit(drv_adc_config_t *init) { -#ifdef CC3D +#if defined(CJMCU) || defined(CC3D) UNUSED(init); #endif @@ -64,59 +64,47 @@ void adcInit(drv_adc_config_t *init) GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; -#ifdef CC3D - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; - adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_0; - adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_BATTERY].enabled = true; - adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5; - -#else - // configure always-present battery index (ADC4) - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; - adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4; +#ifdef VBAT_ADC_GPIO_PIN + GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN; + GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure); + adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5; +#endif +#ifdef EXTERNAL1_ADC_GPIO if (init->enableExternal1) { -#ifdef OLIMEXINO - GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_5; - adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5; + GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN; + GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure); + adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5; -#endif - -#ifdef NAZE - GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_5; - adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5; - adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; - adcConfig[ADC_EXTERNAL1].enabled = true; - adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5; -#endif } -#endif // !CC3D +#endif +#ifdef RSSI_ADC_GPIO if (init->enableRSSI) { - GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_1; - adcConfig[ADC_RSSI].adcChannel = ADC_Channel_1; + GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN; + GPIO_Init(RSSI_ADC_GPIO, &GPIO_InitStructure); + adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5; } +#endif - GPIO_Init(GPIOA, &GPIO_InitStructure); - +#ifdef CURRENT_METER_ADC_GPIO if (init->enableCurrentMeter) { - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1; - GPIO_Init(GPIOB, &GPIO_InitStructure); - adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_9; + GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN; + GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure); + adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5; } - +#endif RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index ac0a5caff6..a09f6ec10f 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -394,24 +394,20 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #endif -#ifdef STM32F10X - // skip ADC for RSSI - if (init->useRSSIADC && timerIndex == PWM2) +#ifdef VBAT_ADC_GPIO + if (init->useVbat && timerHardwarePtr->gpio == VBAT_ADC_GPIO && timerHardwarePtr->pin == VBAT_ADC_GPIO_PIN) { continue; + } #endif -#ifdef CC3D - if (init->useVbat && timerIndex == Vbat_TIMER) { +#ifdef RSSI_ADC_GPIO + if (init->useRSSIADC && timerHardwarePtr->gpio == RSSI_ADC_GPIO && timerHardwarePtr->pin == RSSI_ADC_GPIO_PIN) { continue; } #endif -#ifdef CC3D - if (init->useCurrentMeterADC && timerIndex == CurrentMeter_TIMER) { - continue; - } -#endif -#ifdef CC3D - if (init->useRSSIADC && timerIndex == RSSI_TIMER) { + +#ifdef CURRENT_METER_ADC_GPIO + if (init->useCurrentMeterADC && timerHardwarePtr->gpio == CURRENT_METER_ADC_GPIO && timerHardwarePtr->pin == CURRENT_METER_ADC_GPIO_PIN) { continue; } #endif diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 9db2089ae2..65c4b69457 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -59,10 +59,6 @@ #define SOFTSERIAL_1_TIMER_TX_HARDWARE 1 // PWM 2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 2 // PWM 3 -#define CurrentMeter_TIMER 3 // PWM4 -#define Vbat_TIMER 4 // PWM5 -#define RSSI_TIMER 5 // PWM6 - #define USART3_RX_PIN Pin_11 #define USART3_TX_PIN Pin_10 #define USART3_GPIO GPIOB @@ -75,6 +71,19 @@ #define USE_ADC +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_CHANNEL ADC_Channel_0 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + + #define SENSORS_SET (SENSOR_ACC) #define GPS diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 7063f10c6b..ccc9bebaa3 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -100,6 +100,22 @@ #define USE_ADC +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_4 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + +#define EXTERNAL1_ADC_GPIO GPIOA +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index d2f37240f1..ffd179baaa 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -119,6 +119,22 @@ #define USE_ADC +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_4 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + +#define EXTERNAL1_ADC_GPIO GPIOA +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 7dd042e571..34ec32a7dc 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -87,6 +87,23 @@ #define USE_ADC +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_4 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + +#define EXTERNAL1_ADC_GPIO GPIOA +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 + + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index ddd82cf072..d371f69727 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -97,6 +97,22 @@ #define USE_ADC +#define CURRENT_METER_ADC_GPIO GPIOB +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_4 + +#define RSSI_ADC_GPIO GPIOA +#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_CHANNEL ADC_Channel_1 + +#define EXTERNAL1_ADC_GPIO GPIOA +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define LED0 From e69718b49a9821705f70b6d24a90f979fb980c65 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 21 Jan 2015 01:11:55 +0100 Subject: [PATCH 058/182] Added missing Makefile change from 1a2ee0e5b93d9b1e564a229d15b2cbbf59872ebd --- Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Makefile b/Makefile index 10c62d3574..16aa36be38 100644 --- a/Makefile +++ b/Makefile @@ -441,6 +441,7 @@ STM32F3DISCOVERY_COMMON_SRC = \ drivers/accgyro_l3gd20.c \ drivers/accgyro_l3gd20.c \ drivers/accgyro_lsm303dlhc.c \ + drivers/compass_hmc5883l.c \ $(VCP_SRC) STM32F3DISCOVERY_SRC = \ From 5be2b9229d418271bbe3cb896f025e2f90b126b4 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 21 Jan 2015 02:03:26 +0100 Subject: [PATCH 059/182] Updating travis build configuration. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index a456e51de4..c63522aa43 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,7 +3,7 @@ env: - TARGET=CHEBUZZF3 - TARGET=CJMCU - TARGET=EUSTM32F103RC - - TARGET=MASSIVEF3 + - TARGET=SPRACINGF3 - TARGET=NAZE - TARGET=NAZE32PRO - TARGET=OLIMEXINO From 6f7256242d1a375c386c783be244c89830b201ec Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Wed, 21 Jan 2015 18:42:34 +1300 Subject: [PATCH 060/182] Blackbox: Fix GPS timestamp jitter on low logging rates --- src/main/blackbox/blackbox.c | 40 ++++++++++++++++++-------- src/main/blackbox/blackbox_fielddefs.h | 7 ++++- 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 447f791f19..f30e2c4f91 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -150,6 +150,7 @@ typedef struct blackboxGPSFieldDefinition_t { uint8_t isSigned; uint8_t predict; uint8_t encode; + uint8_t condition; // Decide whether this field should appear in the log } blackboxGPSFieldDefinition_t; /** @@ -213,18 +214,19 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = { #ifdef GPS // GPS position/vel frame static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = { - {"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)}, - {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)}, - {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)} + {"time", UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)}, + {"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)} }; // GPS home frame static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = { - {"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)} + {"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)} }; #endif @@ -631,6 +633,9 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_VBAT: return feature(FEATURE_VBAT); + case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME: + return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom; + case FLIGHT_LOG_FIELD_CONDITION_NEVER: return false; default: @@ -772,8 +777,8 @@ static void writeInterframe(void) //No need to store iteration count since its delta is always 1 /* - * Since the difference between the difference between successive times will be nearly zero, use - * second-order differences. + * Since the difference between the difference between successive times will be nearly zero (due to consistent + * looptime spacing), use second-order differences. */ writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); @@ -980,6 +985,17 @@ static void writeGPSFrame() { blackboxWrite('G'); + /* + * If we're logging every frame, then a GPS frame always appears just after a frame with the + * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame. + * + * If we're not logging every frame, we need to store the time of this GPS frame. + */ + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) { + // Predict the time of the last frame in the main log + writeUnsignedVB(currentTime - blackboxHistory[1]->time); + } + writeUnsignedVB(GPS_numSat); writeSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); @@ -1309,14 +1325,14 @@ void handleBlackbox(void) case BLACKBOX_STATE_SEND_GPS_H_HEADERS: //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, blackboxGpsHFields + 1, - ARRAY_LENGTH(blackboxGpsHFields), NULL, NULL)) { + ARRAY_LENGTH(blackboxGpsHFields), &blackboxGpsHFields[0].condition, &blackboxGpsHFields[1].condition)) { blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS); } break; case BLACKBOX_STATE_SEND_GPS_G_HEADERS: //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, blackboxGpsGFields + 1, - ARRAY_LENGTH(blackboxGpsGFields), NULL, NULL)) { + ARRAY_LENGTH(blackboxGpsGFields), &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) { blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); } break; diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 84f0ee1195..edf67998d3 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -37,6 +37,8 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2, + FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME, + FLIGHT_LOG_FIELD_CONDITION_NEVER, FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS, @@ -72,7 +74,10 @@ typedef enum FlightLogFieldPredictor { FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8, //Predict vbatref, the reference ADC level stored in the header - FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9 + FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9, + + //Predict the last time value written in the main stream + FLIGHT_LOG_FIELD_PREDICTOR_LAST_MAIN_FRAME_TIME = 10 } FlightLogFieldPredictor; From a6409a030bf05c3a72bce99b88e923d5b6ba297b Mon Sep 17 00:00:00 2001 From: Chris Penny Date: Wed, 21 Jan 2015 09:54:27 +0000 Subject: [PATCH 061/182] LED on/off changes to ledStripEnabled flag --- src/main/io/ledstrip.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 3555417a93..e96a5eabbf 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -758,8 +758,6 @@ void updateLedStrip(void) return; } - - uint32_t now = micros(); @@ -907,6 +905,7 @@ void ledStripEnable(void) static void ledStripDisable(void) { setStripColor(&hsv_black); + ws2811UpdateStrip(); } #endif From 69014e68b68f685c12baf0e65d9831ecd6c23282 Mon Sep 17 00:00:00 2001 From: Chris Penny Date: Wed, 21 Jan 2015 10:22:54 +0000 Subject: [PATCH 062/182] minor documentation update --- docs/LedStrip.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 86c2076dc1..08b441a29e 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -12,6 +12,7 @@ supports the following: * Heading/Orientation lights. * Flight mode specific color schemes. * Low battery warning. +* AUX operated on/off switch The function and orientation configuration is fixed for now but later it should be able to be set via the UI or CLI.. From 0fb598d9b03f253fb64193028b07214c94f2a414 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Wed, 21 Jan 2015 20:31:24 +0100 Subject: [PATCH 063/182] blackbox - add amperageLatest logging Raw ADC value is logged, currentMeter configuration is stored in header(as H currentMeter:,) vbatLatest refactored to vbatLatestADC --- src/main/blackbox/blackbox.c | 24 +++++++++++++++++++++--- src/main/blackbox/blackbox.h | 1 + src/main/blackbox/blackbox_fielddefs.h | 1 + src/main/sensors/battery.c | 7 ++++--- src/main/sensors/battery.h | 3 ++- 5 files changed, 29 insertions(+), 7 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f30e2c4f91..e0c82c4795 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -182,6 +182,7 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = { {"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"vbatLatest", UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT}, + {"amperageLatest",UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE}, #ifdef MAG {"magADC[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG}, {"magADC[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG}, @@ -633,6 +634,9 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_VBAT: return feature(FEATURE_VBAT); + case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE: + return feature(FEATURE_CURRENT_METER); + case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME: return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom; @@ -726,6 +730,11 @@ static void writeIntraframe(void) writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF); } + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) { + // 12bit value directly from ADC + writeUnsignedVB(blackboxCurrent->amperageLatest); + } + #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { for (x = 0; x < XYZ_AXIS_COUNT; x++) @@ -811,13 +820,17 @@ static void writeInterframe(void) writeTag8_4S16(deltas); - //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, MAG, BARO + //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO int optionalFieldCount = 0; if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest; } + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) { + deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->amperageLatest - blackboxLast->amperageLatest; + } + #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { for (x = 0; x < XYZ_AXIS_COUNT; x++) @@ -936,7 +949,7 @@ void startBlackbox(void) blackboxHistory[1] = &blackboxHistoryRing[1]; blackboxHistory[2] = &blackboxHistoryRing[2]; - vbatReference = vbatLatest; + vbatReference = vbatLatestADC; //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it @@ -1038,7 +1051,8 @@ static void loadBlackboxState(void) for (i = 0; i < motorCount; i++) blackboxCurrent->motor[i] = motor[i]; - blackboxCurrent->vbatLatest = vbatLatest; + blackboxCurrent->vbatLatest = vbatLatestADC; + blackboxCurrent->amperageLatest = amperageLatestADC; #ifdef MAG for (i = 0; i < XYZ_AXIS_COUNT; i++) @@ -1225,6 +1239,10 @@ static bool blackboxWriteSysinfo() xmitState.u.serialBudget -= strlen("H vbatref:%u\n"); break; + case 13: + blackboxPrintf("H currentMeter:%i,%i\n", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); + + xmitState.u.serialBudget -= strlen("H currentMeter:%i,%i\n"); default: return true; } diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index b971feec59..59085249e9 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -35,6 +35,7 @@ typedef struct blackboxValues_t { int16_t servo[MAX_SUPPORTED_SERVOS]; uint16_t vbatLatest; + uint16_t amperageLatest; #ifdef BARO int32_t BaroAlt; diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index edf67998d3..26d862268f 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -32,6 +32,7 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_MAG, FLIGHT_LOG_FIELD_CONDITION_BARO, FLIGHT_LOG_FIELD_CONDITION_VBAT, + FLIGHT_LOG_FIELD_CONDITION_AMPERAGE, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 9766d40130..86539cb3ff 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -31,7 +31,8 @@ uint16_t batteryWarningVoltage; uint16_t batteryCriticalVoltage; uint8_t vbat = 0; // battery voltage in 0.1V steps -uint16_t vbatLatest = 0; // most recent unsmoothed raw reading from vbat adc +uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC +uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start @@ -55,7 +56,7 @@ void updateBatteryVoltage(void) uint16_t vbatSampleTotal = 0; // store the battery voltage with some other recent battery voltage readings - vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatest = adcGetChannel(ADC_BATTERY); + vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatestADC = adcGetChannel(ADC_BATTERY); // calculate vbat based on the average of recent readings for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) { @@ -114,7 +115,7 @@ void updateCurrentMeter(int32_t lastUpdateAt) static int64_t mAhdrawnRaw = 0; amperageRaw -= amperageRaw / 8; - amperageRaw += adcGetChannel(ADC_CURRENT); + amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT)); amperage = currentSensorToCentiamps(amperageRaw / 8); mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index f90395f5fc..23f918c7c3 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -42,9 +42,10 @@ typedef enum { } batteryState_e; extern uint8_t vbat; -extern uint16_t vbatLatest; +extern uint16_t vbatLatestADC; extern uint8_t batteryCellCount; extern uint16_t batteryWarningVoltage; +extern uint16_t amperageLatestADC; extern int32_t amperage; extern int32_t mAhDrawn; From 83acbda759a1b397b4d9924c52900178c19f3cbc Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Thu, 22 Jan 2015 12:16:58 +1300 Subject: [PATCH 064/182] PID and tuning documentation --- docs/PID tuning.md | 107 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 docs/PID tuning.md diff --git a/docs/PID tuning.md b/docs/PID tuning.md new file mode 100644 index 0000000000..99c770d782 --- /dev/null +++ b/docs/PID tuning.md @@ -0,0 +1,107 @@ +# PID tuning + +Every aspect of flight dynamics is controlled by the selected "PID controller". This is an algorithm which is +responsible for reacting to your stick inputs and keeping the craft stable in the air by using the gyroscopes and/or +accelerometers (depending on your flight mode). + +The "PIDs" are a set of tuning parameters which control the operation of the PID controller. The optimal PID settings +to use are different on every craft, so if you can't find someone with your exact setup who will share their settings +with you, some trial and error is required to find the best performing PID settings. + +A video on how to recognise and correct different flight problems caused by PID settings is available here: + +https://www.youtube.com/watch?v=YNzqTGEl2xQ + +Basically, the goal of the PID controller is to bring the craft's rotation rate in all three axes to the rate that +you're commanding with your sticks. An error is computed which is the difference between your target rotation rate and +the actual one measured by the gyroscopes, and the controller tries to bring this error to zero. + +The P term controls the strength of the correction that is applied to bring the craft toward the target angle or +rotation rate. If the P term is too low, the craft will be difficult to control as it won't respond quickly enough to +keep itself stable. If it is set too high, the craft will rapidly oscillate/shake as it continually overshoots its +target. + +The I term corrects small, long term errors. If it is set too low, the craft's attitude will slowly drift. If it is +set too high, the craft will oscillate (but with slower oscillations than with P being set too high). + +The D term attempts to increase system stability by monitoring the rate of change in the error. If the error is +changing slowly (so the P and I terms aren't having enough impact on reaching the target) the D term causes an increase +in the correction in order to reach the target sooner. If the error is rapidly converging to zero, the D term causes the +strength of the correction to be backed off in order to avoid overshooting the target. + +## PID controllers + +Cleanflight has three built-in PID controllers which each have different flight behavior. Each controller requires +different PID settings for best performance, so if you tune your craft using one PID controller, those settings will +likely not work well on any of the other controllers. + +You can change between PID controllers by running `set pid_controller = X` on the CLI tab of the Cleanflight +Configurator, where X is the number of the controller you want to use. Please read these notes first before trying one +out! + +### PID controller 0, "MultiWii" (default) + +PID Controller 0 is the default controller in Cleanflight, and Cleanflight's default PID settings are tuned to be +middle-of-the-road settings for this controller. It originates from the old MultiWii PID controller from MultiWii 2.2 +and earlier. + +One of the quirks with this controller is that if you increase the P value for an axis, the maximum rotation rates for +that axis are lowered. Hence you need to crank up the pitch or roll rates if you have higher and higher P values. + +In Horizon and Angle modes, this controller uses both the LEVEL "P" and "I" settings in order to tune the +auto-leveling corrections in a similar way to the way that P and I settings are applied to roll and yaw axes in the acro +flight modes. The LEVEL "D" term is used as a limiter to constrain the maximum correction applied by the LEVEL "P" term. + +### PID controller 1, "Rewrite" + +PID Controller 1 is a newer PID controller that is derived from the one in MultiWii 2.3 and later. It works better from +all accounts, and fixes some inherent problems in the way the old one worked. From reports, tuning is apparently easier +on controller 1, and it tolerates a wider range of PID values well. + +Unlike controller 0, controller 1 allows the user to manipulate PID values to tune reaction and stability without +affecting yaw, roll or pitch rotation rates (which are tuned by the dedicated roll & pitch and yaw rate +settings). + +In Angle mode, this controller uses the LEVEL "P" PID setting to decide how strong the auto-level correction should +be. + +In Horizon mode, this controller uses the LEVEL "I" PID setting to decide how much auto-level correction should be +applied. The default Cleanflight setting for "I" will result in virtually no auto-leveling being applied, so that will +need to be increased in order to perform like PID controller 0. + +The LEVEL "D" setting is not used by this controller. + +### PID controller 2, "Baseflight" + +PID Controller 2 is Lux's new floating point PID controller. Both controller 0 and 1 use integer arithmetic, which was +faster in the days of the slower 8-bit MultiWii controllers, but is less precise. + +This controller has code that attempts to compensate for variations in the looptime, which should mean that the PIDs +don't have to be retuned when the looptime setting changes. + +There were initially some problems with horizon mode, and sluggishness in acro mode, that were recently fixed by +nebbian in v1.6.0. The autotune feature does not work on this controller, so don't try to autotune it. + +Even though PC2 is called "pidBaseflight" in the code, it was never in Baseflight or MultiWii. A better name might have +been pidFloatingPoint, or pidCleanflight. It is the first PID Controller designed for 32-bit processors and not derived +from MultiWii. I believe it was named pidBaseflight because it was to be the first true 32-bit processor native PID +controller, and thus the native Baseflight PC, but Timecop never accepted the code into Baseflight. + +The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which +is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to +Horizon mode. The default is 5.0. + +The strength of the auto-leveling correction applied during Horizon mode is set by the parameter "level_horizon" which +is labeled "LEVEL Integral" in the GUI. The default is 3.0, which makes the Horizon mode apply weaker self-leveling than +the Angle mode. Note: There is currently a bug in the Configurator which shows this parameter divided by 100 (so it +shows as 0.03 rather than 3.0). + +The transition between self-leveling and acro behavior in Horizon mode is controlled by the "sensitivity_horizon" +parameter which is labeled "LEVEL Derivative" in the Cleanflight Configurator GUI. This sets the percentage of your +stick travel that should have self-leveling applied to it, so smaller values cause more of the stick area to fly using +only the gyros. The default is 75% + +For example, at a setting of "100" for "sensitivity_horizon", 100% self-leveling strength will be applied at center +stick, 50% self-leveling will be applied at 50% stick, and no self-leveling will be applied at 100% stick. If +sensitivity is decreased to 75, 100% self-leveling will be applied at center stick, 50% will be applied at 63% +stick, and no self-leveling will be applied at 75% stick and onwards. \ No newline at end of file From c4dbe66249cad495c84430f2b3f0ae3490dd7ccd Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:04:32 -0800 Subject: [PATCH 065/182] Removed unused DEG2RAD macro. --- src/main/common/maths.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 85b08c6410..721a3975c3 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -30,8 +30,6 @@ #define RADX10 (M_PI / 1800.0f) // 0.001745329252f #define RAD (M_PI / 180.0f) -#define DEG2RAD(degrees) (degrees * RAD) - #define min(a, b) ((a) < (b) ? (a) : (b)) #define max(a, b) ((a) > (b) ? (a) : (b)) #define abs(x) ((x) > 0 ? (x) : -(x)) From add0b517ededf4705430207cb1eda14a2a21ea5f Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:13:15 -0800 Subject: [PATCH 066/182] Removed unused macro RADX10. --- src/main/common/maths.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 721a3975c3..0e773cd474 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -27,7 +27,6 @@ #endif #define M_PI 3.14159265358979323846f -#define RADX10 (M_PI / 1800.0f) // 0.001745329252f #define RAD (M_PI / 180.0f) #define min(a, b) ((a) < (b) ? (a) : (b)) From 3e6e8f44c208773b846767ab84793e1e9e1b6160 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:14:31 -0800 Subject: [PATCH 067/182] Switched to using degreesToRadians from DEG2RAD macro. --- src/main/flight/imu.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a9eb8b0de5..abcd2a3410 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -88,7 +88,7 @@ void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *ini void imuInit() { - smallAngle = lrintf(acc_1G * cosf(RAD * imuRuntimeConfig->small_angle)); + smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f; } From 6711c56dc51fc5b727e55fed67fc9edd12a7f6d5 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:20:19 -0800 Subject: [PATCH 068/182] Switched to explicitly using M_PI. --- src/main/common/maths.h | 9 +++------ src/main/drivers/accgyro_spi_mpu6000.c | 2 +- src/main/drivers/accgyro_spi_mpu6500.c | 2 +- src/main/flight/imu.c | 14 +++++++------- src/main/flight/navigation.c | 2 +- 5 files changed, 13 insertions(+), 16 deletions(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 0e773cd474..a4014b730f 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -21,13 +21,10 @@ #define sq(x) ((x)*(x)) #endif -#ifdef M_PI -// M_PI should be float, but previous definition may be double -# undef M_PI -#endif -#define M_PI 3.14159265358979323846f +// Use floating point M_PI instead explicitly. +#define M_PIf 3.14159265358979323846f -#define RAD (M_PI / 180.0f) +#define RAD (M_PIf / 180.0f) #define min(a, b) ((a) < (b) ? (a) : (b)) #define max(a, b) ((a) > (b) ? (a) : (b)) diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 590e6e922a..8795bc54da 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -319,7 +319,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) gyro->read = mpu6000SpiGyroRead; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; - //gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; + //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; delay(100); return true; } diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 123ac08c35..7b3893cc1d 100644 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -128,7 +128,7 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf) // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; - //gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; + //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f; // default lpf is 42Hz if (lpf >= 188) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index abcd2a3410..e989308d29 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -90,17 +90,17 @@ void imuInit() { smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; - gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f; + gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } void calculateThrottleAngleScale(uint16_t throttle_correction_angle) { - throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle); + throttleAngleScale = (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) { - fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf + fc_acc = 0.5f / (M_PIf * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf } void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) @@ -259,7 +259,7 @@ static int16_t calculateHeading(t_fp_vector *vec) float sinePitch = sinf(anglerad[AI_PITCH]); float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll; - float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f; + float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f; head = lrintf(hd); if (head < 0) head += 360; @@ -318,8 +318,8 @@ static void getEstimatedAttitude(void) // Attitude of the estimated vector anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z); anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); - inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI)); - inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); + inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf)); + inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf)); if (sensors(SENSOR_MAG)) { rotateV(&EstM.V, &deltaGyroAngle); @@ -349,5 +349,5 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) int angle = lrintf(acosf(cosZ) * throttleAngleScale); if (angle > 900) angle = 900; - return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))); + return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f))); } diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 9eb5627e79..dd09430562 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -163,7 +163,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param) // Low pass filter cut frequency for derivative calculation // Set to "1 / ( 2 * PI * gps_lpf ) - float pidFilter = (1.0f / (2.0f * M_PI * (float)gpsProfile->gps_lpf)); + float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf)); // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative); From fcdc0af2183ba737da7c85476f95c9b1db086a8f Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:26:28 -0800 Subject: [PATCH 069/182] Clarified comments around vector rotate method. --- src/main/flight/imu.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e989308d29..ea8ac87008 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -158,12 +158,11 @@ void normalizeV(struct fp_vector *src, struct fp_vector *dest) } } -// Rotate Estimated vector(s) with small angle approximation, according to the gyro data +// Rotate a vector *v by the euler angles defined by the 3-vector *delta. void rotateV(struct fp_vector *v, fp_angles_t *delta) { struct fp_vector v_tmp = *v; - // This does a "proper" matrix rotation using gyro deltas without small-angle approximation float mat[3][3]; float cosx, sinx, cosy, siny, cosz, sinz; float coszcosx, sinzcosx, coszsinx, sinzsinx; From d1a1cc3f1305f1602ca7571855d8866a66a59a5b Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:27:46 -0800 Subject: [PATCH 070/182] Added clarifying comment to calculateHeading method. --- src/main/flight/imu.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index ea8ac87008..d3f7a1866b 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -247,7 +247,13 @@ void acc_calc(uint32_t deltaT) accSumCount++; } -// baseflight calculation by Luggi09 originates from arducopter +/* +* Baseflight calculation by Luggi09 originates from arducopter +* ============================================================ +* +* Calculate the heading of the craft (in degrees clockwise from North) +* when given a 3-vector representing the direction of North. +*/ static int16_t calculateHeading(t_fp_vector *vec) { int16_t head; From d6b08f22c1c966c78402f018f6284d2d6560f515 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:28:05 -0800 Subject: [PATCH 071/182] Added clarifying comments to calculateHeading body. --- src/main/flight/imu.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d3f7a1866b..e363a2f97e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -264,8 +264,12 @@ static int16_t calculateHeading(t_fp_vector *vec) float sinePitch = sinf(anglerad[AI_PITCH]); float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll; + //TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero, + // or handle the case in which they are and (atan2f(0, 0) is undefined. float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f; head = lrintf(hd); + + // Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive. if (head < 0) head += 360; From 80b78fd808bb706c790bdcffadb4c5318e266dfd Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:29:45 -0800 Subject: [PATCH 072/182] Fixed flight_imu_test after including maths. --- src/test/Makefile | 1 + src/test/unit/flight_imu_unittest.cc | 10 ---------- 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index 8e376ea51b..5001c57230 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -149,6 +149,7 @@ flight_imu_unittest : \ $(OBJECT_DIR)/flight/imu.o \ $(OBJECT_DIR)/flight/altitudehold.o \ $(OBJECT_DIR)/flight_imu_unittest.o \ + $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/gtest_main.a $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 67425aa63d..44aada0687 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -85,18 +85,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) UNUSED(rollAndPitchTrims); } -int32_t applyDeadband(int32_t, int32_t) { return 0; } - uint32_t micros(void) { return 0; } bool isBaroCalibrationComplete(void) { return true; } void performBaroCalibrationCycle(void) {} int32_t baroCalculateAltitude(void) { return 0; } -int constrain(int amt, int low, int high) -{ - UNUSED(amt); - UNUSED(low); - UNUSED(high); - return 0; -} - } From ab8b5efe2cbe8e2c0fbe81da4e19146e37ff4231 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:36:41 -0800 Subject: [PATCH 073/182] Switched initIMU to consistant name. --- src/main/flight/imu.c | 2 +- src/main/main.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e363a2f97e..52820f71d0 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -86,7 +86,7 @@ void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *ini accDeadband = initialAccDeadband; } -void imuInit() +void initIMU() { smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; diff --git a/src/main/main.c b/src/main/main.c index d2584fdba4..a30a6bfe94 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -104,7 +104,7 @@ void beepcodeInit(failsafe_t *initialFailsafe); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig); -void imuInit(void); +void initIMU(void); void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse); void loop(void); @@ -264,7 +264,7 @@ void init(void) LED0_OFF; LED1_OFF; - imuInit(); + initIMU(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); #ifdef MAG From 919a8796e7ee0fdfbfe1452473d9c34689c58fca Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:37:56 -0800 Subject: [PATCH 074/182] Switched configureIMU to consistant name. --- src/main/config/config.c | 2 +- src/main/flight/imu.c | 2 +- src/main/flight/imu.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index f582a8efd5..17c656ddd9 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -613,7 +613,7 @@ void activateConfig(void) imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; imuRuntimeConfig.small_angle = masterConfig.small_angle; - configureImu( + configureIMU( &imuRuntimeConfig, ¤tProfile->pidProfile, ¤tProfile->accDeadband diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 52820f71d0..59f79ea907 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -79,7 +79,7 @@ imuRuntimeConfig_t *imuRuntimeConfig; pidProfile_t *pidProfile; accDeadband_t *accDeadband; -void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband) +void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband) { imuRuntimeConfig = initialImuRuntimeConfig; pidProfile = initialPidProfile; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index de5ac89f01..552093508d 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,7 +30,7 @@ typedef struct imuRuntimeConfig_s { int8_t small_angle; } imuRuntimeConfig_t; -void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband); +void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband); void calculateEstimatedAltitude(uint32_t currentTime); void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); From 29c56309576379a116528d8f92f42188243e216b Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:42:06 -0800 Subject: [PATCH 075/182] Moved vectors, euler angles, rotation and normalization from imu to maths. --- src/main/common/maths.c | 49 ++++++++++++++++++++++++++++++++++++++++ src/main/common/maths.h | 29 ++++++++++++++++++++++++ src/main/flight/flight.h | 22 ------------------ src/main/flight/imu.c | 49 ---------------------------------------- 4 files changed, 78 insertions(+), 71 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 716d83e491..5ed16f916d 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -92,3 +92,52 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) { return ((a / b) - (destMax - destMin)) + destMax; } +// Normalize a vector +void normalizeV(struct fp_vector *src, struct fp_vector *dest) +{ + float length; + + length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z); + if (length != 0) { + dest->X = src->X / length; + dest->Y = src->Y / length; + dest->Z = src->Z / length; + } +} + +// Rotate a vector *v by the euler angles defined by the 3-vector *delta. +void rotateV(struct fp_vector *v, fp_angles_t *delta) +{ + struct fp_vector v_tmp = *v; + + float mat[3][3]; + float cosx, sinx, cosy, siny, cosz, sinz; + float coszcosx, sinzcosx, coszsinx, sinzsinx; + + cosx = cosf(delta->angles.roll); + sinx = sinf(delta->angles.roll); + cosy = cosf(delta->angles.pitch); + siny = sinf(delta->angles.pitch); + cosz = cosf(delta->angles.yaw); + sinz = sinf(delta->angles.yaw); + + coszcosx = cosz * cosx; + sinzcosx = sinz * cosx; + coszsinx = sinx * cosz; + sinzsinx = sinx * sinz; + + mat[0][0] = cosz * cosy; + mat[0][1] = -cosy * sinz; + mat[0][2] = siny; + mat[1][0] = sinzcosx + (coszsinx * siny); + mat[1][1] = coszcosx - (sinzsinx * siny); + mat[1][2] = -sinx * cosy; + mat[2][0] = (sinzsinx) - (coszcosx * siny); + mat[2][1] = (coszsinx) + (sinzcosx * siny); + mat[2][2] = cosy * cosx; + + v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0]; + v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1]; + v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; +} + diff --git a/src/main/common/maths.h b/src/main/common/maths.h index a4014b730f..d65b87df22 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -36,6 +36,31 @@ typedef struct stdev_t int m_n; } stdev_t; +// Floating point 3 vector. +typedef struct fp_vector { + float X; + float Y; + float Z; +} t_fp_vector_def; + +typedef union { + float A[3]; + t_fp_vector_def V; +} t_fp_vector; + +// Floating point Euler angles. +// Be carefull, could be either of degrees or radians. +typedef struct fp_angles { + float roll; + float pitch; + float yaw; +} fp_angles_def; + +typedef union { + float raw[3]; + fp_angles_def angles; +} fp_angles_t; + int32_t applyDeadband(int32_t value, int32_t deadband); int constrain(int amt, int low, int high); @@ -48,3 +73,7 @@ float devStandardDeviation(stdev_t *dev); float degreesToRadians(int16_t degrees); int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax); + +void normalizeV(struct fp_vector *src, struct fp_vector *dest); + +void rotateV(struct fp_vector *v, fp_angles_t *delta); diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index ed96b43b1b..95877badf2 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -61,28 +61,6 @@ typedef enum { #define FLIGHT_DYNAMICS_INDEX_COUNT 3 -typedef struct fp_vector { - float X; - float Y; - float Z; -} t_fp_vector_def; - -typedef union { - float A[3]; - t_fp_vector_def V; -} t_fp_vector; - -typedef struct fp_angles { - float roll; - float pitch; - float yaw; -} fp_angles_def; - -typedef union { - float raw[3]; - fp_angles_def angles; -} fp_angles_t; - typedef struct int16_flightDynamicsTrims_s { int16_t roll; int16_t pitch; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 59f79ea907..cff9b315ce 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -145,55 +145,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) t_fp_vector EstG; -// Normalize a vector -void normalizeV(struct fp_vector *src, struct fp_vector *dest) -{ - float length; - - length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z); - if (length != 0) { - dest->X = src->X / length; - dest->Y = src->Y / length; - dest->Z = src->Z / length; - } -} - -// Rotate a vector *v by the euler angles defined by the 3-vector *delta. -void rotateV(struct fp_vector *v, fp_angles_t *delta) -{ - struct fp_vector v_tmp = *v; - - float mat[3][3]; - float cosx, sinx, cosy, siny, cosz, sinz; - float coszcosx, sinzcosx, coszsinx, sinzsinx; - - cosx = cosf(delta->angles.roll); - sinx = sinf(delta->angles.roll); - cosy = cosf(delta->angles.pitch); - siny = sinf(delta->angles.pitch); - cosz = cosf(delta->angles.yaw); - sinz = sinf(delta->angles.yaw); - - coszcosx = cosz * cosx; - sinzcosx = sinz * cosx; - coszsinx = sinx * cosz; - sinzsinx = sinx * sinz; - - mat[0][0] = cosz * cosy; - mat[0][1] = -cosy * sinz; - mat[0][2] = siny; - mat[1][0] = sinzcosx + (coszsinx * siny); - mat[1][1] = coszcosx - (sinzsinx * siny); - mat[1][2] = -sinx * cosy; - mat[2][0] = (sinzsinx) - (coszcosx * siny); - mat[2][1] = (coszsinx) + (sinzcosx * siny); - mat[2][2] = cosy * cosx; - - v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0]; - v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1]; - v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; -} - void accSum_reset(void) { accSum[0] = 0; From 616c40a827c73fbaf46b929309f67cdf0e39a83c Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Wed, 21 Jan 2015 19:44:01 -0800 Subject: [PATCH 076/182] Added clarifying comment and todo. --- src/main/flight/imu.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index cff9b315ce..f0a4535a00 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -298,12 +298,17 @@ static void getEstimatedAttitude(void) acc_calc(deltaT); // rotate acc vector into earth frame } -// correction of throttle in lateral wind, +// Correction of throttle in lateral wind. int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) { float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); - if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg + /* + * Use 0 as the throttle angle correction if we are inverted, vertical or with a + * small angle < 0.86 deg + * TODO: Define this small angle in config. + */ + if (cosZ <= 0.015f) { return 0; } int angle = lrintf(acosf(cosZ) * throttleAngleScale); From 0a8d5c93902796d4c13aa9aadd51c116b4654975 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 17:43:13 +0100 Subject: [PATCH 077/182] Update CONTRIBUTING.md --- CONTRIBUTING.md | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 2ff00122e4..797e35a842 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,4 +1,14 @@ -# Contributing +# Issues and Support. + +Please remember the issue tracker on github is _not_ for user support. Please also do not email developers directly for support. Instead please use IRC or the forums first, then if the problem is confirmed create an issue that details how to repeat the problem so it can be investigated. + +Issued created without steps to repeat are likely to be closed. E-mail requests for support will go un-answered. + +Remember that issues that are due to mis-configuration, wiring or failure to read documentation just takes time away from the developers and can often be solved without developer interaction by other users. + +Please search for existing issues *before* creating new ones. + +# Developers Please see the Contributing section of the README.md From 71a61d2432850684f8a42733378814cd1907f295 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 17:59:06 +0100 Subject: [PATCH 078/182] SPRACINGF3 - Use 128k flash. --- Makefile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 16aa36be38..f6776aaefe 100644 --- a/Makefile +++ b/Makefile @@ -481,9 +481,9 @@ SPRACINGF3_SRC = \ $(HIGHEND_SRC) \ $(COMMON_SRC) -#ifeq ($(TARGET),SPRACINGF3) -#LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_128k.ld -#endif +ifeq ($(TARGET),SPRACINGF3) +LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_128k.ld +endif # Search path and source files for the ST stdperiph library VPATH := $(VPATH):$(STDPERIPH_DIR)/src From ce0a93a5d86015efaa57fe3d88a0867a10085f55 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 18:06:09 +0100 Subject: [PATCH 079/182] Updating the git workflow notes (stronnag) --- CONTRIBUTING.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 797e35a842..532e8d4d01 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -25,7 +25,7 @@ https://help.github.com/articles/creating-a-pull-request/ The main flow for a contributing is as follows: 1. Login to github, goto the cleanflight repository and press `fork`. -2. `git clone ` +2. Then using the command line/terminal on your computer: `git clone ` 3. `cd cleanflight` 4. `git checkout master` 5. `git checkout -b my-new-code` @@ -45,3 +45,6 @@ Later, you can get the changes from the cleanflight repo into your `master` bran 3. `git fetch cleanflight` 4. `git merge cleanflight/master` + +You can also perform the git commands using the git client inside Eclipse. Refer to the Eclipse git manual. + \ No newline at end of file From 9e73dca5918786ca9b2e584819a543a8d57333de Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 19:19:23 +0100 Subject: [PATCH 080/182] CC3D - Use Flex port in I2C mode unless USART3 is used. Add I2C drivers for compass, baro and display. --- Makefile | 5 +++++ docs/Board - CC3D.md | 16 ++++++++++++++++ src/main/main.c | 7 +++++-- src/main/target/CC3D/target.h | 13 +++++++++++++ 4 files changed, 39 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index f6776aaefe..c4197a7729 100644 --- a/Makefile +++ b/Makefile @@ -381,7 +381,12 @@ CC3D_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/adc.c \ drivers/adc_stm32f10x.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_ms5611.c \ drivers/bus_spi.c \ + drivers/bus_i2c_stm32f10x.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.c \ drivers/gpio_stm32f10x.c \ drivers/inverter.c \ drivers/light_led_stm32f10x.c \ diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md index c1194ba5ec..580bd4c353 100644 --- a/docs/Board - CC3D.md +++ b/docs/Board - CC3D.md @@ -79,6 +79,22 @@ The SoftSerial port is not available when RX_PARALLEL_PWM is used. The transmiss To connect the GUI to the flight controller you need additional hardware attached to the USART1 serial port (by default). +# Flex Port + +The flex port will be enabled in I2C mode unless USART3 is used. You can connect external I2C sensors and displays to this port. + +You cannot use USART3 and I2C at the same time. + +## Flex port pinout + +| Pin | Signal | Notes | +| --- | ------------------ | ----------------------- | +| 1 | GND | | +| 2 | VCC unregulated | | +| 3 | I2C SCL / UART3 TX | 3.3v level | +| 4 | I2C SDA / UART3 RX | 3.3v level (5v tolerant | + + # Flashing There are two primary ways to get Cleanflight onto a CC3D board. diff --git a/src/main/main.c b/src/main/main.c index a30a6bfe94..08c1e64694 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -206,12 +206,15 @@ void init(void) #endif #ifdef USE_I2C -#ifdef NAZE +#if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } +#elif defined(CC3D) + if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { + i2cInit(I2C_DEVICE); + } #else - // Configure the rest of the stuff i2cInit(I2C_DEVICE); #endif #endif diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 65c4b69457..26a1358c61 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -47,8 +47,18 @@ #define ACC_SPI_MPU6000_ALIGN CW270_DEG +// External I2C BARO +#define BARO +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 + +// External I2C MAG +#define MAG +#define USE_MAG_HMC5883 + #define INVERTER #define BEEPER +#define DISPLAY #define USE_USART1 #define USE_USART3 @@ -69,6 +79,9 @@ #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 + #define USE_ADC #define CURRENT_METER_ADC_GPIO GPIOB From 9eff8f1932a30cc561f580399587dafb75dfc76c Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 19:26:36 +0100 Subject: [PATCH 081/182] Adding CC3D OpenPilot bootloader build to travis. --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index c63522aa43..a4457c2c02 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,5 +1,6 @@ env: - TARGET=CC3D + - TARGET=CC3D OPBL=yes - TARGET=CHEBUZZF3 - TARGET=CJMCU - TARGET=EUSTM32F103RC From e58f2948f3d95764203cc5a52c716a183c9754a8 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 19:39:36 +0100 Subject: [PATCH 082/182] STM32F3DISCOVERY - Updating L3GD20 alignment defaults. (MJ666) --- src/main/sensors/initialisation.c | 4 ++-- src/main/target/CHEBUZZF3/target.h | 1 + src/main/target/STM32F3DISCOVERY/target.h | 2 ++ 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index f4cbe90e5f..099e6fe000 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -173,8 +173,8 @@ bool detectGyro(uint16_t gyroLpf) #ifdef USE_GYRO_L3GD20 if (l3gd20Detect(&gyro, gyroLpf)) { -#ifdef GYRO_GYRO_L3GD20_ALIGN - gyroAlign = GYRO_GYRO_L3GD20_ALIGN; +#ifdef GYRO_L3GD20_ALIGN + gyroAlign = GYRO_L3GD20_ALIGN; #endif return true; } diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 78bf6af63a..cc699343cf 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -39,6 +39,7 @@ #define USE_GYRO_L3GD20 #define USE_GYRO_MPU6050 +#define GYRO_L3GD20_ALIGN CW90_DEG #define GYRO_MPU6050_ALIGN CW0_DEG #define ACC diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index ce8e386554..ab840213fb 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -39,6 +39,8 @@ #define GYRO #define USE_GYRO_L3GD20 +#define GYRO_L3GD20_ALIGN CW90_DEG + #define ACC #define USE_ACC_LSM303DLHC From a2628c59d5d70aa57657e375309175c85ff5ef3a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 21:36:05 +0100 Subject: [PATCH 083/182] Cleanup whitespace. --- src/main/io/ledstrip.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index e96a5eabbf..0f66cc02c3 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -48,10 +48,10 @@ #include "io/ledstrip.h" static bool ledStripInitialised = false; +static bool ledStripEnabled = true; static failsafe_t* failsafe; -static bool ledStripEnabled = true; static void ledStripDisable(void); //#define USE_LED_ANIMATION @@ -745,12 +745,12 @@ void updateLedStrip(void) return; } - if ( IS_RC_MODE_ACTIVE(BOXLEDLOW)){ - if (ledStripEnabled){ - ledStripDisable(); + if (IS_RC_MODE_ACTIVE(BOXLEDLOW)) { + if (ledStripEnabled) { + ledStripDisable(); ledStripEnabled = false; } - }else{ + } else { ledStripEnabled = true; } From d2536e379229ed17e2f1d8257945ee236d543fc2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 21:38:09 +0100 Subject: [PATCH 084/182] Ensure LED configuration is re-evaluated after changes to led configuration are made via MSP. --- src/main/io/ledstrip.c | 2 +- src/main/io/ledstrip.h | 2 +- src/main/io/serial_msp.c | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 0f66cc02c3..5389bbfd77 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -337,7 +337,7 @@ void updateLedCount(void) } } -static void reevalulateLedConfig(void) +void reevalulateLedConfig(void) { updateLedCount(); determineLedStripDimensions(); diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 4e434b4003..498504a315 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -73,5 +73,5 @@ bool parseColor(uint8_t index, const char *colorConfig); void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); void ledStripEnable(void); - +void reevalulateLedConfig(void); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5f2064f92f..20ee25d798 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1469,6 +1469,8 @@ static bool processInCommand(void) mask = read8(); ledConfig->xy |= CALCULATE_LED_Y(mask); + + reevalulateLedConfig(); } break; #endif From ddc7a39fa24520cad1c6bbb3a84673b35e5fdf96 Mon Sep 17 00:00:00 2001 From: St7ven Date: Thu, 1 Jan 2015 16:04:01 +0100 Subject: [PATCH 085/182] Add support for LED thrust ring. (St7ven) This commit includes various fixed not included in the original pull request. --- docs/LedStrip.md | 19 +++++ src/main/io/ledstrip.c | 185 ++++++++++++++++++++++++++++++++++------- src/main/io/ledstrip.h | 10 ++- 3 files changed, 179 insertions(+), 35 deletions(-) diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 08b441a29e..4305c99075 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -111,6 +111,7 @@ Note: It is perfectly possible to configure an LED to have all directions `NESWU * `I` - `I`ndicator. * `A` - `A`rmed state. * `T` - `T`hrust state. +* `R` - `R`ing thrust state. Example: @@ -171,6 +172,24 @@ This mode fades the LED current LED color to the previous/next color in the HSB throttle is in the middle position the color is unaffected, thus it can be mixed with orientation colors to indicate orientation and throttle at the same time. +#### Thrust ring state + +This mode is allows you to use a 12, 16 or 24 leds ring (e.g. NeoPixel ring) for an afterburner effect. When armed the leds use the following sequences: 2 On, 4 Off, 2 On, 4 Off, and so on. The light pattern rotates clockwise as throttle increases. + +A better effect is acheived when LEDs configured for thrust ring have no other functions. + +LED direction and X/Y positions are irrelevant for thrust ring LED state. The order of the LEDs that have the state determines how the LED behaves. + +Each LED of the ring can be a different color. The color can be selected between the 15 colors availables. + +For example, led 0 is set as a `R`ing thrust state led in color 13 as follow. + +``` +led 0 2,2::R:13 +``` + +LED strips and rings can be combined. + ## Positioning Cut the strip into sections as per diagrams below. When the strips are cut ensure you reconnect each output to each input with cable where the break is made. diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 5389bbfd77..cece9d3c61 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -55,6 +55,7 @@ static failsafe_t* failsafe; static void ledStripDisable(void); //#define USE_LED_ANIMATION +//#define USE_LED_RING_DEFAULT_CONFIG // timers #ifdef USE_LED_ANIMATION @@ -63,6 +64,7 @@ static uint32_t nextAnimationUpdateAt = 0; static uint32_t nextIndicatorFlashAt = 0; static uint32_t nextWarningFlashAt = 0; +static uint32_t nextRotationUpdateAt = 0; #define LED_STRIP_20HZ ((1000 * 1000) / 20) #define LED_STRIP_10HZ ((1000 * 1000) / 10) @@ -225,23 +227,44 @@ static const modeColorIndexes_t baroModeColors = { uint8_t ledGridWidth; uint8_t ledGridHeight; uint8_t ledCount; +uint8_t ledsInRingCount; ledConfig_t *ledConfigs; +hsvColor_t *colors; + +#ifdef USE_LED_RING_DEFAULT_CONFIG const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 2, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 2, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 2, 0), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 0), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 0, 0), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 0, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 0, 2), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 2), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, }; +#else +const ledConfig_t defaultLedStripConfig[] = { + { CALCULATE_LED_XY( 2, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 2), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, +}; +#endif + /* @@ -258,12 +281,13 @@ typedef enum { X_COORDINATE, Y_COORDINATE, DIRECTIONS, - FUNCTIONS + FUNCTIONS, + RING_COLORS } parseState_e; -#define PARSE_STATE_COUNT 4 +#define PARSE_STATE_COUNT 5 -static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', '\0' }; +static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0' }; static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' }; #define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0])) @@ -276,14 +300,15 @@ static const uint8_t directionMappings[DIRECTION_COUNT] = { LED_DIRECTION_DOWN }; -static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T' }; +static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R' }; #define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) static const uint16_t functionMappings[FUNCTION_COUNT] = { LED_FUNCTION_INDICATOR, LED_FUNCTION_WARNING, LED_FUNCTION_FLIGHT_MODE, LED_FUNCTION_ARM_STATE, - LED_FUNCTION_THROTTLE + LED_FUNCTION_THROTTLE, + LED_FUNCTION_THRUST_RING }; // grid offsets @@ -327,13 +352,24 @@ void determineOrientationLimits(void) void updateLedCount(void) { + const ledConfig_t *ledConfig; uint8_t ledIndex; ledCount = 0; + ledsInRingCount = 0; + for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) { - if (ledConfigs[ledIndex].flags == 0 && ledConfigs[ledIndex].xy == 0) { + + ledConfig = &ledConfigs[ledIndex]; + + if (ledConfig->flags == 0 && ledConfig->xy == 0) { break; } + ledCount++; + + if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { + ledsInRingCount++; + } } } @@ -410,6 +446,15 @@ bool parseLedStripConfig(uint8_t ledIndex, const char *config) } } break; + case RING_COLORS: + if (atoi(chunk) < CONFIGURABLE_COLOR_COUNT) { + ledConfig->color = atoi(chunk); + } else { + ledConfig->color = 0; + } + break; + default : + break; } parseState++; @@ -433,11 +478,13 @@ void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSiz char directions[DIRECTION_COUNT]; uint8_t index; uint8_t mappingIndex; + ledConfig_t *ledConfig = &ledConfigs[ledIndex]; memset(ledConfigBuffer, 0, bufferSize); memset(&functions, 0, sizeof(functions)); memset(&directions, 0, sizeof(directions)); + //memset(&ringColors, 0 ,sizeof(ringColors)); for (mappingIndex = 0, index = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { if (ledConfig->flags & functionMappings[mappingIndex]) { @@ -451,7 +498,7 @@ void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSiz } } - sprintf(ledConfigBuffer, "%u,%u:%s:%s", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions); + sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions, ledConfig->color); } void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors) @@ -529,7 +576,9 @@ void applyLedModeLayer(void) ledConfig = &ledConfigs[ledIndex]; - setLedHsv(ledIndex, &hsv_black); + if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) { + setLedHsv(ledIndex, &hsv_black); + } if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { if (ledConfig->flags & LED_FUNCTION_ARM_STATE) { @@ -682,7 +731,7 @@ void applyLedThrottleLayer() uint8_t ledIndex; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; - if(!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { + if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { continue; } @@ -695,15 +744,77 @@ void applyLedThrottleLayer() } } -#ifdef USE_LED_ANIMATION -static uint8_t frameCounter = 0; - -static uint8_t previousRow; -static uint8_t currentRow; -static uint8_t nextRow; - -static void updateLedAnimationState(void) +int applyLedThrustRingLayer(void) { + uint8_t oppositeLedIndex = ledsInRingCount >> 1; + uint8_t ledIndex; + int returnedValue = 1; + int throttleScaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); + static uint8_t rotationPhase = 0; + static bool nextLedOn = false; + hsvColor_t ringColor; + const ledConfig_t *ledConfig; + + + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + + ledConfig = &ledConfigs[ledIndex]; + + ringColor = colors[ledConfig->color]; + + if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { + + if (!ARMING_FLAG(ARMED)) { + + if (nextLedOn == false) { + nextLedOn = true; + } + else { + nextLedOn = false; + ringColor = hsv_black; + } + + returnedValue = 1; + } + else { + if (rotationPhase == ((oppositeLedIndex) - 1)) { + if (!((ledIndex == rotationPhase) || (ledIndex == (rotationPhase + oppositeLedIndex)) + || (ledIndex == (rotationPhase +1)) || (ledIndex == 0))) { + ringColor = hsv_black; + } + } + else if (!((ledIndex == rotationPhase) || (ledIndex == (rotationPhase + oppositeLedIndex)) + || (ledIndex == (rotationPhase +1)) || (ledIndex == (rotationPhase + oppositeLedIndex +1 )))) { + ringColor = hsv_black; + } + else { + // Led stay on + } + + returnedValue = throttleScaled; + } + setLedHsv(ledIndex, &ringColor); + } + } + + if (rotationPhase >= (oppositeLedIndex - 1)) { + rotationPhase = 0; + } + else { + rotationPhase++; + } + + return returnedValue; +} + +#ifdef USE_LED_ANIMATION +void updateLedAnimationState(void) +{ + static uint8_t frameCounter = 0; + + static uint8_t previousRow; + static uint8_t currentRow; + static uint8_t nextRow; uint8_t animationFrames = ledGridHeight; previousRow = (frameCounter + animationFrames - 1) % animationFrames; @@ -741,7 +852,8 @@ static void applyLedAnimationLayer(void) void updateLedStrip(void) { - if (!(ledStripInitialised && isWS2811LedStripReady())) { + + if (!(ledStripInitialised && isWS2811LedStripReady())) { return; } @@ -762,12 +874,14 @@ void updateLedStrip(void) uint32_t now = micros(); bool indicatorFlashNow = indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool warningFlashNow =warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; + bool warningFlashNow = warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; + bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L; #ifdef USE_LED_ANIMATION bool animationUpdateNow = animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; #endif if (!( indicatorFlashNow || + rotationUpdateNow || warningFlashNow #ifdef USE_LED_ANIMATION || animationUpdateNow @@ -812,10 +926,16 @@ void updateLedStrip(void) nextAnimationUpdateAt = now + LED_STRIP_20HZ; updateLedAnimationState(); } - applyLedAnimationLayer(); #endif + if (rotationUpdateNow) { + + int animationSpeedScaled = applyLedThrustRingLayer(); + + nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScaled; // TODO will be changed with more specifics animation + } + ws2811UpdateStrip(); } @@ -835,6 +955,7 @@ bool parseColor(uint8_t index, const char *colorConfig) if (val > HSV_HUE_MAX) { ok = false; continue; + } colors[index].h = val; break; diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 498504a315..b6d8cf20f7 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -18,6 +18,7 @@ #pragma once #define MAX_LED_STRIP_LENGTH 32 +#define CONFIGURABLE_COLOR_COUNT 16 #define LED_X_BIT_OFFSET 4 #define LED_Y_BIT_OFFSET 0 @@ -30,6 +31,7 @@ #define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET) #define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET) + #define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y)) typedef enum { @@ -44,7 +46,8 @@ typedef enum { LED_FUNCTION_WARNING = (1 << 7), LED_FUNCTION_FLIGHT_MODE = (1 << 8), LED_FUNCTION_ARM_STATE = (1 << 9), - LED_FUNCTION_THROTTLE = (1 << 10) + LED_FUNCTION_THROTTLE = (1 << 10), + LED_FUNCTION_THRUST_RING = (1 << 11), } ledFlag_e; #define LED_DIRECTION_BIT_OFFSET 0 @@ -54,17 +57,18 @@ typedef enum { typedef struct ledConfig_s { - uint8_t xy; // see LED_X/Y_MASK defines + uint8_t xy; // see LED_X/Y_MASK defines + uint8_t color; // see colors (config_master) uint16_t flags; // see ledFlag_e } ledConfig_t; extern uint8_t ledCount; -#define CONFIGURABLE_COLOR_COUNT 16 bool parseLedStripConfig(uint8_t ledIndex, const char *config); void updateLedStrip(void); +void updateLedRing(void); void applyDefaultLedStripConfig(ledConfig_t *ledConfig); void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); From e7302a9e10081c028fab666aa6c9c70a03f19057 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 22:24:58 +0100 Subject: [PATCH 086/182] Update MSP to allow setting of LED colors. --- src/main/io/serial_msp.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 20ee25d798..36a86f81e3 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -121,7 +121,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 3 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 4 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -1104,13 +1104,14 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LED_STRIP_CONFIG: - headSerialReply(MAX_LED_STRIP_LENGTH * 6); + headSerialReply(MAX_LED_STRIP_LENGTH * 7); for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); serialize8(GET_LED_X(ledConfig)); serialize8(GET_LED_Y(ledConfig)); + serialize8(ledConfig->color); } break; #endif @@ -1450,7 +1451,7 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != 7) { + if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != 8) { headSerialError(0); break; } @@ -1470,6 +1471,8 @@ static bool processInCommand(void) mask = read8(); ledConfig->xy |= CALCULATE_LED_Y(mask); + ledConfig->color = read8(); + reevalulateLedConfig(); } break; From ae752217c67f1af90c8a00a5353f27d264590a95 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 22:56:38 +0100 Subject: [PATCH 087/182] Avoid static use. Causes problems when the amount of LEDs in the ring is odd. --- src/main/io/ledstrip.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index cece9d3c61..be6914948c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -751,7 +751,7 @@ int applyLedThrustRingLayer(void) int returnedValue = 1; int throttleScaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); static uint8_t rotationPhase = 0; - static bool nextLedOn = false; + bool nextLedOn = false; hsvColor_t ringColor; const ledConfig_t *ledConfig; From 8bdca1b38eaea09b59bc7f34525f9c5c0c816e66 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 22:57:34 +0100 Subject: [PATCH 088/182] Improve magic number use. --- src/main/io/serial_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 36a86f81e3..c70e19fe87 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1451,7 +1451,7 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != 8) { + if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) { headSerialError(0); break; } From 5a025b71647ac50c6bf585705ecc3a02192bbd3f Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 22:58:08 +0100 Subject: [PATCH 089/182] Bump config version for LED color storage. --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 17c656ddd9..d8659bd83e 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -108,7 +108,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 88; +static const uint8_t EEPROM_CONF_VERSION = 89; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { From b50c5d97246d5d05ff56707126351ce7da54f19a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 22 Jan 2015 23:46:23 +0100 Subject: [PATCH 090/182] Simplify and cleanup led ring code. This changes the behaviour, but since the effect is nice we'll mark it with a commit. --- src/main/io/ledstrip.c | 82 +++++++++++++++++++----------------------- 1 file changed, 37 insertions(+), 45 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index be6914948c..6d9aa4346e 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -744,67 +744,53 @@ void applyLedThrottleLayer() } } -int applyLedThrustRingLayer(void) +void applyLedThrustRingLayer(void) { - uint8_t oppositeLedIndex = ledsInRingCount >> 1; uint8_t ledIndex; - int returnedValue = 1; - int throttleScaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); static uint8_t rotationPhase = 0; bool nextLedOn = false; hsvColor_t ringColor; const ledConfig_t *ledConfig; + uint8_t phaseCount = ledsInRingCount / 2; + uint8_t ledRingIndex = 0; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; - ringColor = colors[ledConfig->color]; - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - - if (!ARMING_FLAG(ARMED)) { - - if (nextLedOn == false) { - nextLedOn = true; - } - else { - nextLedOn = false; - ringColor = hsv_black; - } - - returnedValue = 1; - } - else { - if (rotationPhase == ((oppositeLedIndex) - 1)) { - if (!((ledIndex == rotationPhase) || (ledIndex == (rotationPhase + oppositeLedIndex)) - || (ledIndex == (rotationPhase +1)) || (ledIndex == 0))) { - ringColor = hsv_black; - } - } - else if (!((ledIndex == rotationPhase) || (ledIndex == (rotationPhase + oppositeLedIndex)) - || (ledIndex == (rotationPhase +1)) || (ledIndex == (rotationPhase + oppositeLedIndex +1 )))) { - ringColor = hsv_black; - } - else { - // Led stay on - } - - returnedValue = throttleScaled; - } - setLedHsv(ledIndex, &ringColor); + if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) { + continue; } + + bool applyColor = false; + if (ARMING_FLAG(ARMED)) { + if (ledRingIndex == rotationPhase || ledIndex == rotationPhase + phaseCount) { + applyColor = true; + } + } else { + if (nextLedOn == false) { + applyColor = true; + } + nextLedOn = !nextLedOn; + } + + if (applyColor) { + ringColor = colors[ledConfig->color]; + } else { + ringColor = hsv_black; + } + + setLedHsv(ledIndex, &ringColor); + + ledRingIndex++; } - if (rotationPhase >= (oppositeLedIndex - 1)) { + rotationPhase++; + if (rotationPhase == phaseCount) { rotationPhase = 0; } - else { - rotationPhase++; - } - - return returnedValue; } #ifdef USE_LED_ANIMATION @@ -931,9 +917,15 @@ void updateLedStrip(void) if (rotationUpdateNow) { - int animationSpeedScaled = applyLedThrustRingLayer(); + applyLedThrustRingLayer(); - nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScaled; // TODO will be changed with more specifics animation + uint8_t animationSpeedScale = 1; + + if (ARMING_FLAG(ARMED)) { + animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); + } + + nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale; } ws2811UpdateStrip(); From 9fb1863d746f0f8eeb0909454bbe88e03af18d93 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 23 Jan 2015 00:06:20 +0100 Subject: [PATCH 091/182] Update LED ring code to have the documented behavior. The logic is greatly simplified compared to the code in the original pull request. --- src/main/io/ledstrip.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 6d9aa4346e..67d8e21634 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -744,29 +744,29 @@ void applyLedThrottleLayer() } } +#define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off +#define ROTATION_SEQUENCE_LED_WIDTH 2 + void applyLedThrustRingLayer(void) { uint8_t ledIndex; - static uint8_t rotationPhase = 0; + static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; bool nextLedOn = false; hsvColor_t ringColor; const ledConfig_t *ledConfig; - uint8_t phaseCount = ledsInRingCount / 2; - uint8_t ledRingIndex = 0; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) { continue; } bool applyColor = false; if (ARMING_FLAG(ARMED)) { - if (ledRingIndex == rotationPhase || ledIndex == rotationPhase + phaseCount) { + if ((ledRingIndex + rotationPhase) % ROTATION_SEQUENCE_LED_COUNT < ROTATION_SEQUENCE_LED_WIDTH) { applyColor = true; } } else { @@ -787,9 +787,9 @@ void applyLedThrustRingLayer(void) ledRingIndex++; } - rotationPhase++; - if (rotationPhase == phaseCount) { - rotationPhase = 0; + rotationPhase--; + if (rotationPhase == 0) { + rotationPhase = ROTATION_SEQUENCE_LED_COUNT; } } From 839b8408eaa29543bad94535d75bcf44fb325723 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 23 Jan 2015 01:16:54 +0100 Subject: [PATCH 092/182] Fix LED strip mask for LED ring function. --- src/main/io/ledstrip.h | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index b6d8cf20f7..c62dd4c4ce 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -51,9 +51,23 @@ typedef enum { } ledFlag_e; #define LED_DIRECTION_BIT_OFFSET 0 -#define LED_DIRECTION_MASK 0x3F +#define LED_DIRECTION_MASK ( \ + LED_DIRECTION_NORTH | \ + LED_DIRECTION_EAST | \ + LED_DIRECTION_SOUTH | \ + LED_DIRECTION_WEST | \ + LED_DIRECTION_UP | \ + LED_DIRECTION_DOWN \ +) #define LED_FUNCTION_BIT_OFFSET 6 -#define LED_FUNCTION_MASK 0x7C0 +#define LED_FUNCTION_MASK ( \ + LED_FUNCTION_INDICATOR | \ + LED_FUNCTION_WARNING | \ + LED_FUNCTION_FLIGHT_MODE | \ + LED_FUNCTION_ARM_STATE | \ + LED_FUNCTION_THROTTLE | \ + LED_FUNCTION_THRUST_RING \ +) typedef struct ledConfig_s { From 793f83c4386b111c7c42a11bda5529d6e9119f13 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 23 Jan 2015 02:13:36 +0100 Subject: [PATCH 093/182] Remove commented out old code. --- src/main/io/ledstrip.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 67d8e21634..a2402f1960 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -484,7 +484,6 @@ void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSiz memset(ledConfigBuffer, 0, bufferSize); memset(&functions, 0, sizeof(functions)); memset(&directions, 0, sizeof(directions)); - //memset(&ringColors, 0 ,sizeof(ringColors)); for (mappingIndex = 0, index = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { if (ledConfig->flags & functionMappings[mappingIndex]) { From bf86deed4c3f0e1a9cb43ecb4d2e04e418c6fa49 Mon Sep 17 00:00:00 2001 From: Marc Egli Date: Fri, 23 Jan 2015 11:48:45 +0100 Subject: [PATCH 094/182] Add support for current sensors with a negative scale --- src/main/io/serial_cli.c | 4 ++-- src/main/sensors/battery.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index adf96d7f57..72338bc427 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -302,8 +302,8 @@ const clivalue_t valueTable[] = { { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 }, { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, 10, 50 }, - { "current_meter_scale", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, 1, 10000 }, - { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, 0, 1650 }, + { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, -10000, 10000 }, + { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, 0, 3300 }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, 0, 1 }, diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 23f918c7c3..aa5878ad91 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -27,7 +27,7 @@ typedef struct batteryConfig_s { uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V) uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V) - uint16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A + int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code. From 74e274c0a27106e209bca5142aadf9b39c957dbb Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 23 Jan 2015 19:20:00 +0100 Subject: [PATCH 095/182] SPRACINGF3 - Fix linker script memory address. --- src/main/target/stm32_flash_f303_128k.ld | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/stm32_flash_f303_128k.ld b/src/main/target/stm32_flash_f303_128k.ld index da6cbda9d4..3fe0aa8d52 100644 --- a/src/main/target/stm32_flash_f303_128k.ld +++ b/src/main/target/stm32_flash_f303_128k.ld @@ -22,7 +22,7 @@ _Min_Stack_Size = 0x400; /* required amount of stack */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x04000000, LENGTH = 126K /* last 2kb used for config storage */ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */ RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } From 6afa021d639a0cf13a663d2ac44b410d247618ae Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 23 Jan 2015 20:21:14 +0100 Subject: [PATCH 096/182] Fix bootloader mode for STM32F3 targets --- src/main/startup/startup_stm32f30x_md_gcc.S | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/main/startup/startup_stm32f30x_md_gcc.S b/src/main/startup/startup_stm32f30x_md_gcc.S index 9c2af124cb..e1387cbc0e 100644 --- a/src/main/startup/startup_stm32f30x_md_gcc.S +++ b/src/main/startup/startup_stm32f30x_md_gcc.S @@ -70,6 +70,12 @@ defined in linker script */ .weak Reset_Handler .type Reset_Handler, %function Reset_Handler: + ldr r0, =0x20009FFC // HJI 11/9/2012 + ldr r1, =0xDEADBEEF // HJI 11/9/2012 + ldr r2, [r0, #0] // HJI 11/9/2012 + str r0, [r0, #0] // HJI 11/9/2012 + cmp r2, r1 // HJI 11/9/2012 + beq Reboot_Loader // HJI 11/9/2012 /* Copy the data segment initializers from flash to SRAM */ movs r1, #0 @@ -104,6 +110,18 @@ LoopFillZerobss: /* Call the application's entry point.*/ bl main bx lr + +LoopForever: + b LoopForever + +Reboot_Loader: // HJI 11/9/2012 + + // Reboot to ROM // HJI 11/9/2012 + ldr r0, =0x1FFFD800 // HJI 4/26/2013 + ldr sp,[r0, #0] // HJI 11/9/2012 + ldr r0,[r0, #4] // HJI 11/9/2012 + bx r0 // HJI 11/9/2012 + .size Reset_Handler, .-Reset_Handler /** From 861f5b672550d91129f5db8acad3c4a08bc6e66b Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 16:26:56 -0800 Subject: [PATCH 097/182] Switched to using cpp as the default language and specifying arm in the makefile. --- .travis.yml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index c63522aa43..1c108bf808 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,14 +11,15 @@ env: - TARGET=SPARKY - TARGET=STM32F3DISCOVERY - TARGET=ALIENWIIF1 -language: c -compiler: arm-none-eabi-gcc +# We use cpp for unit tests, and c for the main project. +language: cpp +compiler: clang before_install: sudo add-apt-repository -y ppa:terry.guo/gcc-arm-embedded && sudo apt-get update install: sudo apt-get install build-essential gcc-arm-none-eabi git -before_script: $CC --version +before_script: arm-none-eabi-gcc --version script: make -j2 notifications: irc: "chat.freenode.net#cleanflight" use_notice: true - skip_join: true \ No newline at end of file + skip_join: true From 377f9dfc83b2707e4727980a894f4046dab99e09 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 16:37:13 -0800 Subject: [PATCH 098/182] Introduced a 'run all tests' command to the makefile. --- src/test/Makefile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/test/Makefile b/src/test/Makefile index 5001c57230..15c9ec9049 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -303,4 +303,8 @@ ws2811_unittest : \ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +test: $(TESTS) + for test in $(TESTS) ; do \ + $(OBJECT_DIR)/$$test; \ + done From 9a9cb0dbaae18974614a163bbfe9ad11df9d12a7 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 16:39:26 -0800 Subject: [PATCH 099/182] Make primative test runner script and skipped failing test for now. --- .travis.sh | 7 +++++++ src/test/Makefile | 4 ++-- 2 files changed, 9 insertions(+), 2 deletions(-) create mode 100755 .travis.sh diff --git a/.travis.sh b/.travis.sh new file mode 100755 index 0000000000..77a0820a4c --- /dev/null +++ b/.travis.sh @@ -0,0 +1,7 @@ +#!/bin/bash +# A hacky way of running the unit tests at the same time as the normal builds. +if [ $RUNTESTS ] ; then + cd ./src/test && make test +else + make -j2 +fi diff --git a/src/test/Makefile b/src/test/Makefile index 15c9ec9049..fcb6e63826 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -46,12 +46,12 @@ CXX_FLAGS = $(COMMON_FLAGS) \ TESTS = \ battery_unittest \ flight_imu_unittest \ - altitude_hold_unittest \ gps_conversion_unittest \ telemetry_hott_unittest \ rc_controls_unittest \ ledstrip_unittest \ - ws2811_unittest + ws2811_unittest \ + #altitude_hold_unittest \ # All Google Test headers. Usually you shouldn't change this # definition. From 3c4fcf918c1465f676b338ae200f1653760f036f Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 16:49:44 -0800 Subject: [PATCH 100/182] Add an extra travis run which runs the tests. --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 1c108bf808..3205ef41af 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,4 +1,5 @@ env: + - RUNTESTS=True - TARGET=CC3D - TARGET=CHEBUZZF3 - TARGET=CJMCU From 7fd639025296c5509a090ba6f2b766f9ff4b06c1 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 16:53:40 -0800 Subject: [PATCH 101/182] Switched to running our new script in travis. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 3205ef41af..93b76b6788 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,7 +18,7 @@ compiler: clang before_install: sudo add-apt-repository -y ppa:terry.guo/gcc-arm-embedded && sudo apt-get update install: sudo apt-get install build-essential gcc-arm-none-eabi git before_script: arm-none-eabi-gcc --version -script: make -j2 +script: ./.travis.sh notifications: irc: "chat.freenode.net#cleanflight" From e58825288bc8a5e4bc6f9944cf232d6c148f8f82 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 21:50:29 -0800 Subject: [PATCH 102/182] Make makefile return non zero exit code on test failure. --- src/test/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/Makefile b/src/test/Makefile index fcb6e63826..7ebf7f80ec 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -304,7 +304,7 @@ ws2811_unittest : \ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ test: $(TESTS) - for test in $(TESTS) ; do \ + set -e && for test in $(TESTS) ; do \ $(OBJECT_DIR)/$$test; \ done From 5f917f6b14c66842f807a24543c1b7f15b147d56 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 21:46:42 -0800 Subject: [PATCH 103/182] Added maths test. Conflicts: src/test/Makefile --- src/test/Makefile | 18 +++++++ src/test/unit/maths_unittest.cc | 92 +++++++++++++++++++++++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 src/test/unit/maths_unittest.cc diff --git a/src/test/Makefile b/src/test/Makefile index 5001c57230..1e43fc0f92 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -47,6 +47,7 @@ TESTS = \ battery_unittest \ flight_imu_unittest \ altitude_hold_unittest \ + maths_unittest \ gps_conversion_unittest \ telemetry_hott_unittest \ rc_controls_unittest \ @@ -154,6 +155,23 @@ flight_imu_unittest : \ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/maths_unittest.o : \ + $(TEST_DIR)/maths_unittest.cc \ + $(USER_DIR)/flight/imu.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/maths_unittest.cc -o $@ + +maths_unittest : \ + $(OBJECT_DIR)/flight/imu.o \ + $(OBJECT_DIR)/flight/altitudehold.o \ + $(OBJECT_DIR)/maths_unittest.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(OBJECT_DIR)/flight/altitudehold.o : \ diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc new file mode 100644 index 0000000000..50c9b9847c --- /dev/null +++ b/src/test/unit/maths_unittest.cc @@ -0,0 +1,92 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include + +#define BARO + +extern "C" { + #include "common/axis.h" + #include "flight/flight.h" + + #include "sensors/sensors.h" + #include "drivers/sensor.h" + #include "drivers/accgyro.h" + #include "drivers/compass.h" + #include "sensors/gyro.h" + #include "sensors/compass.h" + #include "sensors/acceleration.h" + #include "sensors/barometer.h" + + #include "config/runtime_config.h" + + #include "flight/mixer.h" + #include "flight/imu.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +#define DOWNWARDS_THRUST true +#define UPWARDS_THRUST false + + +TEST(MathsUnittest, Placeholder) +{ + // TODO test things + EXPECT_EQ(true, true); +} + +// STUBS + +extern "C" { +uint32_t rcModeActivationMask; +int16_t rcCommand[4]; + +uint16_t acc_1G; +int16_t heading; +gyro_t gyro; +int16_t magADC[XYZ_AXIS_COUNT]; +int32_t BaroAlt; +int16_t debug[4]; + +uint8_t stateFlags; +uint16_t flightModeFlags; +uint8_t armingFlags; + +int32_t sonarAlt; + + +void gyroGetADC(void) {}; +bool sensors(uint32_t mask) +{ + UNUSED(mask); + return false; +}; +void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) +{ + UNUSED(rollAndPitchTrims); +} + +uint32_t micros(void) { return 0; } +bool isBaroCalibrationComplete(void) { return true; } +void performBaroCalibrationCycle(void) {} +int32_t baroCalculateAltitude(void) { return 0; } +} From be03ed95fa7b1f3d2274fb202039d2399d32c566 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 22:40:00 -0800 Subject: [PATCH 104/182] Renamed min, max and abs macros to MIN MAX and ABS. --- src/main/blackbox/blackbox.c | 4 ++-- src/main/common/maths.c | 2 +- src/main/common/maths.h | 6 +++--- src/main/common/typeconversion.c | 2 +- src/main/drivers/compass_hmc5883l.c | 4 ++-- src/main/flight/altitudehold.c | 8 ++++---- src/main/flight/flight.c | 10 +++++----- src/main/flight/mixer.c | 2 +- src/main/flight/navigation.c | 16 ++++++++-------- src/main/io/display.c | 4 ++-- src/main/io/ledstrip.c | 8 ++++---- src/main/io/rc_controls.c | 4 ++-- src/main/io/serial_msp.c | 4 ++-- src/main/mw.c | 6 +++--- src/main/telemetry/frsky.c | 4 ++-- 15 files changed, 42 insertions(+), 42 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 447f791f19..251b42cda2 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -894,7 +894,7 @@ static void configureBlackboxPort(void) * * 9 / 1250 = 7200 / 1000000 */ - serialChunkSize = max((masterConfig.looptime * 9) / 1250, 4); + serialChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); } static void releaseBlackboxPort(void) @@ -1133,7 +1133,7 @@ static bool blackboxWriteSysinfo() } // How many bytes can we afford to transmit this loop? - xmitState.u.serialBudget = min(xmitState.u.serialBudget + serialChunkSize, 64); + xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + serialChunkSize, 64); // Most headers will consume at least 20 bytes so wait until we've built up that much link budget if (xmitState.u.serialBudget < 20) { diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 5ed16f916d..15d02f8f66 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -22,7 +22,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband) { - if (abs(value) < deadband) { + if (ABS(value) < deadband) { value = 0; } else if (value > 0) { value -= deadband; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index d65b87df22..8dc281fee3 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -26,9 +26,9 @@ #define RAD (M_PIf / 180.0f) -#define min(a, b) ((a) < (b) ? (a) : (b)) -#define max(a, b) ((a) > (b) ? (a) : (b)) -#define abs(x) ((x) > 0 ? (x) : -(x)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define ABS(x) ((x) > 0 ? (x) : -(x)) typedef struct stdev_t { diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 7df5bb1370..4e3f8b4c2e 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -159,7 +159,7 @@ char *ftoa(float x, char *floatString) value = (int32_t)(x * 1000.0f); // Convert float * 1000 to an integer - itoa(abs(value), intString1, 10); // Create string from abs of integer value + itoa(ABS(value), intString1, 10); // Create string from abs of integer value if (value >= 0) intString2[0] = ' '; // Positive number, add a pad space diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 63d0566c90..afa40d6c45 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -177,7 +177,7 @@ void hmc5883lInit(void) xyz_total[Z] += magADC[Z]; // Detect saturation. - if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) { + if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { bret = false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } @@ -197,7 +197,7 @@ void hmc5883lInit(void) xyz_total[Z] -= magADC[Z]; // Detect saturation. - if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) { + if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { bret = false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index d2bb2b146c..f74a32c73e 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -91,7 +91,7 @@ static void applyMultirotorAltHold(void) // multirotor alt hold if (rcControlsConfig->alt_hold_fast_change) { // rapid alt changes - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { errorVelocityI = 0; isAltHoldChanged = 1; rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband; @@ -104,7 +104,7 @@ static void applyMultirotorAltHold(void) } } else { // slow alt changes, mostly used for aerial photography - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2; velocityControl = 1; @@ -172,12 +172,12 @@ void updateSonarAltHoldState(void) bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination) { - return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; + return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; } int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination) { - return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees)); + return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees)); } diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index dbc049b3d3..7b994e5f6d 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -117,11 +117,11 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); - if(abs(stickPosAil) > abs(stickPosEle)){ - mostDeflectedPos = abs(stickPosAil); + if(ABS(stickPosAil) > ABS(stickPosEle)){ + mostDeflectedPos = ABS(stickPosAil); } else { - mostDeflectedPos = abs(stickPosEle); + mostDeflectedPos = ABS(stickPosEle); } // Progressively turn off the horizon self level strength as the stick is banged over @@ -220,7 +220,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa UNUSED(controlRateConfig); // **** PITCH & ROLL & YAW PID **** - prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500] + prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500] for (axis = 0; axis < 3; axis++) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC @@ -252,7 +252,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp - if ((abs(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && abs(rcCommand[axis]) > 100)) + if ((ABS(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100)) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 324bdf12fc..18c9e380fc 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -509,7 +509,7 @@ void mixTable(void) if (motorCount > 3) { // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); + axisPID[YAW] = constrain(axisPID[YAW], -100 - ABS(rcCommand[YAW]), +100 + ABS(rcCommand[YAW])); } // motors for non-servo mixes diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index dd09430562..9c1e919cc8 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -306,7 +306,7 @@ void onGpsNewData(void) dTnav = (float)(millis() - nav_loopTimer) / 1000.0f; nav_loopTimer = millis(); // prevent runup from bad GPS - dTnav = min(dTnav, 1.0f); + dTnav = MIN(dTnav, 1.0f); GPS_calculateDistanceAndDirectionToHome(); @@ -413,7 +413,7 @@ void gpsUsePIDs(pidProfile_t *pidProfile) // static void GPS_calc_longitude_scaling(int32_t lat) { - float rads = (abs((float)lat) / 10000000.0f) * 0.0174532925f; + float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f; GPS_scaleLonDown = cosf(rads); } @@ -442,7 +442,7 @@ static bool check_missed_wp(void) int32_t temp; temp = target_bearing - original_target_bearing; temp = wrap_18000(temp); - return (abs(temp) > 10000); // we passed the waypoint by 100 degrees + return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees } #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f @@ -536,7 +536,7 @@ static void GPS_calc_poshold(void) // get rid of noise #if defined(GPS_LOW_SPEED_D_FILTER) - if (abs(actual_speed[axis]) < 50) + if (ABS(actual_speed[axis]) < 50) d = 0; #endif @@ -582,7 +582,7 @@ static void GPS_calc_nav_rate(uint16_t max_speed) // static void GPS_update_crosstrack(void) { - if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following + if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following float temp = (target_bearing - original_target_bearing) * RADX100; crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000); @@ -607,10 +607,10 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow) { // max_speed is default 400 or 4m/s if (_slow) { - max_speed = min(max_speed, wp_distance / 2); + max_speed = MIN(max_speed, wp_distance / 2); } else { - max_speed = min(max_speed, wp_distance); - max_speed = max(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s + max_speed = MIN(max_speed, wp_distance); + max_speed = MAX(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s } // limit the ramp up of the speed diff --git a/src/main/io/display.c b/src/main/io/display.c index 001615e874..ef103fd44e 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -292,12 +292,12 @@ void showProfilePage(void) void showGpsPage() { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - i2c_OLED_set_xy(max(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++); + i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++); uint32_t index; for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) { uint8_t bargraphValue = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1); - bargraphValue = min(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); + bargraphValue = MIN(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphValue); } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 75eadc25da..1a8cd62608 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -775,10 +775,10 @@ void updateLedStrip(void) if (indicatorFlashNow) { - uint8_t rollScale = abs(rcCommand[ROLL]) / 50; - uint8_t pitchScale = abs(rcCommand[PITCH]) / 50; - uint8_t scale = max(rollScale, pitchScale); - nextIndicatorFlashAt = now + (LED_STRIP_5HZ / max(1, scale)); + uint8_t rollScale = ABS(rcCommand[ROLL]) / 50; + uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50; + uint8_t scale = MAX(rollScale, pitchScale); + nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale)); if (indicatorFlashState == 0) { indicatorFlashState = 1; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 9bd4560085..64a70ba40e 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -64,7 +64,7 @@ bool isUsingSticksForArming(void) bool areSticksInApModePosition(uint16_t ap_mode) { - return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode; + return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -518,7 +518,7 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) } int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { - return min(abs(rcData[axis] - midrc), 500); + return MIN(ABS(rcData[axis] - midrc), 500); } void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index e7b1d1a43b..5076986cbb 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -820,9 +820,9 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery serialize16(rssi); if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { - serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps + serialize16((uint16_t)constrain((ABS(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps } else - serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps + serialize16((uint16_t)constrain(ABS(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps break; case MSP_RC_TUNING: headSerialReply(7); diff --git a/src/main/mw.c b/src/main/mw.c index 9b2a45e0a6..f7081090b1 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -182,7 +182,7 @@ void annexCode(void) } for (axis = 0; axis < 3; axis++) { - tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); + tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { @@ -205,7 +205,7 @@ void annexCode(void) } } rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; - prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * abs(tmp) / 500; + prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; @@ -397,7 +397,7 @@ void updateInflightCalibrationState(void) void updateMagHold(void) { - if (abs(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) { + if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) { int16_t dif = heading - magHold; if (dif <= -180) dif += 360; diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index be9e0b830e..f30f961eb3 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -161,7 +161,7 @@ static void sendBaro(void) sendDataHead(ID_ALTITUDE_BP); serialize16(BaroAlt / 100); sendDataHead(ID_ALTITUDE_AP); - serialize16(abs(BaroAlt % 100)); + serialize16(ABS(BaroAlt % 100)); } static void sendGpsAltitude(void) @@ -247,7 +247,7 @@ static void sendTime(void) static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) { int32_t absgps, deg, min; - absgps = abs(mwiigps); + absgps = ABS(mwiigps); deg = absgps / GPS_DEGREES_DIVIDER; absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 min = absgps / GPS_DEGREES_DIVIDER; // minutes left From cfa4e19acd197e2c339fb65895f7aba12d359aa7 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 21:46:42 -0800 Subject: [PATCH 105/182] Added maths test. Conflicts: src/test/Makefile --- src/test/Makefile | 18 +++++++ src/test/unit/maths_unittest.cc | 92 +++++++++++++++++++++++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 src/test/unit/maths_unittest.cc diff --git a/src/test/Makefile b/src/test/Makefile index 5001c57230..1e43fc0f92 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -47,6 +47,7 @@ TESTS = \ battery_unittest \ flight_imu_unittest \ altitude_hold_unittest \ + maths_unittest \ gps_conversion_unittest \ telemetry_hott_unittest \ rc_controls_unittest \ @@ -154,6 +155,23 @@ flight_imu_unittest : \ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/maths_unittest.o : \ + $(TEST_DIR)/maths_unittest.cc \ + $(USER_DIR)/flight/imu.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/maths_unittest.cc -o $@ + +maths_unittest : \ + $(OBJECT_DIR)/flight/imu.o \ + $(OBJECT_DIR)/flight/altitudehold.o \ + $(OBJECT_DIR)/maths_unittest.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + $(OBJECT_DIR)/flight/altitudehold.o : \ diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc new file mode 100644 index 0000000000..50c9b9847c --- /dev/null +++ b/src/test/unit/maths_unittest.cc @@ -0,0 +1,92 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include + +#define BARO + +extern "C" { + #include "common/axis.h" + #include "flight/flight.h" + + #include "sensors/sensors.h" + #include "drivers/sensor.h" + #include "drivers/accgyro.h" + #include "drivers/compass.h" + #include "sensors/gyro.h" + #include "sensors/compass.h" + #include "sensors/acceleration.h" + #include "sensors/barometer.h" + + #include "config/runtime_config.h" + + #include "flight/mixer.h" + #include "flight/imu.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +#define DOWNWARDS_THRUST true +#define UPWARDS_THRUST false + + +TEST(MathsUnittest, Placeholder) +{ + // TODO test things + EXPECT_EQ(true, true); +} + +// STUBS + +extern "C" { +uint32_t rcModeActivationMask; +int16_t rcCommand[4]; + +uint16_t acc_1G; +int16_t heading; +gyro_t gyro; +int16_t magADC[XYZ_AXIS_COUNT]; +int32_t BaroAlt; +int16_t debug[4]; + +uint8_t stateFlags; +uint16_t flightModeFlags; +uint8_t armingFlags; + +int32_t sonarAlt; + + +void gyroGetADC(void) {}; +bool sensors(uint32_t mask) +{ + UNUSED(mask); + return false; +}; +void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) +{ + UNUSED(rollAndPitchTrims); +} + +uint32_t micros(void) { return 0; } +bool isBaroCalibrationComplete(void) { return true; } +void performBaroCalibrationCycle(void) {} +int32_t baroCalculateAltitude(void) { return 0; } +} From 53c0a09b0879a6e77842cc11ee6de8d8171be37c Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 22:37:39 -0800 Subject: [PATCH 106/182] Implemented actual tests. --- src/test/unit/maths_unittest.cc | 146 ++++++++++++++++++++------------ 1 file changed, 93 insertions(+), 53 deletions(-) diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 50c9b9847c..7536c33a43 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -23,70 +23,110 @@ #define BARO extern "C" { - #include "common/axis.h" - #include "flight/flight.h" - - #include "sensors/sensors.h" - #include "drivers/sensor.h" - #include "drivers/accgyro.h" - #include "drivers/compass.h" - #include "sensors/gyro.h" - #include "sensors/compass.h" - #include "sensors/acceleration.h" - #include "sensors/barometer.h" + #include "common/maths.h" #include "config/runtime_config.h" - - #include "flight/mixer.h" - #include "flight/imu.h" } #include "unittest_macros.h" #include "gtest/gtest.h" -#define DOWNWARDS_THRUST true -#define UPWARDS_THRUST false - - -TEST(MathsUnittest, Placeholder) +TEST(MathsUnittest, TestConstrain) { - // TODO test things - EXPECT_EQ(true, true); + // Within bounds. + EXPECT_EQ(constrain(1, 0, 2), 1); + + // Equal to bottom bound. + EXPECT_EQ(constrain(1, 1, 2), 1); + // Equal to top bound. + EXPECT_EQ(constrain(1, 0, 1), 1); + + // Equal to both bottom and top bound. + EXPECT_EQ(constrain(1, 1, 1), 1); + + // Above top bound. + EXPECT_EQ(constrain(2, 0, 1), 1); + // Below bottom bound. + EXPECT_EQ(constrain(0, 1, 2), 1); + + // Above bouth bounds. + EXPECT_EQ(constrain(2, 0, 1), 1); + // Below bouth bounds. + EXPECT_EQ(constrain(0, 1, 2), 1); } -// STUBS - -extern "C" { -uint32_t rcModeActivationMask; -int16_t rcCommand[4]; - -uint16_t acc_1G; -int16_t heading; -gyro_t gyro; -int16_t magADC[XYZ_AXIS_COUNT]; -int32_t BaroAlt; -int16_t debug[4]; - -uint8_t stateFlags; -uint16_t flightModeFlags; -uint8_t armingFlags; - -int32_t sonarAlt; - - -void gyroGetADC(void) {}; -bool sensors(uint32_t mask) +TEST(MathsUnittest, TestConstrainf) { - UNUSED(mask); - return false; -}; -void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) -{ - UNUSED(rollAndPitchTrims); + // Within bounds. + EXPECT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f); + + // Equal to bottom bound. + EXPECT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f); + // Equal to top bound. + EXPECT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f); + + // Equal to both bottom and top bound. + EXPECT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f); + + // Above top bound. + EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f); + // Below bottom bound. + EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f); + + // Above bouth bounds. + EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f); + // Below bouth bounds. + EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f); } -uint32_t micros(void) { return 0; } -bool isBaroCalibrationComplete(void) { return true; } -void performBaroCalibrationCycle(void) {} -int32_t baroCalculateAltitude(void) { return 0; } +TEST(MathsUnittest, TestDegreesToRadians) +{ + EXPECT_EQ(degreesToRadians(0), 0.0f); + EXPECT_EQ(degreesToRadians(90), 0.5f * M_PIf); + EXPECT_EQ(degreesToRadians(180), M_PIf); + EXPECT_EQ(degreesToRadians(-180), - M_PIf); +} + +TEST(MathsUnittest, TestApplyDeadband) +{ + EXPECT_EQ(applyDeadband(0, 0), 0); + EXPECT_EQ(applyDeadband(1, 0), 1); + EXPECT_EQ(applyDeadband(-1, 0), -1); + + EXPECT_EQ(applyDeadband(0, 10), 0); + EXPECT_EQ(applyDeadband(1, 10), 0); + EXPECT_EQ(applyDeadband(10, 10), 0); + + EXPECT_EQ(applyDeadband(11, 10), 1); + EXPECT_EQ(applyDeadband(-11, 10), -1); +} + +void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b) +{ + EXPECT_EQ(a->X, b->X); + EXPECT_EQ(a->Y, b->Y); + EXPECT_EQ(a->Z, b->Z); +} + +TEST(MathsUnittest, TestRotateVectorWithNoAngle) +{ + fp_vector vector = {1.0f, 0.0f, 0.0f}; + fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}}; + + rotateV(&vector, &euler_angles); + fp_vector expected_result = {1.0f, 0.0f, 0.0f}; + + expectVectorsAreEqual(&vector, &expected_result); +} + +TEST(MathsUnittest, TestRotateVectorAroundAxis) +{ + // Rotate a vector <1, 0, 0> around an each axis x y and z. + fp_vector vector = {1.0f, 0.0f, 0.0f}; + fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}}; + + rotateV(&vector, &euler_angles); + fp_vector expected_result = {1.0f, 0.0f, 0.0f}; + + expectVectorsAreEqual(&vector, &expected_result); } From 33481c86d7a87eaf84795d46a19ddea72d5c430c Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 22:44:47 -0800 Subject: [PATCH 107/182] Fixed test dependencies. --- src/test/Makefile | 3 --- src/test/unit/maths_unittest.cc | 2 -- 2 files changed, 5 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index 1e43fc0f92..f089fdddf6 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -157,15 +157,12 @@ flight_imu_unittest : \ $(OBJECT_DIR)/maths_unittest.o : \ $(TEST_DIR)/maths_unittest.cc \ - $(USER_DIR)/flight/imu.h \ $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/maths_unittest.cc -o $@ maths_unittest : \ - $(OBJECT_DIR)/flight/imu.o \ - $(OBJECT_DIR)/flight/altitudehold.o \ $(OBJECT_DIR)/maths_unittest.o \ $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/gtest_main.a diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 7536c33a43..0b341671f6 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -24,8 +24,6 @@ extern "C" { #include "common/maths.h" - - #include "config/runtime_config.h" } #include "unittest_macros.h" From d691f7284904c43c2e9a8bcdfd93da83307298fc Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:03:22 -0800 Subject: [PATCH 108/182] Moved configuring of IMU all into one function call. --- src/main/config/config.c | 7 +++---- src/main/flight/imu.c | 21 ++++++++++++++++----- src/main/flight/imu.h | 12 +++++++++--- 3 files changed, 28 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 17c656ddd9..010e686b08 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -616,7 +616,9 @@ void activateConfig(void) configureIMU( &imuRuntimeConfig, ¤tProfile->pidProfile, - ¤tProfile->accDeadband + ¤tProfile->accDeadband, + currentProfile->accz_lpf_cutoff, + currentProfile->throttle_correction_angle ); configureAltitudeHold( @@ -626,9 +628,6 @@ void activateConfig(void) &masterConfig.escAndServoConfig ); - calculateThrottleAngleScale(currentProfile->throttle_correction_angle); - calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff); - #ifdef BARO useBarometerConfig(¤tProfile->barometerConfig); #endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f0a4535a00..284d11ab0e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -79,11 +79,19 @@ imuRuntimeConfig_t *imuRuntimeConfig; pidProfile_t *pidProfile; accDeadband_t *accDeadband; -void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband) +void configureIMU( + imuRuntimeConfig_t *initialImuRuntimeConfig, + pidProfile_t *initialPidProfile, + accDeadband_t *initialAccDeadband, + float accz_lpf_cutoff, + uint16_t throttle_correction_angle +) { imuRuntimeConfig = initialImuRuntimeConfig; pidProfile = initialPidProfile; accDeadband = initialAccDeadband; + fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff); + throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } void initIMU() @@ -93,14 +101,17 @@ void initIMU() gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } -void calculateThrottleAngleScale(uint16_t throttle_correction_angle) +float calculateThrottleAngleScale(uint16_t throttle_correction_angle) { - throttleAngleScale = (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); + return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } -void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) +/* +* Calculate RC time constant used in the accZ lpf. +*/ +float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) { - fc_acc = 0.5f / (M_PIf * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf + return 0.5f / (M_PIf * accz_lpf_cutoff); } void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 552093508d..cf03c8e7f0 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,12 +30,18 @@ typedef struct imuRuntimeConfig_s { int8_t small_angle; } imuRuntimeConfig_t; -void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband); +void configureIMU( + imuRuntimeConfig_t *initialImuRuntimeConfig, + pidProfile_t *initialPidProfile, + accDeadband_t *initialAccDeadband, + float accz_lpf_cutoff, + uint16_t throttle_correction_angle +); void calculateEstimatedAltitude(uint32_t currentTime); void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); -void calculateThrottleAngleScale(uint16_t throttle_correction_angle); +float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); -void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); +float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); void accSum_reset(void); From 8d994df45757fb6bd046a14e5e65842618943775 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:06:46 -0800 Subject: [PATCH 109/182] Move code around to avoid forward declaration. --- src/main/flight/imu.c | 52 +++++++++++++++++++++---------------------- 1 file changed, 25 insertions(+), 27 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 284d11ab0e..4b4bbb47bb 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -73,8 +73,6 @@ int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians -static void getEstimatedAttitude(void); - imuRuntimeConfig_t *imuRuntimeConfig; pidProfile_t *pidProfile; accDeadband_t *accDeadband; @@ -114,31 +112,6 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) return 0.5f / (M_PIf * accz_lpf_cutoff); } -void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) -{ - static int16_t gyroYawSmooth = 0; - - gyroGetADC(); - if (sensors(SENSOR_ACC)) { - updateAccelerationReadings(accelerometerTrims); - getEstimatedAttitude(); - } else { - accADC[X] = 0; - accADC[Y] = 0; - accADC[Z] = 0; - } - - gyroData[FD_ROLL] = gyroADC[FD_ROLL]; - gyroData[FD_PITCH] = gyroADC[FD_PITCH]; - - if (mixerMode == MIXER_TRI) { - gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3; - gyroYawSmooth = gyroData[FD_YAW]; - } else { - gyroData[FD_YAW] = gyroADC[FD_YAW]; - } -} - // ************************************************** // Simplified IMU based on "Complementary Filter" // Inspired by http://starlino.com/imu_guide.html @@ -309,6 +282,31 @@ static void getEstimatedAttitude(void) acc_calc(deltaT); // rotate acc vector into earth frame } +void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) +{ + static int16_t gyroYawSmooth = 0; + + gyroGetADC(); + if (sensors(SENSOR_ACC)) { + updateAccelerationReadings(accelerometerTrims); + getEstimatedAttitude(); + } else { + accADC[X] = 0; + accADC[Y] = 0; + accADC[Z] = 0; + } + + gyroData[FD_ROLL] = gyroADC[FD_ROLL]; + gyroData[FD_PITCH] = gyroADC[FD_PITCH]; + + if (mixerMode == MIXER_TRI) { + gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3; + gyroYawSmooth = gyroData[FD_YAW]; + } else { + gyroData[FD_YAW] = gyroADC[FD_YAW]; + } +} + // Correction of throttle in lateral wind. int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) { From f312636b9fef90977a52dcd721791be1f5508196 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:08:38 -0800 Subject: [PATCH 110/182] Add comments suggesting moving throttle angle correction code into own module. --- src/main/flight/imu.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 4b4bbb47bb..307b7205a2 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -82,6 +82,7 @@ void configureIMU( pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband, float accz_lpf_cutoff, + //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c uint16_t throttle_correction_angle ) { @@ -89,6 +90,7 @@ void configureIMU( pidProfile = initialPidProfile; accDeadband = initialAccDeadband; fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff); + //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } @@ -99,6 +101,7 @@ void initIMU() gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } +//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c float calculateThrottleAngleScale(uint16_t throttle_correction_angle) { return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); @@ -307,7 +310,7 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) } } -// Correction of throttle in lateral wind. +//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) { float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); From 6a0d1b84f2ca3d29fbddf18ca59993e2f7354f1d Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:27:53 -0800 Subject: [PATCH 111/182] Add tests for the calculateHeading method of the IMU. --- src/test/unit/flight_imu_unittest.cc | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 44aada0687..d82d12fb1d 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -24,6 +24,7 @@ extern "C" { #include "common/axis.h" + #include "common/maths.h" #include "flight/flight.h" #include "sensors/sensors.h" @@ -48,10 +49,23 @@ extern "C" { #define UPWARDS_THRUST false -TEST(FlightImuTest, Placeholder) +TEST(FlightImuTest, TestCalculateHeading) { - // TODO test things - EXPECT_EQ(true, true); + //TODO: Add test cases using the Z dimension. + t_fp_vector north = {.A={1.0f, 0.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&north), 0); + + t_fp_vector east = {.A={0.0f, 1.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&east), 90); + + t_fp_vector south = {.A={-1.0f, 0.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&south), 180); + + t_fp_vector west = {.A={0.0f, -1.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&west), 270); + + t_fp_vector north_east = {.A={1.0f, 1.0f, 0.0f}}; + EXPECT_EQ(calculateHeading(&north_east), 45); } // STUBS From 67a2d5cd756b2b536447e12462e99e0a27a544b5 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Fri, 23 Jan 2015 23:29:45 -0800 Subject: [PATCH 112/182] Add test for calculate heading. --- src/main/flight/imu.c | 28 +++++++++++++++++++++++++--- src/main/flight/imu.h | 2 ++ 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 307b7205a2..3428f73412 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -188,11 +188,33 @@ void acc_calc(uint32_t deltaT) /* * Baseflight calculation by Luggi09 originates from arducopter * ============================================================ +* This function turns a vector which is (usually) the direction +* of magnetic flux in the coordinate system of the craft into +* a compass heading in degrees, clockwise away from north. Note +* that the magnetic flux is not parrelell with the vector towards +* magnetic north it's self but rather is parrelell with the ground +* when near the equator. * -* Calculate the heading of the craft (in degrees clockwise from North) -* when given a 3-vector representing the direction of North. +* First we consider it in 2D: +* +* An example, the vector <1, 1> would be turned into the heading +* 45 degrees, representing it's angle clockwise from north. +* +* ***************** * +* * | <1,1> * +* * | / * +* * | / * +* * |/ * +* * * * +* * * +* * * +* * * +* * * +* ******************* +* +* //TODO: Add explanation for how it uses the Z dimension. */ -static int16_t calculateHeading(t_fp_vector *vec) +int16_t calculateHeading(t_fp_vector *vec) { int16_t head; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index cf03c8e7f0..45949c4e1e 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -44,4 +44,6 @@ float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); +int16_t calculateHeading(t_fp_vector *vec); + void accSum_reset(void); From ec7bcbe2628f852418ea063e8c730b200fb36ae5 Mon Sep 17 00:00:00 2001 From: Philippe-France Date: Sat, 24 Jan 2015 15:08:51 +0100 Subject: [PATCH 113/182] solution of issue 318 --- src/main/drivers/pwm_rx.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index e0b72a6ba0..5d057a050b 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -234,9 +234,7 @@ static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb); if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) { - if (pwmInputPort->state == 0) { - captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT; - } + captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT; pwmInputPort->missedEvents = 0; } } From 83748bac98806ced978bd3032ed632bbab692eb2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 25 Jan 2015 00:52:27 +0100 Subject: [PATCH 114/182] STM32F3 - Fix serial port constraints and functions when VCP is not used. --- src/main/io/serial.c | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 7493f51d2e..e9e9a47122 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -76,13 +76,14 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { #ifdef USE_VCP {SERIAL_PORT_USB_VCP, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, #endif +#ifdef USE_USART1 {SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, - {SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, -#if (SERIAL_PORT_COUNT > 3) - {SERIAL_PORT_USART3, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, -#if (SERIAL_PORT_COUNT > 4) - {SERIAL_PORT_USART4, NULL, SCENARIO_UNUSED, FUNCTION_NONE} #endif +#ifdef USE_USART2 + {SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, +#endif +#ifdef USE_USART3 + {SERIAL_PORT_USART3, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, #endif }; @@ -90,13 +91,14 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { #ifdef USE_VCP {SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE }, #endif +#ifdef USE_USART1 {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, - {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, -#if (SERIAL_PORT_COUNT > 3) - {SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, -#if (SERIAL_PORT_COUNT > 4) - {SERIAL_PORT_USART4, 9600, 115200, SPF_SUPPORTS_CALLBACK} #endif +#ifdef USE_USART2 + {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, +#endif +#ifdef USE_USART3 + {SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #endif }; From 22f80129f2ef150d38e1bf3bc0a59f7ba390c96c Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 25 Jan 2015 01:07:14 +0100 Subject: [PATCH 115/182] Update calculate heading comment (ledvinap). --- src/main/flight/imu.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3428f73412..afea1727e9 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -188,12 +188,13 @@ void acc_calc(uint32_t deltaT) /* * Baseflight calculation by Luggi09 originates from arducopter * ============================================================ -* This function turns a vector which is (usually) the direction -* of magnetic flux in the coordinate system of the craft into -* a compass heading in degrees, clockwise away from north. Note -* that the magnetic flux is not parrelell with the vector towards -* magnetic north it's self but rather is parrelell with the ground -* when near the equator. +* This function rotates magnetic vector to cancel actual yaw and +* pitch of craft. Then it computes it's direction in X/Y plane. +* This value is returned as compass heading, value is 0-360 degrees. +* +* Note that Earth's magnetic field is not parallel with ground unless +* you are near equator. Its inclination is considerable, >60 degrees +* towards ground in most of Europe. * * First we consider it in 2D: * From 9500fc2f33b9a060842785b4766898c95c12c140 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 25 Jan 2015 01:07:38 +0100 Subject: [PATCH 116/182] Fix compiler error. --- src/main/config/config.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index d545e21993..d3b7db24c3 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -25,6 +25,8 @@ #include "common/color.h" #include "common/axis.h" +#include "common/maths.h" + #include "flight/flight.h" #include "drivers/sensor.h" From c93f0a71e1ed68e658f9f190bb75db28566da38d Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 25 Jan 2015 01:24:57 +0100 Subject: [PATCH 117/182] Adding unit tests for constrain wih negative values since the constrain method takes signed arguments. --- src/test/unit/maths_unittest.cc | 27 ++++++++++++++++++++++----- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 0b341671f6..429f2396d6 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -31,7 +31,9 @@ extern "C" { TEST(MathsUnittest, TestConstrain) { - // Within bounds. + // Within bounds + EXPECT_EQ(constrain(0, 0, 0), 0); + EXPECT_EQ(constrain(1, 1, 1), 1); EXPECT_EQ(constrain(1, 0, 2), 1); // Equal to bottom bound. @@ -46,11 +48,26 @@ TEST(MathsUnittest, TestConstrain) EXPECT_EQ(constrain(2, 0, 1), 1); // Below bottom bound. EXPECT_EQ(constrain(0, 1, 2), 1); +} - // Above bouth bounds. - EXPECT_EQ(constrain(2, 0, 1), 1); - // Below bouth bounds. - EXPECT_EQ(constrain(0, 1, 2), 1); +TEST(MathsUnittest, TestConstrainNegatives) +{ + // Within bounds. + EXPECT_EQ(constrain(-1, -1, -1), -1); + EXPECT_EQ(constrain(-1, -2, 0), -1); + + // Equal to bottom bound. + EXPECT_EQ(constrain(-1, -1, 0), -1); + // Equal to top bound. + EXPECT_EQ(constrain(-1, -2, -1), -1); + + // Equal to both bottom and top bound. + EXPECT_EQ(constrain(-1, -1, -1), -1); + + // Above top bound. + EXPECT_EQ(constrain(-1, -3, -2), -2); + // Below bottom bound. + EXPECT_EQ(constrain(-3, -2, -1), -2); } TEST(MathsUnittest, TestConstrainf) From aaf308bbcd8a741b55cfdb3fd611075446696e92 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 25 Jan 2015 01:35:57 +0100 Subject: [PATCH 118/182] Fix compilation failure of altitude_hold_unittest --- src/test/unit/altitude_hold_unittest.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc index b9df220b11..b2e1e328cc 100644 --- a/src/test/unit/altitude_hold_unittest.cc +++ b/src/test/unit/altitude_hold_unittest.cc @@ -27,6 +27,7 @@ extern "C" { #include "flight/flight.h" #include "sensors/sensors.h" + #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" From 3916e5f2b5bec79810c5d0e18efac114a196f685 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 25 Jan 2015 01:36:30 +0100 Subject: [PATCH 119/182] Update ledstrip unit test to deal with recently added led color. --- src/test/unit/ledstrip_unittest.cc | 201 +++++++++++++++-------------- 1 file changed, 102 insertions(+), 99 deletions(-) diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 04c2a53869..a1e86d20d7 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -32,6 +32,8 @@ extern "C" { #include "rx/rx.h" + #include "io/rc_controls.h" + #include "drivers/light_ws2811strip.h" #include "io/ledstrip.h" } @@ -66,38 +68,38 @@ TEST(LedStripTest, parseLedStripConfig) // given static const ledConfig_t expectedLedStripConfig[WS2811_LED_STRIP_LENGTH] = { - { CALCULATE_LED_XY( 9, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(10, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(11, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(11, 11), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(10, 10), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 9, 9), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 9, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 9, 9), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(10, 5), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(11, 4), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(12, 3), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(12, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(11, 1), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(10, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(10, 5), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(11, 4), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(12, 3), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(12, 2), 0, LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(11, 1), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(10, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 7, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 6, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 5, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 6, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 5, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 4), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 2, 5), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 3), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 2, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 2, 9), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 1, 10), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 9), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, { 0, 0 }, { 0, 0 }, @@ -110,42 +112,42 @@ TEST(LedStripTest, parseLedStripConfig) // Spider quad // right rear cluster - "9,9:S:FW", - "10,10:S:FW", - "11,11:S:IA", - "11,11:E:IA", - "10,10:E:F", - "9,9:E:F", + "9,9:S:FW:0", + "10,10:S:FW:0", + "11,11:S:IA:0", + "11,11:E:IA:0", + "10,10:E:F:0", + "9,9:E:F:0", // right front cluster - "10,5:S:F", - "11,4:S:F", - "12,3:S:IA", - "12,2:N:IA", - "11,1:N:F", - "10,0:N:F", + "10,5:S:F:0", + "11,4:S:F:0", + "12,3:S:IA:0", + "12,2:N:IA:0", + "11,1:N:F:0", + "10,0:N:F:0", // center front cluster - "7,0:N:FW", - "6,0:N:FW", - "5,0:N:FW", - "4,0:N:FW", + "7,0:N:FW:0", + "6,0:N:FW:0", + "5,0:N:FW:0", + "4,0:N:FW:0", // left front cluster - "2,0:N:F", - "1,1:N:F", - "0,2:N:IA", - "0,3:W:IA", - "1,4:W:F", - "2,5:W:F", + "2,0:N:F:0", + "1,1:N:F:0", + "0,2:N:IA:0", + "0,3:W:IA:0", + "1,4:W:F:0", + "2,5:W:F:0", // left rear cluster - "2,9:W:F", - "1,10:W:F", - "0,11:W:IA", - "0,11:S:IA", - "1,10:S:FW", - "2,9:S:FW" + "2,9:W:F:0", + "1,10:W:F:0", + "0,11:W:IA:0", + "0,11:S:IA:0", + "1,10:S:FW:0", + "2,9:S:FW:0" }; // and memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); @@ -191,14 +193,14 @@ TEST(LedStripTest, smallestGridWithCenter) // and static const ledConfig_t testLedConfigs[] = { - { CALCULATE_LED_XY( 2, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING } + { CALCULATE_LED_XY( 2, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 2), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING } }; memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs)); @@ -227,10 +229,10 @@ TEST(LedStripTest, smallestGrid) // and static const ledConfig_t testLedConfigs[] = { - { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, }; memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs)); @@ -252,45 +254,45 @@ TEST(LedStripTest, smallestGrid) } /* - { CALCULATE_LED_XY( 1, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 14), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 13), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 12), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 13), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 12), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 10), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 9), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 8), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 7), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 6), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 5), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 4), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 9), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 6), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 3), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 3, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 3, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 1), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 2), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 2), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 3), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 4), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 5), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 6), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 7), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 8), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 9), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 10), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 11), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 3), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 4), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 5), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 6), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 9), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 12), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 13), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 12), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 13), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 3, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 3, 14), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, */ @@ -345,6 +347,7 @@ uint8_t armingFlags = 0; uint16_t flightModeFlags = 0; int16_t rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +uint32_t rcModeActivationMask; batteryState_e calculateBatteryState(void) { return BATTERY_OK; From 6868999d636def7428845cc671746d3a7c248224 Mon Sep 17 00:00:00 2001 From: tracernz Date: Sun, 25 Jan 2015 19:27:58 +1300 Subject: [PATCH 120/182] Set default current meter type Current meter type added to resetBatteryConfig in config.c so it defaults to ADC. PWM mapping skips the current meter ADC pin only if used. --- src/main/config/config.c | 2 +- src/main/main.c | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 58f3a5ea87..ea8d512673 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -225,7 +225,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->currentMeterOffset = 0; batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A) batteryConfig->batteryCapacity = 0; - + batteryConfig->currentMeterType = CURRENT_SENSOR_ADC; } void resetSerialConfig(serialConfig_t *serialConfig) diff --git a/src/main/main.c b/src/main/main.c index 4a9656b8df..bcbd18412e 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -287,7 +287,8 @@ void init(void) 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); + 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); From 6376f9a8a87c6e24e70031a099469107f501a594 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sun, 25 Jan 2015 10:51:07 +0100 Subject: [PATCH 121/182] Enabled the outer package CRC check. --- src/main/rx/xbus.c | 63 ++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 58 insertions(+), 5 deletions(-) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index ed3015f6ac..c373e293eb 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -41,6 +41,7 @@ #define XBUS_FRAME_SIZE 27 #define XBUS_RJ01_FRAME_SIZE 33 +#define XBUS_RJ01_MESSAGE_LENGTH 30 #define XBUS_CRC_AND_VALUE 0x8000 #define XBUS_CRC_POLY 0x1021 @@ -146,6 +147,30 @@ static uint16_t xBusCRC16(uint16_t crc, uint8_t value) return crc; } +// Full RJ01 message CRC calculations +uint8_t xBusRj01CRC8(uint8_t inData, uint8_t seed) +{ + uint8_t bitsLeft; + uint8_t temp; + + for (bitsLeft = 8; bitsLeft > 0; bitsLeft--) { + temp = ((seed ^ inData) & 0x01); + + if (temp == 0) { + seed >>= 1; + } else { + seed ^= 0x18; + seed >>= 1; + seed |= 0x80; + } + + inData >>= 1; + } + + return seed; +} + + static void xBusUnpackModeBFrame(void) { // Calculate the CRC of the incoming frame @@ -184,6 +209,7 @@ static void xBusUnpackModeBFrame(void) static void xBusUnpackRJ01Frame(void) { // Calculate the CRC of the incoming frame + uint8_t outerCrc = 0; uint16_t crc = 0; uint16_t inCrc = 0; uint8_t i = 0; @@ -194,15 +220,42 @@ static void xBusUnpackRJ01Frame(void) // a MODE B setting in the radio (XG14 tested) // the MODE_B -frame is packed within some // at the moment unknown bytes before and after: - // 0xA1 __ __ 0xA1 12*(High + Low) CRC1 CRC2 + __ __ __ + // 0xA1 LEN __ 0xA1 12*(High + Low) CRC1 CRC2 + __ __ CRC_OUTER // Compared to a standard MODE B frame that only - // contains the "middle" package + // contains the "middle" package. // Hence, at the moment, the unknown header and footer - // of the RJ01 MODEB packages are discarded. This is - // ok as the CRC-checksum of the embedded package works - // out nicely. + // of the RJ01 MODEB packages are discarded. + // However, the LAST byte (CRC_OUTER) is infact an 8-bit + // CRC for the whole package, using the Dallas-One-Wire CRC + // method. + // So, we check both these values as well as the provided length + // of the outer/full message (LEN) + + // + // Check we have correct length of message + // + if (xBusFrame[1] != XBUS_RJ01_MESSAGE_LENGTH) + { + // Unknown package as length is not ok + return; + } + + // + // CRC calculation & check for full message + // + for (i = 0; i < xBusFrameLength - 1; i++) { + outerCrc = xBusRj01CRC8(outerCrc, xBusFrame[i]); + } + + if (outerCrc != xBusFrame[xBusFrameLength - 1]) + { + // CRC does not match, skip this frame + return; + } + // // Calculate CRC bytes of the "embedded MODE B frame" + // for (i = 3; i < xBusFrameLength - 5; i++) { inCrc = xBusCRC16(inCrc, xBusFrame[i]); } From 5c96d36442c269ad605a4b72e09f15e0c406c22c Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sun, 25 Jan 2015 13:47:45 +0100 Subject: [PATCH 122/182] Refactored the frame unpack to be used by both XBUS protocols. --- src/main/rx/xbus.c | 47 ++++++++++------------------------------------ 1 file changed, 10 insertions(+), 37 deletions(-) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index c373e293eb..9730e26b2d 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -42,6 +42,7 @@ #define XBUS_RJ01_FRAME_SIZE 33 #define XBUS_RJ01_MESSAGE_LENGTH 30 +#define XBUS_RJ01_OFFSET_BYTES 3 #define XBUS_CRC_AND_VALUE 0x8000 #define XBUS_CRC_POLY 0x1021 @@ -171,7 +172,7 @@ uint8_t xBusRj01CRC8(uint8_t inData, uint8_t seed) } -static void xBusUnpackModeBFrame(void) +static void xBusUnpackModeBFrame(uint8_t offsetBytes) { // Calculate the CRC of the incoming frame uint16_t crc = 0; @@ -181,19 +182,19 @@ static void xBusUnpackModeBFrame(void) uint8_t frameAddr; // Calculate on all bytes except the final two CRC bytes - for (i = 0; i < xBusFrameLength - 2; i++) { - inCrc = xBusCRC16(inCrc, xBusFrame[i]); + for (i = 0; i < XBUS_FRAME_SIZE - 2; i++) { + inCrc = xBusCRC16(inCrc, xBusFrame[i+offsetBytes]); } // Get the received CRC - crc = ((uint16_t)xBusFrame[xBusFrameLength-2]) << 8; - crc = crc + ((uint16_t)xBusFrame[xBusFrameLength-1]); + crc = ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 2]) << 8; + crc = crc + ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 1]); if (crc == inCrc) { // Unpack the data, we have a valid frame for (i = 0; i < xBusChannelCount; i++) { - frameAddr = 1 + i * 2; + frameAddr = offsetBytes + 1 + i * 2; value = ((uint16_t)xBusFrame[frameAddr]) << 8; value = value + ((uint16_t)xBusFrame[frameAddr + 1]); @@ -210,11 +211,7 @@ static void xBusUnpackRJ01Frame(void) { // Calculate the CRC of the incoming frame uint8_t outerCrc = 0; - uint16_t crc = 0; - uint16_t inCrc = 0; uint8_t i = 0; - uint16_t value; - uint8_t frameAddr; // When using the Align RJ01 receiver with // a MODE B setting in the radio (XG14 tested) @@ -253,32 +250,8 @@ static void xBusUnpackRJ01Frame(void) return; } - // - // Calculate CRC bytes of the "embedded MODE B frame" - // - for (i = 3; i < xBusFrameLength - 5; i++) { - inCrc = xBusCRC16(inCrc, xBusFrame[i]); - } - - // Get the received CRC (of the "embedded MODE B frame") - crc = ((uint16_t)xBusFrame[xBusFrameLength-5]) << 8; - crc = crc + ((uint16_t)xBusFrame[xBusFrameLength-4]); - - if (crc == inCrc) { - // Unpack the data, we have a valid frame - for (i = 0; i < xBusChannelCount; i++) { - - frameAddr = 4 + i * 2; - value = ((uint16_t)xBusFrame[frameAddr]) << 8; - value = value + ((uint16_t)xBusFrame[frameAddr + 1]); - - // Convert to internal format - xBusChannelData[i] = XBUS_CONVERT_TO_USEC(value); - } - - xBusFrameReceived = true; - } - + // Now unpack the "embedded MODE B frame" + xBusUnpackModeBFrame(XBUS_RJ01_OFFSET_BYTES); } // Receive ISR callback @@ -312,7 +285,7 @@ static void xBusDataReceive(uint16_t c) if (xBusFramePosition == xBusFrameLength) { switch (xBusProvider) { case SERIALRX_XBUS_MODE_B: - xBusUnpackModeBFrame(); + xBusUnpackModeBFrame(0); case SERIALRX_XBUS_MODE_B_RJ01: xBusUnpackRJ01Frame(); } From 5760519a032f657c3e982eb687acf18fede49829 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 25 Jan 2015 13:59:48 +0100 Subject: [PATCH 123/182] CC3D - Adding USB Virtual COM port suport. You can now connect the configurator via the USB port. --- Makefile | 45 ++++++++++++++++++++++------------- src/main/io/serial.c | 6 +++++ src/main/target/CC3D/target.h | 3 ++- src/main/vcp/hw_config.c | 15 ++++++++---- src/main/vcp/hw_config.h | 6 +++++ 5 files changed, 54 insertions(+), 21 deletions(-) diff --git a/Makefile b/Makefile index c4197a7729..f818e81071 100644 --- a/Makefile +++ b/Makefile @@ -53,13 +53,13 @@ LINKER_DIR = $(ROOT)/src/main/target # Search path for sources VPATH := $(SRC_DIR):$(SRC_DIR)/startup +USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver +USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c)) ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY)) STDPERIPH_DIR = $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver -USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver -USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c)) STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) EXCLUDES = stm32f30x_crc.c \ @@ -155,14 +155,26 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(CMSIS_DIR)/CM3/CoreSupport \ $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x \ +DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC) + +ifeq ($(TARGET),CC3D) +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(USBFS_DIR)/inc \ + $(ROOT)/src/main/vcp + +VPATH := $(VPATH):$(USBFS_DIR)/src + +DEVICE_STDPERIPH_SRC := $(DEVICE_STDPERIPH_SRC) \ + $(USBPERIPH_SRC) + +endif + LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f103_128k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m3 TARGET_FLAGS = -D$(TARGET) -pedantic DEVICE_FLAGS = -DSTM32F10X_MD -DSTM32F10X -DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC) - endif TARGET_DIR = $(ROOT)/src/main/target/$(TARGET) @@ -233,11 +245,21 @@ HIGHEND_SRC = flight/autotune.c \ telemetry/frsky.c \ telemetry/hott.c \ telemetry/msp.c \ - telemetry/smartport.c \ + telemetry/smartport.c \ sensors/sonar.c \ sensors/barometer.c \ blackbox/blackbox.c +VCP_SRC = \ + vcp/hw_config.c \ + vcp/stm32_it.c \ + vcp/usb_desc.c \ + vcp/usb_endp.c \ + vcp/usb_istr.c \ + vcp/usb_prop.c \ + vcp/usb_pwr.c \ + drivers/serial_usb_vcp.c + NAZE_SRC = startup_stm32f10x_md_gcc.S \ drivers/accgyro_adxl345.c \ drivers/accgyro_bma280.c \ @@ -403,7 +425,8 @@ CC3D_SRC = \ drivers/timer.c \ drivers/timer_stm32f10x.c \ $(HIGHEND_SRC) \ - $(COMMON_SRC) + $(COMMON_SRC) \ + $(VCP_SRC) STM32F30x_COMMON_SRC = \ startup_stm32f30x_md_gcc.S \ @@ -425,16 +448,6 @@ STM32F30x_COMMON_SRC = \ drivers/timer.c \ drivers/timer_stm32f30x.c -VCP_SRC = \ - vcp/hw_config.c \ - vcp/stm32_it.c \ - vcp/usb_desc.c \ - vcp/usb_endp.c \ - vcp/usb_istr.c \ - vcp/usb_prop.c \ - vcp/usb_pwr.c \ - drivers/serial_usb_vcp.c - NAZE32PRO_SRC = \ $(STM32F30x_COMMON_SRC) \ $(HIGHEND_SRC) \ diff --git a/src/main/io/serial.c b/src/main/io/serial.c index e9e9a47122..6a3d974b2b 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -106,12 +106,18 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { #ifdef CC3D static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { +#ifdef USE_VCP + {SERIAL_PORT_USB_VCP, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, +#endif {SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, {SERIAL_PORT_USART3, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, {SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_UNUSED, FUNCTION_NONE} }; const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { +#ifdef USE_VCP + {SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE }, +#endif {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, {SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, {SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE} diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 26a1358c61..a06d3540a6 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -60,10 +60,11 @@ #define BEEPER #define DISPLAY +#define USE_VCP #define USE_USART1 #define USE_USART3 #define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 3 +#define SERIAL_PORT_COUNT 4 #define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 1 // PWM 2 diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c index 59cb84eb79..cb0600d132 100644 --- a/src/main/vcp/hw_config.c +++ b/src/main/vcp/hw_config.c @@ -92,11 +92,11 @@ void Set_System(void) GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; // HJI GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; // HJI #else - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); // HJI + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); // HJI - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;// HJI - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;// HJI - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;// HJI + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;// HJI + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;// HJI + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;// HJI #endif GPIO_Init(GPIOA, &GPIO_InitStructure); // HJI @@ -123,6 +123,13 @@ void Set_System(void) GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_14); #endif /* STM32F37X && STM32F303xC)*/ +#if defined(STM32F10X) + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; + GPIO_Init(GPIOA, &GPIO_InitStructure); +#endif /* Configure the EXTI line 18 connected internally to the USB IP */ EXTI_ClearITPendingBit(EXTI_Line18); diff --git a/src/main/vcp/hw_config.h b/src/main/vcp/hw_config.h index f077f6ddb5..ee40ed7085 100644 --- a/src/main/vcp/hw_config.h +++ b/src/main/vcp/hw_config.h @@ -32,7 +32,13 @@ /* Includes ------------------------------------------------------------------*/ //#include "platform_config.h" #include "usb_type.h" +#ifdef STM32F303 #include "stm32f30x.h" +#endif + +#ifdef STM32F10X +#include "stm32f10x.h" +#endif /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ From 0db1807fec7d80020f6e623d7547afd0d425ee7f Mon Sep 17 00:00:00 2001 From: Marc Egli Date: Sun, 25 Jan 2015 16:17:35 +0100 Subject: [PATCH 124/182] fix the length of some msp messages --- src/main/io/serial_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 2b8ca9f64e..041a01c5f3 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1024,7 +1024,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_BOARD_ALIGNMENT: - headSerialReply(3); + headSerialReply(6); serialize16(masterConfig.boardAlignment.rollDegrees); serialize16(masterConfig.boardAlignment.pitchDegrees); serialize16(masterConfig.boardAlignment.yawDegrees); @@ -1042,7 +1042,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_RX_CONFIG: - headSerialReply(7); + headSerialReply(8); serialize8(masterConfig.rxConfig.serialrx_provider); serialize16(masterConfig.rxConfig.maxcheck); serialize16(masterConfig.rxConfig.midrc); From 52f082fcd719d26edb2135da4dec505b8f071645 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 25 Jan 2015 16:03:13 +0100 Subject: [PATCH 125/182] LedStrip - Add support for solid colors. Update default LED strip configuration to include a ring. Also includes documentation updates that were not updated when ring support was added. --- docs/LedStrip.md | 282 +++++++++++++++++++---------- src/main/io/ledstrip.c | 59 ++++-- src/main/io/ledstrip.h | 5 +- src/test/unit/ledstrip_unittest.cc | 43 +++-- 4 files changed, 258 insertions(+), 131 deletions(-) diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 4305c99075..dffc9d99f6 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -84,11 +84,11 @@ If you enable LED_STRIP feature and the feature is turned off again after a rebo Configure the LEDs using the `led` command. -The `led` command takes either zero or two arguments - an zero-based led number and a pair of coordinates, direction flags and mode flags. +The `led` command takes either zero or two arguments - an zero-based led number and a sequence which indicates pair of coordinates, direction flags and mode flags and a color. If used with zero arguments it prints out the led configuration which can be copied for future reference. -Each led is configured using the following template: `x,y:ddd:mmm` +Each led is configured using the following template: `x,y:ddd:mmm:cc` `x` and `y` are grid coordinates of a 0 based 16x16 grid, north west is 0,0, south east is 15,15 `ddd` specifies the directions, since an led can face in any direction it can have multiple directions. Directions are: @@ -112,22 +112,28 @@ Note: It is perfectly possible to configure an LED to have all directions `NESWU * `A` - `A`rmed state. * `T` - `T`hrust state. * `R` - `R`ing thrust state. +* `C` - `C`olor. + +`cc` specifies the color number (0 based index). Example: ``` -led 0 0,15:SD:IAW -led 1 15,0:ND:IAW -led 2 0,0:ND:IAW -led 3 0,15:SD:IAW +led 0 0,15:SD:IAW:0 +led 1 15,0:ND:IAW:0 +led 2 0,0:ND:IAW:0 +led 3 0,15:SD:IAW:0 +led 4 7,7::C:1 +led 5 8,8::C:2 ``` -to erase an led, and to mark the end of the chain, use `0,0::` as the second argument, like this: +To erase an led, and to mark the end of the chain, use `0,0::` as the second argument, like this: ``` -led 4 0,0:: +led 4 0,0::: ``` +It is best to erase all LEDs that you do not have connected. ### Modes @@ -156,6 +162,8 @@ LEDs are set in a specific order: That is, south facing LEDs have priority. +The mapping between modes led placement and colors is currently fixed and cannot be changed. + #### Indicator This mode flashes LEDs that correspond to roll and pitch stick positions. i.e. they indicate the direction the craft is going to turn. @@ -180,7 +188,7 @@ A better effect is acheived when LEDs configured for thrust ring have no other f LED direction and X/Y positions are irrelevant for thrust ring LED state. The order of the LEDs that have the state determines how the LED behaves. -Each LED of the ring can be a different color. The color can be selected between the 15 colors availables. +Each LED of the ring can be a different color. The color can be selected between the 16 colors availables. For example, led 0 is set as a `R`ing thrust state led in color 13 as follow. @@ -190,6 +198,70 @@ led 0 2,2::R:13 LED strips and rings can be combined. +#### Solid Color + +The mode allows you to set an LED to be permanently on and set to a specific color. + +x,y position and directions are ignored when using this mode. + +Other modes will override or combine with the color mode. + +For example, to set led 0 to always use color 10 you would issue this command. + +``` +led 0 0,0::C:10 +``` + +### Colors + +Colors can be configured using the cli `color` command. + +The `color` command takes either zero or two arguments - an zero-based color number and a sequence which indicates pair of hue, saturation and value (HSV). + +See http://en.wikipedia.org/wiki/HSL_and_HSV + +If used with zero arguments it prints out the color configuration which can be copied for future reference. + +The default color configuration is as follows: + +| Index | Color | +| ----- | ----------- | +| 0 | black | +| 1 | white | +| 2 | red | +| 3 | orange | +| 4 | yellow | +| 5 | lime green | +| 6 | green | +| 7 | mint green | +| 8 | cyan | +| 9 | light blue | +| 10 | blue | +| 11 | dark violet | +| 12 | magenta | +| 13 | deep pink | +| 14 | black | +| 15 | black | + +``` +color 0 0,0,0 +color 1 0,255,255 +color 2 0,0,255 +color 3 30,0,255 +color 4 60,0,255 +color 5 90,0,255 +color 6 120,0,255 +color 7 150,0,255 +color 8 180,0,255 +color 9 210,0,255 +color 10 240,0,255 +color 11 270,0,255 +color 12 300,0,255 +color 13 330,0,255 +color 14 0,0,0 +color 15 0,0,0 +``` + ## Positioning Cut the strip into sections as per diagrams below. When the strips are cut ensure you reconnect each output to each input with cable where the break is made. @@ -201,128 +273,150 @@ Orientation is when viewed with the front of the aircraft facing away from you a The default configuration is as follows ``` -led 0 2,2:ES:IA -led 1 2,1:E:WF -led 2 2,0:NE:IA -led 3 1,0:N:F -led 4 0,0:NW:IA -led 5 0,1:W:WF -led 6 0,2:SW:IA -led 7 1,2:S:WF -led 8 1,1:U:WF -led 9 1,1:U:WF -led 10 1,1:D:WF -led 11 1,1:D:WF +led 0 15,15:ES:IA:0 +led 1 15,8:E:WF:0 +led 2 15,7:E:WF:0 +led 3 15,0:NE:IA:0 +led 4 8,0:N:F:0 +led 5 7,0:N:F:0 +led 6 0,0:NW:IA:0 +led 7 0,7:W:WF:0 +led 8 0,8:W:WF:0 +led 9 0,15:SW:IA:0 +led 10 7,15:S:WF:0 +led 11 8,15:S:WF:0 +led 12 7,7:U:WF:0 +led 13 8,7:U:WF:0 +led 14 7,8:D:WF:0 +led 15 8,8:D:WF:0 +led 16 8,9::R:3 +led 17 9,10::R:3 +led 18 10,11::R:3 +led 19 10,12::R:3 +led 20 9,13::R:3 +led 21 8,14::R:3 +led 22 7,14::R:3 +led 23 6,13::R:3 +led 24 5,12::R:3 +led 25 5,11::R:3 +led 26 6,10::R:3 +led 27 7,9::R:3 +led 28 0,0:::0 +led 29 0,0:::0 +led 30 0,0:::0 +led 31 0,0:::0 ``` Which translates into the following positions: ``` - 5 3 - \ / - \ 4 / - \ FRONT / - 6 | 9-12 | 2 - / BACK \ - / 8 \ - / \ - 7 1 + 6 3 + \ / + \ 5-4 / + \ FRONT / + 7,8 | 12-15 | 1,2 + / BACK \ + / 10,11 \ + / \ + 9 0 + RING 16-27 ``` -LEDs 1,3,5 and 7 should be placed underneath the quad, facing downwards. -LEDs 2, 4, 6 and 8 should be positioned so the face east/north/west/south, respectively. -LEDs 9-10 should be placed facing down, in the middle -LEDs 11-12 should be placed facing up, in the middle +LEDs 0,3,6 and 9 should be placed underneath the quad, facing downwards. +LEDs 1-2, 4-5, 7-8 and 10-11 should be positioned so the face east/north/west/south, respectively. +LEDs 12-13 should be placed facing down, in the middle +LEDs 14-15 should be placed facing up, in the middle +LEDs 16-17 should be placed in a ring and positioned at the rear facing south. -This is the default so that if you don't want to place LEDs top and bottom in the middle just connect the first 8 LEDs. +This is the default so that if you don't want to place LEDs top and bottom in the middle just connect the first 12 LEDs. ### Example 16 LED config ``` -led 0 15,15:SD:IA -led 1 8,8:E:FW -led 2 8,7:E:FW -led 3 15,0:ND:IA -led 4 7,7:N:FW -led 5 8,7:N:FW -led 6 0,0:ND:IA -led 7 7,7:W:FW -led 8 7,8:W:FW -led 9 0,15:SD:IA -led 10 7,8:S:FW -led 11 8,8:S:FW -led 12 7,7:D:FW -led 13 8,7:D:FW -led 14 7,7:U:FW -led 15 8,7:U:FW +led 0 15,15:SD:IA:0 +led 1 8,8:E:FW:0 +led 2 8,7:E:FW:0 +led 3 15,0:ND:IA:0 +led 4 7,7:N:FW:0 +led 5 8,7:N:FW:0 +led 6 0,0:ND:IA:0 +led 7 7,7:W:FW:0 +led 8 7,8:W:FW:0 +led 9 0,15:SD:IA:0 +led 10 7,8:S:FW:0 +led 11 8,8:S:FW:0 +led 12 7,7:D:FW:0 +led 13 8,7:D:FW:0 +led 14 7,7:U:FW:0 +led 15 8,7:U:FW:0 ``` Which translates into the following positions: ``` - 7 4 + 6 3 \ / - \ 6-5 / - 8 \ FRONT / 3 - | 13-16 | - 9 / BACK \ 2 - / 11-12 \ + \ 5-4 / + 7 \ FRONT / 2 + | 12-15 | + 8 / BACK \ 1 + / 10-11 \ / \ - 10 1 + 9 0 ``` -LEDs 1,4,7 and 10 should be placed underneath the quad, facing downwards. -LEDs 2-3, 6-5, 8-9 and 11-12 should be positioned so the face east/north/west/south, respectively. -LEDs 13-14 should be placed facing down, in the middle -LEDs 15-16 should be placed facing up, in the middle +LEDs 0,3,6 and 9 should be placed underneath the quad, facing downwards. +LEDs 1-2, 4-5, 7-8 and 10-11 should be positioned so the face east/north/west/south, respectively. +LEDs 12-13 should be placed facing down, in the middle +LEDs 14-15 should be placed facing up, in the middle ### Exmple 28 LED config ``` #right rear cluster -led 0 9,9:S:FWT -led 1 10,10:S:FWT -led 2 11,11:S:IA -led 3 11,11:E:IA -led 4 10,10:E:AT -led 5 9,9:E:AT +led 0 9,9:S:FWT:0 +led 1 10,10:S:FWT:0 +led 2 11,11:S:IA:0 +led 3 11,11:E:IA:0 +led 4 10,10:E:AT:0 +led 5 9,9:E:AT:0 # right front cluster -led 6 10,5:S:F -led 7 11,4:S:F -led 8 12,3:S:IA -led 9 12,2:N:IA -led 10 11,1:N:F -led 11 10,0:N:F +led 6 10,5:S:F:0 +led 7 11,4:S:F:0 +led 8 12,3:S:IA:0 +led 9 12,2:N:IA:0 +led 10 11,1:N:F:0 +led 11 10,0:N:F:0 # center front cluster -led 12 7,0:N:FW -led 13 6,0:N:FW -led 14 5,0:N:FW -led 15 4,0:N:FW +led 12 7,0:N:FW:0 +led 13 6,0:N:FW:0 +led 14 5,0:N:FW:0 +led 15 4,0:N:FW:0 # left front cluster -led 16 2,0:N:F -led 17 1,1:N:F -led 18 0,2:N:IA -led 19 0,3:W:IA -led 20 1,4:S:F -led 21 2,5:S:F +led 16 2,0:N:F:0 +led 17 1,1:N:F:0 +led 18 0,2:N:IA:0 +led 19 0,3:W:IA:0 +led 20 1,4:S:F:0 +led 21 2,5:S:F:0 # left rear cluster -led 22 2,9:W:AT -led 23 1,10:W:AT -led 24 0,11:W:IA -led 25 0,11:S:IA -led 26 1,10:S:FWT -led 27 2,9:S:FWT +led 22 2,9:W:AT:0 +led 23 1,10:W:AT:0 +led 24 0,11:W:IA:0 +led 25 0,11:S:IA:0 +led 26 1,10:S:FWT:0 +led 27 2,9:S:FWT:0 ``` ``` - 17-19 10-12 -20-22 \ / 7-9 + 16-18 9-11 +19-21 \ / 6-8 \ 13-16 / \ FRONT / / BACK \ / \ -23-25 / \ 4-6 - 26-28 1-3 +22-24 / \ 3-5 + 25-27 0-2 ``` All LEDs should face outwards from the chassis in this configuration. diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 53a95118cc..34adfbf50d 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -250,18 +250,44 @@ const ledConfig_t defaultLedStripConfig[] = { }; #else const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 2, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(15, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + { CALCULATE_LED_XY(15, 8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY(15, 7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + + { CALCULATE_LED_XY(15, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + { CALCULATE_LED_XY( 8, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 2), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + + { CALCULATE_LED_XY( 0, 7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + + { CALCULATE_LED_XY( 0, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + { CALCULATE_LED_XY( 7, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 8, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + + { CALCULATE_LED_XY( 7, 7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 8, 7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 7, 8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 8, 8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + + { CALCULATE_LED_XY( 8, 9), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 9, 10), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY(10, 11), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY(10, 12), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 9, 13), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 8, 14), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 7, 14), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 6, 13), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 5, 12), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 5, 11), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 6, 10), 3, LED_FUNCTION_THRUST_RING}, + { CALCULATE_LED_XY( 7, 9), 3, LED_FUNCTION_THRUST_RING}, + }; #endif @@ -300,7 +326,7 @@ static const uint8_t directionMappings[DIRECTION_COUNT] = { LED_DIRECTION_DOWN }; -static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R' }; +static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R', 'C' }; #define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) static const uint16_t functionMappings[FUNCTION_COUNT] = { LED_FUNCTION_INDICATOR, @@ -308,7 +334,8 @@ static const uint16_t functionMappings[FUNCTION_COUNT] = { LED_FUNCTION_FLIGHT_MODE, LED_FUNCTION_ARM_STATE, LED_FUNCTION_THROTTLE, - LED_FUNCTION_THRUST_RING + LED_FUNCTION_THRUST_RING, + LED_FUNCTION_COLOR }; // grid offsets @@ -576,7 +603,11 @@ void applyLedModeLayer(void) ledConfig = &ledConfigs[ledIndex]; if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - setLedHsv(ledIndex, &hsv_black); + if (ledConfig->flags & LED_FUNCTION_COLOR) { + setLedHsv(ledIndex, &colors[ledConfig->color]); + } else { + setLedHsv(ledIndex, &hsv_black); + } } if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index c62dd4c4ce..99d0397646 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -48,6 +48,7 @@ typedef enum { LED_FUNCTION_ARM_STATE = (1 << 9), LED_FUNCTION_THROTTLE = (1 << 10), LED_FUNCTION_THRUST_RING = (1 << 11), + LED_FUNCTION_COLOR = (1 << 12), } ledFlag_e; #define LED_DIRECTION_BIT_OFFSET 0 @@ -66,7 +67,8 @@ typedef enum { LED_FUNCTION_FLIGHT_MODE | \ LED_FUNCTION_ARM_STATE | \ LED_FUNCTION_THROTTLE | \ - LED_FUNCTION_THRUST_RING \ + LED_FUNCTION_THRUST_RING | \ + LED_FUNCTION_COLOR \ ) @@ -77,6 +79,7 @@ typedef struct ledConfig_s { } ledConfig_t; extern uint8_t ledCount; +extern uint8_t ledsInRingCount; diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index a1e86d20d7..8ad6e31442 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -58,14 +58,6 @@ extern "C" { TEST(LedStripTest, parseLedStripConfig) { - /* - * 0..5 - rear right cluster, 0..2 rear 3..5 right - * 6..11 - front right cluster, 6..8 rear, 9..11 front - * 12..15 - front center cluster - * 16..21 - front left cluster, 16..18 front, 19..21 rear - * 22..27 - rear left cluster, 22..24 left, 25..27 rear - */ - // given static const ledConfig_t expectedLedStripConfig[WS2811_LED_STRIP_LENGTH] = { { CALCULATE_LED_XY( 9, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, @@ -73,7 +65,6 @@ TEST(LedStripTest, parseLedStripConfig) { CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, { CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, { CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 9, 9), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, { CALCULATE_LED_XY(10, 5), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, { CALCULATE_LED_XY(11, 4), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, @@ -83,8 +74,8 @@ TEST(LedStripTest, parseLedStripConfig) { CALCULATE_LED_XY(10, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, { CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 6, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 5, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 6, 0), 1, LED_DIRECTION_NORTH | LED_FUNCTION_COLOR | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 5, 0), 1, LED_DIRECTION_NORTH | LED_FUNCTION_COLOR | LED_FUNCTION_WARNING }, { CALCULATE_LED_XY( 4, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, @@ -94,17 +85,19 @@ TEST(LedStripTest, parseLedStripConfig) { CALCULATE_LED_XY( 1, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, { CALCULATE_LED_XY( 2, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 2, 9), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, { CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, { CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, { CALCULATE_LED_XY( 2, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { 0, 0 }, - { 0, 0 }, - { 0, 0 }, - { 0, 0 }, + { CALCULATE_LED_XY( 7, 7), 14, LED_FUNCTION_THRUST_RING }, + { CALCULATE_LED_XY( 8, 7), 15, LED_FUNCTION_THRUST_RING }, + { CALCULATE_LED_XY( 8, 8), 14, LED_FUNCTION_THRUST_RING }, + { CALCULATE_LED_XY( 7, 8), 15, LED_FUNCTION_THRUST_RING }, + + { 0, 0, 0 }, + { 0, 0, 0 }, }; // and @@ -117,7 +110,6 @@ TEST(LedStripTest, parseLedStripConfig) "11,11:S:IA:0", "11,11:E:IA:0", "10,10:E:F:0", - "9,9:E:F:0", // right front cluster "10,5:S:F:0", @@ -129,8 +121,8 @@ TEST(LedStripTest, parseLedStripConfig) // center front cluster "7,0:N:FW:0", - "6,0:N:FW:0", - "5,0:N:FW:0", + "6,0:N:CW:1", + "5,0:N:CW:1", "4,0:N:FW:0", // left front cluster @@ -142,12 +134,17 @@ TEST(LedStripTest, parseLedStripConfig) "2,5:W:F:0", // left rear cluster - "2,9:W:F:0", "1,10:W:F:0", "0,11:W:IA:0", "0,11:S:IA:0", "1,10:S:FW:0", - "2,9:S:FW:0" + "2,9:S:FW:0", + + // thrust ring + "7,7::R:14", + "8,7::R:15", + "8,8::R:14", + "7,8::R:15" }; // and memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); @@ -163,7 +160,8 @@ TEST(LedStripTest, parseLedStripConfig) // then EXPECT_EQ(true, ok); - EXPECT_EQ(28, ledCount); + EXPECT_EQ(30, ledCount); + EXPECT_EQ(4, ledsInRingCount); // and @@ -172,6 +170,7 @@ TEST(LedStripTest, parseLedStripConfig) EXPECT_EQ(expectedLedStripConfig[index].xy, ledConfigs[index].xy); EXPECT_EQ(expectedLedStripConfig[index].flags, ledConfigs[index].flags); + EXPECT_EQ(expectedLedStripConfig[index].color, ledConfigs[index].color); } // then From 99089b9e70107269d171f05b3590b1548b65ae8d Mon Sep 17 00:00:00 2001 From: Marc Egli Date: Sun, 25 Jan 2015 16:45:41 +0100 Subject: [PATCH 126/182] add new msp messages for voltage meter and enhance messages for current meter --- src/main/io/serial_msp.c | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 041a01c5f3..7c75a05bcc 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -200,6 +200,9 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_CF_SERIAL_CONFIG 54 #define MSP_SET_CF_SERIAL_CONFIG 55 +#define MSP_VOLTAGE_METER_CONFIG 56 +#define MSP_SET_VOLTAGE_METER_CONFIG 57 + // // Baseflight MSP commands (if enabled they exist in Cleanflight) // @@ -1030,10 +1033,20 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.boardAlignment.yawDegrees); break; - case MSP_CURRENT_METER_CONFIG: + case MSP_VOLTAGE_METER_CONFIG: headSerialReply(4); + serialize8(masterConfig.batteryConfig.vbatscale); + serialize8(masterConfig.batteryConfig.vbatmincellvoltage); + serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage); + serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage); + break; + + case MSP_CURRENT_METER_CONFIG: + headSerialReply(7); serialize16(masterConfig.batteryConfig.currentMeterScale); serialize16(masterConfig.batteryConfig.currentMeterOffset); + serialize8(0); // current meter type + serialize16(masterConfig.batteryConfig.batteryCapacity); break; case MSP_MIXER: @@ -1370,9 +1383,18 @@ static bool processInCommand(void) masterConfig.boardAlignment.yawDegrees = read16(); break; + case MSP_SET_VOLTAGE_METER_CONFIG: + masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended + masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI + masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI + masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert + break; + case MSP_SET_CURRENT_METER_CONFIG: masterConfig.batteryConfig.currentMeterScale = read16(); masterConfig.batteryConfig.currentMeterOffset = read16(); + read(8); // current meter type + masterConfig.batteryConfig.batteryCapacity = read16(); break; #ifndef USE_QUAD_MIXER_ONLY From 8d4f47ff35224e56ecd903ff4a15c2809d8926e3 Mon Sep 17 00:00:00 2001 From: Pierre Hugo Date: Sun, 25 Jan 2015 14:26:59 -0800 Subject: [PATCH 127/182] Added tests for inclination angle. --- src/main/flight/altitudehold.c | 6 ++++- src/test/unit/altitude_hold_unittest.cc | 31 +++++++++++++++++++++++++ 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index f74a32c73e..5fa40d624b 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -175,12 +175,16 @@ bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination) return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; } +/* +* This (poorly named) function merely returns whichever is higher, roll inclination or pitch inclination. +* //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft +* (my best interpretation of scalar 'tiltAngle') or rename the function. +*/ int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination) { return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees)); } - int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old) { int32_t result = 0; diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc index b2e1e328cc..bc2b7e4677 100644 --- a/src/test/unit/altitude_hold_unittest.cc +++ b/src/test/unit/altitude_hold_unittest.cc @@ -54,6 +54,7 @@ extern "C" { extern "C" { bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations); + uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations); } typedef struct inclinationExpectation_s { @@ -90,6 +91,36 @@ TEST(AltitudeHoldTest, IsThrustFacingDownwards) } } +typedef struct inclinationAngleExpectations_s { + rollAndPitchInclination_t inclination; + uint16_t expected_angle; +} inclinationAngleExpectations_t; + +TEST(AltitudeHoldTest, TestCalculateTiltAngle) +{ + inclinationAngleExpectations_t inclinationAngleExpectations[] = { + { {0, 0}, 0}, + { {1, 0}, 1}, + { {0, 1}, 1}, + { {0, -1}, 1}, + { {-1, 0}, 1}, + { {-1, -2}, 2}, + { {-2, -1}, 2}, + { {1, 2}, 2}, + { {2, 1}, 2} + }; + + rollAndPitchInclination_t inclination = {0, 0}; + uint16_t tilt_angle = calculateTiltAngle(&inclination); + EXPECT_EQ(tilt_angle, 0); + + for (uint8_t i = 0; i < 9; i++) { + inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i]; + uint16_t result = calculateTiltAngle(&expectation->inclination); + EXPECT_EQ(expectation->expected_angle, result); + } +} + // STUBS extern "C" { From 0590a6e3af6801d9bfa1ea61df80204b9a790a83 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 26 Jan 2015 00:23:01 +0100 Subject: [PATCH 128/182] Fixing transposed variable names (acceleroto) --- src/main/flight/autotune.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 7ecf5a80bf..e47437f881 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -129,7 +129,7 @@ static autotunePhase_e nextPhase = FIRST_TUNE_PHASE; static float targetAngle = 0; static float targetAngleAtPeak; -static float secondPeakAngle, firstPeakAngle; // deci dgrees, 180 deg = 1800 +static float firstPeakAngle, secondPeakAngle; // deci dgrees, 180 deg = 1800 typedef struct fp_pid { float p; @@ -171,7 +171,7 @@ static void autotuneLogCycleStart() static void startNewCycle(void) { rising = !rising; - secondPeakAngle = firstPeakAngle = 0; + firstPeakAngle = secondPeakAngle = 0; #ifdef BLACKBOX autotuneLogCycleStart(); @@ -229,18 +229,18 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin debug[2] = DEGREES_TO_DECIDEGREES(targetAngle); #endif - if (firstPeakAngle == 0) { + if (secondPeakAngle == 0) { // The peak will be when our angular velocity is negative. To be sure we are in the right place, // we also check to make sure our angle position is greater than zero. - if (currentAngle > secondPeakAngle) { + if (currentAngle > firstPeakAngle) { // we are still going up - secondPeakAngle = currentAngle; + firstPeakAngle = currentAngle; targetAngleAtPeak = targetAngle; - debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle); + debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle); - } else if (secondPeakAngle > 0) { + } else if (firstPeakAngle > 0) { switch (cycle) { case CYCLE_TUNE_I: // when checking the I value, we would like to overshoot the target position by half of the max oscillation. @@ -276,7 +276,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin case CYCLE_TUNE_PD2: // we are checking P and D values // set up to look for the 2nd peak - firstPeakAngle = currentAngle; + secondPeakAngle = currentAngle; timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS; break; } @@ -284,23 +284,23 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin } else { // We saw the first peak while tuning PD, looking for the second - if (currentAngle < firstPeakAngle) { - firstPeakAngle = currentAngle; - debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle); + if (currentAngle < secondPeakAngle) { + secondPeakAngle = currentAngle; + debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle); } - float oscillationAmplitude = secondPeakAngle - firstPeakAngle; + float oscillationAmplitude = firstPeakAngle - secondPeakAngle; uint32_t now = millis(); int32_t signedDiff = now - timeoutAt; bool timedOut = signedDiff >= 0L; // stop looking for the 2nd peak if we time out or if we change direction again after moving by more than half the maximum oscillation - if (timedOut || (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 && currentAngle > firstPeakAngle)) { + if (timedOut || (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 && currentAngle > secondPeakAngle)) { // analyze the data // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude - if (secondPeakAngle > targetAngleAtPeak) { + if (firstPeakAngle > targetAngleAtPeak) { // overshot #ifdef DEBUG_AUTOTUNE debug[0] = 1; @@ -339,7 +339,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin if (feature(FEATURE_BLACKBOX)) { flightLogEvent_autotuneCycleResult_t eventData; - eventData.overshot = secondPeakAngle > targetAngleAtPeak ? 1 : 0; + eventData.overshot = firstPeakAngle > targetAngleAtPeak ? 1 : 0; eventData.p = pidProfile->P8[pidIndex]; eventData.i = pidProfile->I8[pidIndex]; eventData.d = pidProfile->D8[pidIndex]; @@ -409,7 +409,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle rising = true; cycle = CYCLE_TUNE_PD; - secondPeakAngle = firstPeakAngle = 0; + firstPeakAngle = secondPeakAngle = 0; pidProfile = pidProfileToTune; pidController = pidControllerInUse; From febf80915f268b53359df4cd28df56ccf6a96c17 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 26 Jan 2015 00:26:40 +0100 Subject: [PATCH 129/182] Remove duplicated logic in autotune overshot detection and blackblox logging. Also allowed comments to be deleted. --- src/main/flight/autotune.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index e47437f881..b36168e6bf 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -300,8 +300,8 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin // analyze the data // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude - if (firstPeakAngle > targetAngleAtPeak) { - // overshot + bool overshot = firstPeakAngle > targetAngleAtPeak; + if (overshot) { #ifdef DEBUG_AUTOTUNE debug[0] = 1; #endif @@ -319,7 +319,6 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; #endif } else { - // undershot #ifdef DEBUG_AUTOTUNE debug[0] = 2; #endif @@ -339,7 +338,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin if (feature(FEATURE_BLACKBOX)) { flightLogEvent_autotuneCycleResult_t eventData; - eventData.overshot = firstPeakAngle > targetAngleAtPeak ? 1 : 0; + eventData.overshot = overshot; eventData.p = pidProfile->P8[pidIndex]; eventData.i = pidProfile->I8[pidIndex]; eventData.d = pidProfile->D8[pidIndex]; From ec3d85ae92d271c2fbdea43bcd2cd239c409be60 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Mon, 26 Jan 2015 16:56:23 +1300 Subject: [PATCH 130/182] Fix bug that allowed arming craft before gyro calibration completed --- src/main/mw.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index f7081090b1..12425863a8 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -151,8 +151,8 @@ void updateAutotuneState(void) bool isCalibrating() { #ifdef BARO - if (sensors(SENSOR_ACC) && !isBaroCalibrationComplete()) { - return false; + if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) { + return true; } #endif From 48161a31ca97231e756f5d0c1c60d36201d132ba Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 6 Jan 2015 18:26:27 +0100 Subject: [PATCH 131/182] MultiWii 2.3 PID controller will be activated with set pid_controller = 3 --- src/main/flight/flight.c | 93 ++++++++++++++++++++++++++++++++++++++++ src/main/io/serial_cli.c | 2 +- 2 files changed, 94 insertions(+), 1 deletion(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 7b994e5f6d..9899f65ad8 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -287,8 +287,98 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } +#define GYRO_P_MAX 300 #define GYRO_I_MAX 256 +static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, + rollAndPitchTrims_t *angleTrim) +{ + int axis, prop = 0; + int32_t rc, error, errorAngle; + int32_t delta; + static int32_t delta1[2], delta2[2]; + int32_t PTerm, ITerm, DTerm; + int32_t PTermACC, ITermACC; + static int16_t lastGyro[2] = {0,0}; + static int16_t errorAngleI[2] = {0,0}; + static int32_t errorGyroI_YAW; + + if (FLIGHT_MODE(HORIZON_MODE)) prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 512); + + // PITCH & ROLL + for(axis = 0; axis < 2; axis++) { + rc = rcCommand[axis] << 1; + error = rc - gyroData[axis]; + errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here + if (abs(gyroData[axis]) > 640) errorGyroI[axis] = 0; + + ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 + + PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6; + + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // axis relying on ACC + // 50 degrees max inclination +#ifdef GPS + errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#else + errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclnation), + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#endif + +#ifdef AUTOTUNE + if (shouldAutotune()) { + errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); + } +#endif + + errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here + + PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result + + int16_t limit = pidProfile->D8[PIDLEVEL] * 5; + PTermACC = constrain(PTermACC, -limit, +limit); + + ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result + + ITerm = ITermACC + ((ITerm-ITermACC) * prop >> 9); + PTerm = PTermACC + ((PTerm-PTermACC) * prop >> 9); + } + + PTerm -= ((int32_t)gyroData[axis] * dynP8[axis]) >> 6; // 32 bits is needed for calculation + + delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastGyro[axis] = gyroData[axis]; + DTerm = delta1[axis] + delta2[axis] + delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + + DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation + + axisPID[axis] = PTerm + ITerm - DTerm; + } + + //YAW + rc = (int32_t)rcCommand[YAW] * (2*controlRateConfig->yawRate + 30) >> 5; + error = rc - gyroData[YAW]; + errorGyroI_YAW += (int32_t)error * pidProfile->I8[YAW]; + errorGyroI_YAW = constrain(errorGyroI_YAW, 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); + if (abs(rc) > 50) errorGyroI_YAW = 0; + + PTerm = (int32_t)error * pidProfile->P8[YAW] >> 6; + + // Constrain YAW by D value if not servo driven in that case servolimits apply +// if(NumberOfMotors > 3) { + int16_t limit = GYRO_P_MAX - pidProfile->D8[YAW]; + PTerm = constrain(PTerm, -limit, +limit); +// } + + ITerm = constrain((int16_t)(errorGyroI_YAW >> 13), -GYRO_I_MAX, +GYRO_I_MAX); + + axisPID[YAW] = PTerm + ITerm; +} + + static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -390,6 +480,9 @@ void setPIDController(int type) break; case 2: pid_controller = pidBaseflight; + break; + case 3: + pid_controller = pidMultiWii23; } } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index adf96d7f57..2b54fb6597 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -369,7 +369,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 2 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 3 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, From dda5c2ccb7c3ef34f2ad4e93e73a0ed0984e0de0 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 7 Jan 2015 23:59:02 +0100 Subject: [PATCH 132/182] Cleanup code and implement Hybrid MultiWii PID controller Roll and pitch is using 2.2 algorithm Yaw is using 2.3 algorithm --- src/main/flight/flight.c | 130 +++++++++++++++++++++++++++++++++------ src/main/io/serial_cli.c | 2 +- 2 files changed, 112 insertions(+), 20 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 9899f65ad8..d3ee006a58 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -295,18 +295,15 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control { int axis, prop = 0; int32_t rc, error, errorAngle; + int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; + static int16_t lastGyro[2] = { 0, 0 }; + static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 }; int32_t delta; - static int32_t delta1[2], delta2[2]; - int32_t PTerm, ITerm, DTerm; - int32_t PTermACC, ITermACC; - static int16_t lastGyro[2] = {0,0}; - static int16_t errorAngleI[2] = {0,0}; - static int32_t errorGyroI_YAW; if (FLIGHT_MODE(HORIZON_MODE)) prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 512); // PITCH & ROLL - for(axis = 0; axis < 2; axis++) { + for (axis = 0; axis < 2; axis++) { rc = rcCommand[axis] << 1; error = rc - gyroData[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here @@ -322,7 +319,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #else - errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclnation), + errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #endif @@ -341,8 +338,8 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result - ITerm = ITermACC + ((ITerm-ITermACC) * prop >> 9); - PTerm = PTermACC + ((PTerm-PTermACC) * prop >> 9); + ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9); + PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9); } PTerm -= ((int32_t)gyroData[axis] * dynP8[axis]) >> 6; // 32 bits is needed for calculation @@ -359,25 +356,117 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control } //YAW - rc = (int32_t)rcCommand[YAW] * (2*controlRateConfig->yawRate + 30) >> 5; - error = rc - gyroData[YAW]; - errorGyroI_YAW += (int32_t)error * pidProfile->I8[YAW]; - errorGyroI_YAW = constrain(errorGyroI_YAW, 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); - if (abs(rc) > 50) errorGyroI_YAW = 0; + rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; + error = rc - gyroData[FD_YAW]; + errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; + errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); + if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; - PTerm = (int32_t)error * pidProfile->P8[YAW] >> 6; + PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // Constrain YAW by D value if not servo driven in that case servolimits apply // if(NumberOfMotors > 3) { - int16_t limit = GYRO_P_MAX - pidProfile->D8[YAW]; + int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW]; PTerm = constrain(PTerm, -limit, +limit); // } - ITerm = constrain((int16_t)(errorGyroI_YAW >> 13), -GYRO_I_MAX, +GYRO_I_MAX); + ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); - axisPID[YAW] = PTerm + ITerm; + axisPID[FD_YAW] = PTerm + ITerm; } +static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) +{ + int axis, prop; + int32_t rc, error, errorAngle; + int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; + static int16_t lastGyro[2] = { 0, 0 }; + static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 }; + int32_t deltaSum; + int32_t delta; + + UNUSED(controlRateConfig); + + // **** PITCH & ROLL **** + prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500] + + for (axis = 0; axis < 2; axis++) { + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { // MODE relying on ACC + // observe max inclination +#ifdef GPS + errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#else + errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#endif + +#ifdef AUTOTUNE + if (shouldAutotune()) { + errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); + } +#endif + + PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result + PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5); + + errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp + ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; + } + if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // MODE relying on GYRO + error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis]; + error -= gyroData[axis] / 4; + + PTermGYRO = rcCommand[axis]; + + errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp + if (abs(gyroData[axis]) > (640 * 4)) + errorGyroI[axis] = 0; + + ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; + } + if (FLIGHT_MODE(HORIZON_MODE)) { + PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; + ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500; + } else { + if (FLIGHT_MODE(ANGLE_MODE)) { + PTerm = PTermACC; + ITerm = ITermACC; + } else { + PTerm = PTermGYRO; + ITerm = ITermGYRO; + } + } + + PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation + delta = (gyroData[axis] - lastGyro[axis]) / 4; + lastGyro[axis] = gyroData[axis]; + deltaSum = delta1[axis] + delta2[axis] + delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + DTerm = (deltaSum * dynD8[axis]) / 32; + axisPID[axis] = PTerm + ITerm - DTerm; + } + //YAW + rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; + error = rc - gyroData[FD_YAW]; + errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; + errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); + if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; + + PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; + + // Constrain YAW by D value if not servo driven in that case servolimits apply +// if(NumberOfMotors > 3) { + int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW]; + PTerm = constrain(PTerm, -limit, +limit); +// } + + ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); + + axisPID[FD_YAW] = PTerm + ITerm; +} static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) @@ -483,6 +572,9 @@ void setPIDController(int type) break; case 3: pid_controller = pidMultiWii23; + break; + case 4: + pid_controller = pidMultiWiiHybrid; } } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2b54fb6597..a7f25061d3 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -369,7 +369,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 3 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 4 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, From c3cc92415df38314af250a07e75e982d818e5d2c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 9 Jan 2015 01:19:11 +0100 Subject: [PATCH 133/182] Fix gyroData[axis] scaling --- src/main/flight/flight.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index d3ee006a58..20d8405f8a 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -305,9 +305,9 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control // PITCH & ROLL for (axis = 0; axis < 2; axis++) { rc = rcCommand[axis] << 1; - error = rc - gyroData[axis]; + error = rc - (gyroData[axis] / 4); errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here - if (abs(gyroData[axis]) > 640) errorGyroI[axis] = 0; + if (abs(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0; ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 @@ -342,9 +342,9 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9); } - PTerm -= ((int32_t)gyroData[axis] * dynP8[axis]) >> 6; // 32 bits is needed for calculation + PTerm -= ((int32_t)(gyroData[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation - delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + delta = (gyroData[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyro[axis] = gyroData[axis]; DTerm = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; @@ -357,7 +357,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; - error = rc - gyroData[FD_YAW]; + error = rc - (gyroData[FD_YAW] / 4); errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; @@ -450,7 +450,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con } //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; - error = rc - gyroData[FD_YAW]; + error = rc - (gyroData[FD_YAW] / 4); errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; From 190e4c328dab1be4c2ff0f25badee5da40890b5d Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sun, 18 Jan 2015 16:01:01 +0100 Subject: [PATCH 134/182] Required updates after rebase --- src/main/flight/flight.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 20d8405f8a..c0a7d768bb 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -291,8 +291,10 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa #define GYRO_I_MAX 256 static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, - rollAndPitchTrims_t *angleTrim) + rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { + UNUSED(rxConfig); + int axis, prop = 0; int32_t rc, error, errorAngle; int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; @@ -376,8 +378,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control } static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { + UNUSED(rxConfig); + int axis, prop; int32_t rc, error, errorAngle; int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; From 97b4b786d145d77bc465a7c4c23fe40996927b55 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 21 Jan 2015 23:00:09 +0100 Subject: [PATCH 135/182] Remove yaw gyro scale for AlienWii 32 target, additional PID controllers BLACKBOX support --- src/main/flight/flight.c | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index c0a7d768bb..c7cdaee4a8 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -359,7 +359,11 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; +#ifdef ALIENWII32 + error = rc - gyroData[FD_YAW]; +#else error = rc - (gyroData[FD_YAW] / 4); +#endif errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; @@ -375,6 +379,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); axisPID[FD_YAW] = PTerm + ITerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif } static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, @@ -454,7 +464,11 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con } //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; +#ifdef ALIENWII32 + error = rc - gyroData[FD_YAW]; +#else error = rc - (gyroData[FD_YAW] / 4); +#endif errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; @@ -470,6 +484,12 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); axisPID[FD_YAW] = PTerm + ITerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif } static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, From 029945fbba709cd2cc5f49047052d2afa56e0311 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 23 Jan 2015 01:28:12 +0100 Subject: [PATCH 136/182] Fix motorCount issue for PID controllers --- src/main/flight/flight.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index c7cdaee4a8..72c3bd01fc 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -42,6 +42,7 @@ #include "io/gps.h" extern uint16_t cycleTime; +extern uint8_t motorCount; int16_t heading, magHold; int16_t axisPID[3]; @@ -371,10 +372,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // Constrain YAW by D value if not servo driven in that case servolimits apply -// if(NumberOfMotors > 3) { + if(motorCount > 3) { int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW]; PTerm = constrain(PTerm, -limit, +limit); -// } + } ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); @@ -476,10 +477,10 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // Constrain YAW by D value if not servo driven in that case servolimits apply -// if(NumberOfMotors > 3) { + if(motorCount > 3) { int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW]; PTerm = constrain(PTerm, -limit, +limit); -// } + } ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); From 4e0059ce4ccbf9416b8c326b331191c607f78468 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sun, 25 Jan 2015 22:53:31 +0100 Subject: [PATCH 137/182] Fix MIN, MAX and ABS in the PID controllers --- src/main/flight/flight.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 72c3bd01fc..05f2258702 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -303,14 +303,14 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 }; int32_t delta; - if (FLIGHT_MODE(HORIZON_MODE)) prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 512); + if (FLIGHT_MODE(HORIZON_MODE)) prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); // PITCH & ROLL for (axis = 0; axis < 2; axis++) { rc = rcCommand[axis] << 1; error = rc - (gyroData[axis] / 4); errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here - if (abs(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0; + if (ABS(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0; ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 @@ -367,7 +367,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control #endif errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); - if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; + if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0; PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; @@ -404,7 +404,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con UNUSED(controlRateConfig); // **** PITCH & ROLL **** - prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 500); // range [0;500] + prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500] for (axis = 0; axis < 2; axis++) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { // MODE relying on ACC @@ -436,7 +436,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp - if (abs(gyroData[axis]) > (640 * 4)) + if (ABS(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; @@ -472,7 +472,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #endif errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); - if (abs(rc) > 50) errorGyroI[FD_YAW] = 0; + if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0; PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; From a810df42283676e48fdb5c7348a0ac3021b42b6a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 26 Jan 2015 10:17:02 +0100 Subject: [PATCH 138/182] Fix stale comment in autotune. --- src/main/flight/autotune.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index b36168e6bf..7c9cd8b3fc 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -129,7 +129,7 @@ static autotunePhase_e nextPhase = FIRST_TUNE_PHASE; static float targetAngle = 0; static float targetAngleAtPeak; -static float firstPeakAngle, secondPeakAngle; // deci dgrees, 180 deg = 1800 +static float firstPeakAngle, secondPeakAngle; // in degrees typedef struct fp_pid { float p; From 9708c00a2e21c4f74f55d24b2b9b8199d1267e8e Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Tue, 27 Jan 2015 01:43:13 +1300 Subject: [PATCH 139/182] Blackbox: make logging of autotune events more detailed --- src/main/blackbox/blackbox.c | 17 +++++++++-- src/main/blackbox/blackbox_fielddefs.h | 15 ++++++++-- src/main/flight/autotune.c | 40 ++++++++++++++++++++++---- 3 files changed, 62 insertions(+), 10 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 211e25911c..49677e062a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -354,6 +354,12 @@ static void writeSignedVB(int32_t value) writeUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); } +static void writeS16(int16_t value) +{ + blackboxWrite(value & 0xFF); + blackboxWrite((value >> 8) & 0xFF); +} + /** * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits */ @@ -1270,17 +1276,24 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) break; case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: blackboxWrite(data->autotuneCycleStart.phase); - blackboxWrite(data->autotuneCycleStart.cycle); + blackboxWrite(data->autotuneCycleStart.cycle | (data->autotuneCycleStart.rising ? 0x80 : 0)); blackboxWrite(data->autotuneCycleStart.p); blackboxWrite(data->autotuneCycleStart.i); blackboxWrite(data->autotuneCycleStart.d); break; case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT: - blackboxWrite(data->autotuneCycleResult.overshot); + blackboxWrite(data->autotuneCycleResult.flags); blackboxWrite(data->autotuneCycleStart.p); blackboxWrite(data->autotuneCycleStart.i); blackboxWrite(data->autotuneCycleStart.d); break; + case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS: + writeS16(data->autotuneTargets.currentAngle); + blackboxWrite((uint8_t) data->autotuneTargets.targetAngle); + blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak); + writeS16(data->autotuneTargets.firstPeakAngle); + writeS16(data->autotuneTargets.secondPeakAngle); + break; case FLIGHT_LOG_EVENT_LOG_END: blackboxPrint("End of log"); blackboxWrite(0); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 26d862268f..d328e71e2c 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -101,6 +101,7 @@ typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, + FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; @@ -114,21 +115,31 @@ typedef struct flightLogEvent_autotuneCycleStart_t { uint8_t p; uint8_t i; uint8_t d; + uint8_t rising; } flightLogEvent_autotuneCycleStart_t; +#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1 +#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2 + typedef struct flightLogEvent_autotuneCycleResult_t { - uint8_t overshot; + uint8_t flags; uint8_t p; uint8_t i; uint8_t d; } flightLogEvent_autotuneCycleResult_t; +typedef struct flightLogEvent_autotuneTargets_t { + uint16_t currentAngle; + int8_t targetAngle, targetAngleAtPeak; + uint16_t firstPeakAngle, secondPeakAngle; +} flightLogEvent_autotuneTargets_t; + typedef union flightLogEventData_t { flightLogEvent_syncBeep_t syncBeep; flightLogEvent_autotuneCycleStart_t autotuneCycleStart; flightLogEvent_autotuneCycleResult_t autotuneCycleResult; - + flightLogEvent_autotuneTargets_t autotuneTargets; } flightLogEventData_t; typedef struct flightLogEvent_t diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index b36168e6bf..04d8e6ad53 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -161,11 +161,32 @@ static void autotuneLogCycleStart() eventData.p = pid.p * MULTIWII_P_MULTIPLIER; eventData.i = pid.i * MULTIWII_I_MULTIPLIER; eventData.d = pid.d; + eventData.rising = rising ? 1 : 0; blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData); } } +static void autotuneLogAngleTargets(float currentAngle) +{ + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneTargets_t eventData; + + // targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision: + eventData.targetAngle = (int) targetAngle; + // and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement: + eventData.targetAngleAtPeak = (int) targetAngleAtPeak; + + // currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again: + eventData.currentAngle = round(currentAngle * 10); + // the peak angles are only ever set to currentAngle, so they get the same treatment: + eventData.firstPeakAngle = round(firstPeakAngle * 10); + eventData.secondPeakAngle = round(secondPeakAngle * 10); + + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS, (flightLogEventData_t*)&eventData); + } +} + #endif static void startNewCycle(void) @@ -199,6 +220,7 @@ static void updateTargetAngle(void) float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle) { float currentAngle; + bool overshot; if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) { return errorAngle; @@ -229,6 +251,10 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin debug[2] = DEGREES_TO_DECIDEGREES(targetAngle); #endif +#ifdef BLACKBOX + autotuneLogAngleTargets(currentAngle); +#endif + if (secondPeakAngle == 0) { // The peak will be when our angular velocity is negative. To be sure we are in the right place, // we also check to make sure our angle position is greater than zero. @@ -244,20 +270,22 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin switch (cycle) { case CYCLE_TUNE_I: // when checking the I value, we would like to overshoot the target position by half of the max oscillation. - if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) { - pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; - } else { + overshot = currentAngle - targetAngle >= AUTOTUNE_MAX_OSCILLATION_ANGLE / 2; + + if (overshot) { pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { pid.i = AUTOTUNE_MINIMUM_I_VALUE; } + } else { + pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; } #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { flightLogEvent_autotuneCycleResult_t eventData; - eventData.overshot = currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 ? 0 : 1; + eventData.flags = overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT: 0; eventData.p = pidProfile->P8[pidIndex]; eventData.i = pidProfile->I8[pidIndex]; eventData.d = pidProfile->D8[pidIndex]; @@ -300,7 +328,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin // analyze the data // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude - bool overshot = firstPeakAngle > targetAngleAtPeak; + overshot = firstPeakAngle > targetAngleAtPeak; if (overshot) { #ifdef DEBUG_AUTOTUNE debug[0] = 1; @@ -338,7 +366,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin if (feature(FEATURE_BLACKBOX)) { flightLogEvent_autotuneCycleResult_t eventData; - eventData.overshot = overshot; + eventData.flags = (overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT : 0) | (timedOut ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT : 0); eventData.p = pidProfile->P8[pidIndex]; eventData.i = pidProfile->I8[pidIndex]; eventData.d = pidProfile->D8[pidIndex]; From f513016cc9cc02228106319ab514e1743edd6025 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 26 Jan 2015 15:26:23 +0100 Subject: [PATCH 140/182] Fix battery unit test compilation. --- src/test/unit/battery_unittest.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index 0d30723eca..52bd836d1c 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -73,6 +73,9 @@ TEST(BatteryTest, BatteryADCToVoltage) extern "C" { +uint8_t armingFlags = 0; +int16_t rcCommand[4] = {0,0,0,0}; + uint16_t adcGetChannel(uint8_t channel) { UNUSED(channel); From cd9dcd9ac78aabe8a195ad2a73c286f2a6192d54 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 26 Jan 2015 15:27:11 +0100 Subject: [PATCH 141/182] Set executable flag on .travis.sh --- .travis.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 .travis.sh diff --git a/.travis.sh b/.travis.sh old mode 100644 new mode 100755 From 7bd98e557a8eac7b74d55d6da31876ea83b6c059 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 26 Jan 2015 15:34:34 +0100 Subject: [PATCH 142/182] Add current_meter_type into the new MSP command. --- src/main/io/serial_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 7c75a05bcc..a9eff4df27 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1045,7 +1045,7 @@ static bool processOutCommand(uint8_t cmdMSP) headSerialReply(7); serialize16(masterConfig.batteryConfig.currentMeterScale); serialize16(masterConfig.batteryConfig.currentMeterOffset); - serialize8(0); // current meter type + serialize8(masterConfig.batteryConfig.currentMeterType); serialize16(masterConfig.batteryConfig.batteryCapacity); break; @@ -1393,7 +1393,7 @@ static bool processInCommand(void) case MSP_SET_CURRENT_METER_CONFIG: masterConfig.batteryConfig.currentMeterScale = read16(); masterConfig.batteryConfig.currentMeterOffset = read16(); - read(8); // current meter type + masterConfig.batteryConfig.currentMeterType = read8(); masterConfig.batteryConfig.batteryCapacity = read16(); break; From cf8338d95bf696de5de1be21bfe35dd77de6287e Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 26 Jan 2015 18:00:07 +0100 Subject: [PATCH 143/182] Fix Blackbox recording of alternative PID controllers --- src/main/flight/flight.c | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 05f2258702..9b9bcdbd72 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -356,6 +356,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation axisPID[axis] = PTerm + ITerm - DTerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif } //YAW @@ -382,9 +388,9 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control axisPID[FD_YAW] = PTerm + ITerm; #ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; + axisPID_P[FD_YAW] = PTerm; + axisPID_I[FD_YAW] = ITerm; + axisPID_D[FD_YAW] = DTerm; #endif } @@ -462,6 +468,12 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con delta1[axis] = delta; DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif } //YAW rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; @@ -486,10 +498,11 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con axisPID[FD_YAW] = PTerm + ITerm; + #ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; + axisPID_P[FD_YAW] = PTerm; + axisPID_I[FD_YAW] = ITerm; + axisPID_D[FD_YAW] = DTerm; #endif } From 396731a42838a6130ed5bc4458fbe41be6012d72 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 6 Jan 2015 18:21:24 +0100 Subject: [PATCH 144/182] Intitial support for ALIENWIIF3 target --- .travis.yml | 1 + Makefile | 8 +- docs/Board - AlienWii32.md | 10 +- docs/Spektrum bind.md | 4 +- src/main/config/config.c | 15 +- src/main/drivers/pwm_mapping.c | 2 +- src/main/drivers/timer.c | 2 +- src/main/target/ALIENWIIF3/system_stm32f30x.c | 372 ++++++++++++++++++ src/main/target/ALIENWIIF3/system_stm32f30x.h | 76 ++++ src/main/target/ALIENWIIF3/target.h | 144 +++++++ src/main/target/NAZE/target.h | 4 +- 11 files changed, 625 insertions(+), 13 deletions(-) create mode 100644 src/main/target/ALIENWIIF3/system_stm32f30x.c create mode 100644 src/main/target/ALIENWIIF3/system_stm32f30x.h create mode 100644 src/main/target/ALIENWIIF3/target.h diff --git a/.travis.yml b/.travis.yml index 7db4e3f0ca..26ade6fb87 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,6 +13,7 @@ env: - TARGET=SPARKY - TARGET=STM32F3DISCOVERY - TARGET=ALIENWIIF1 + - TARGET=ALIENWIIF3 # We use cpp for unit tests, and c for the main project. language: cpp compiler: clang diff --git a/Makefile b/Makefile index f818e81071..f2d4c5951d 100644 --- a/Makefile +++ b/Makefile @@ -35,7 +35,7 @@ SERIAL_DEVICE ?= /dev/ttyUSB0 FORKNAME = cleanflight -VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENWIIF1 +VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 CC3D CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENWIIF1 ALIENWIIF3 # Valid targets for OP BootLoader support OPBL_VALID_TARGETS = CC3D @@ -56,7 +56,7 @@ VPATH := $(SRC_DIR):$(SRC_DIR)/startup USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c)) -ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY)) +ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3)) STDPERIPH_DIR = $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver @@ -296,7 +296,7 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \ $(HIGHEND_SRC) \ $(COMMON_SRC) -ALIENWIIF1_SRC = $(NAZE_SRC) +ALIENWIIF1_SRC = $(NAZE_SRC) EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \ drivers/accgyro_adxl345.c \ @@ -491,6 +491,8 @@ SPARKY_SRC = \ $(COMMON_SRC) \ $(VCP_SRC) +ALIENWIIF3_SRC = $(SPARKY_SRC) + SPRACINGF3_SRC = \ $(STM32F30x_COMMON_SRC) \ drivers/accgyro_mpu6050.c \ diff --git a/docs/Board - AlienWii32.md b/docs/Board - AlienWii32.md index 912aaaf911..f442edcef2 100644 --- a/docs/Board - AlienWii32.md +++ b/docs/Board - AlienWii32.md @@ -1,10 +1,12 @@ -# Board - AlienWii32 +# Board - AlienWii32 (ALIENWIIF1 and ALIENWIIF3 target) -The AlienWii32 is actually in prototype stage and only a few samples exist. There is some more field testing ongoing. The information below is preliminary and will be updated as needed. +The AlienWii32 is actually in prototype stage and only a few samples exist. There are two different variants and some more field testing with some users is ongoing. The information below is preliminary and will be updated as needed. Here are the hardware specifications: -- STM32F103CBT6 MCU +- STM32F103CBT6 MCU (ALIENWIIF1) +- STM32F303CCT6 MCU (ALIENWIIF3) +- optional integrated serial/ppm receiver (ALIENWIIF3 only, future enhancement) - MPU6050 accelerometer/gyro sensor unit - 8x 4.2A brushed ESCs, integrated, to run the strongest micro motors - extra-wide traces on the PCB, for maximum power throughput @@ -21,4 +23,4 @@ Here are the hardware specifications: Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. -The pin layout is very similar as the NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The AlienWii32 firmware will be built with TARGET=NAZE and OPTIONS="AlienWii32". The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with an small Quadcopter. An preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienWii32. The mixer can be activated with "mixer custom" in the CLI. To use the AlienWii32 in an Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the Cleanflight configurator. +The pin layout for the ALIENWIIF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENWIIF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The AlienWii32 firmware will be built as target ALIENWIIF1 or ALIENWIIF3. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with an small Quadcopter. An preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienWii32. The mixer can be activated with "mixer custom" in the CLI. To use the AlienWii32 in an Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the Cleanflight configurator. diff --git a/docs/Spektrum bind.md b/docs/Spektrum bind.md index d756f2de96..eb50eb31ce 100644 --- a/docs/Spektrum bind.md +++ b/docs/Spektrum bind.md @@ -2,7 +2,7 @@ Spektrum bind with hardware bind plug support. -The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D targets. +The Spektrum bind code is actually enabled for the NAZE, NAZE32PRO, CJMCU, EUSTM32F103RC, SPARKY, CC3D, ALIENWIIF1, ALIENWIIF3 targets. ## Configure the bind code @@ -46,4 +46,4 @@ http://wiki.openpilot.org/display/Doc/Spektrum+Satellite ### Supported Hardware -NAZE, NAZE32PRO, CJMCU, SPARKY, EUSTM32F103RC, CC3D targets (AlienWii32 with hardware bind plug) +NAZE, NAZE32PRO, CJMCU, SPARKY, EUSTM32F103RC, CC3D targets and ALIENWIIF1, ALIENWIIF3 targets with hardware bind plug diff --git a/src/main/config/config.c b/src/main/config/config.c index 6ec55de3d2..a357a870df 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -453,20 +453,33 @@ static void resetConf(void) masterConfig.blackbox_rate_denom = 1; #endif - // alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target) + // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets #ifdef ALIENWII32 featureSet(FEATURE_RX_SERIAL); featureSet(FEATURE_MOTOR_STOP); featureSet(FEATURE_FAILSAFE); + featureClear(FEATURE_VBAT); +#ifdef ALIENWIIF3 + masterConfig.serialConfig.serial_port_scenario[2] = lookupScenarioIndex(SCENARIO_SERIAL_RX_ONLY); +#else masterConfig.serialConfig.serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_SERIAL_RX_ONLY); +#endif masterConfig.rxConfig.serialrx_provider = 1; masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.escAndServoConfig.minthrottle = 1000; masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; + masterConfig.looptime = 2000; +// currentProfile->pidController = 3; + currentProfile->pidProfile.P8[ROLL] = 36; + currentProfile->pidProfile.P8[PITCH] = 36; + currentProfile->failsafeConfig.failsafe_delay = 2; + currentProfile->failsafeConfig.failsafe_off_delay = 0; + currentProfile->failsafeConfig.failsafe_throttle = 1000; currentControlRateProfile->rcRate8 = 130; currentControlRateProfile->rollPitchRate = 20; currentControlRateProfile->yawRate = 60; + currentControlRateProfile->yawRate = 100; parseRcChannels("TAER1234", &masterConfig.rxConfig); // { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index a09f6ec10f..4edd6cbfa5 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -235,7 +235,7 @@ static const uint16_t airPWM[] = { }; #endif -#ifdef SPARKY +#if defined(SPARKY) || defined(ALIENWIIF3) static const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 5c10e1674c..f17c54f211 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -175,7 +175,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #endif -#ifdef SPARKY +#if defined(SPARKY) || defined(ALIENWIIF3) const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // // 6 x 3 pin headers diff --git a/src/main/target/ALIENWIIF3/system_stm32f30x.c b/src/main/target/ALIENWIIF3/system_stm32f30x.c new file mode 100644 index 0000000000..fca6969825 --- /dev/null +++ b/src/main/target/ALIENWIIF3/system_stm32f30x.c @@ -0,0 +1,372 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.c + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F30x devices, + * and is generated by the clock configuration tool + * stm32f30x_Clock_Configuration_V1.0.0.xls + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f30x.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (8 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f30x.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the SystemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define + * in "stm32f30x.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + * Supported STM32F30x device + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 72000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *---------------------------------------------------------------------------- + * PLLMUL | 9 + *----------------------------------------------------------------------------- + * PREDIV | 1 + *----------------------------------------------------------------------------- + * USB Clock | ENABLE + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** @addtogroup STM32F30x_System_Private_Includes + * @{ + */ + +#include "stm32f30x.h" + +uint32_t hse_value = HSE_VALUE; + +/** + * @} + */ + +/* Private typedef -----------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Defines + * @{ + */ +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/** + * @} + */ + +/* Private macro -------------------------------------------------------------*/ + +/** @addtogroup STM32F30x_System_Private_Variables + * @{ + */ + + uint32_t SystemCoreClock = 72000000; + + __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_FunctionPrototypes + * @{ + */ + +void SetSysClock(void); + +/** + * @} + */ + +/** @addtogroup STM32F30x_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR &= 0xF87FC00C; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */ + RCC->CFGR &= (uint32_t)0xFF80FFFF; + + /* Reset PREDIV1[3:0] bits */ + RCC->CFGR2 &= (uint32_t)0xFFFFFFF0; + + /* Reset USARTSW[1:0], I2CSW and TIMs bits */ + RCC->CFGR3 &= (uint32_t)0xFF00FCCC; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + //SetSysClock(); // called from main() + +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ +#endif +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value + * 8 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate (void) +{ + uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock */ + /* Get PLL clock source and multiplication factor ----------------------*/ + pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; + pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllsource == 0x00) + { + /* HSI oscillator clock divided by 2 selected as PLL clock entry */ + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; + } + else + { + prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; + /* HSE oscillator clock selected as PREDIV1 clock entry */ + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } + break; + default: /* HSI used as system clock */ + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK clock frequency ----------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK clock frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in SystemInit() function). + * @param None + * @retval None + */ +void SetSysClock(void) +{ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + + /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ + /* Enable HSE */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Enable Prefetch Buffer and set Flash Latency */ + FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; + + /* HCLK = SYSCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 1 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 2 */ + RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; + + /* PLL configuration */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + /* Enable PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Select PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; + + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL) + { + } + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ + diff --git a/src/main/target/ALIENWIIF3/system_stm32f30x.h b/src/main/target/ALIENWIIF3/system_stm32f30x.h new file mode 100644 index 0000000000..4f999d3058 --- /dev/null +++ b/src/main/target/ALIENWIIF3/system_stm32f30x.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @file system_stm32f30x.h + * @author MCD Application Team + * @version V1.1.1 + * @date 28-March-2014 + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f30x_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F30X_H +#define __SYSTEM_STM32F30X_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +/** @addtogroup STM32F30x_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F30X_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h new file mode 100644 index 0000000000..009782a697 --- /dev/null +++ b/src/main/target/ALIENWIIF3/target.h @@ -0,0 +1,144 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "AWF3" // AlienWii32 F3. + +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_4 // Blue LEDs - PB4 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB +#define LED1_GPIO GPIOB +#define LED1_PIN Pin_5 // Green LEDs - PB5 +#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB + +#define USABLE_TIMER_CHANNEL_COUNT 11 + +// MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component. +#define GYRO +#define USE_GYRO_MPU6050 + +#define GYRO_MPU6050_ALIGN CW270_DEG + +#define ACC +#define USE_ACC_MPU6050 + +#define ACC_MPU6050_ALIGN CW270_DEG + +//#define BARO +//#define USE_BARO_MS5611 + +#define MAG +#define USE_MAG_AK8975 + +#define MAG_AK8975_ALIGN CW0_DEG_FLIP + +#define LED0 +#define LED1 + +#define USE_VCP +#define USE_USART1 // Conn 1 - TX (PB6) RX PB7 (AF7) +#define USE_USART2 // Input - RX (PA3) +#define USE_USART3 // Servo out - 10/RX (PB11) 11/TX (PB10) +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN GPIO_Pin_6 // PB6 +#define UART1_RX_PIN GPIO_Pin_7 // PB7 +#define UART1_GPIO GPIOB +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource6 +#define UART1_RX_PINSOURCE GPIO_PinSource7 + +#define UART2_TX_PIN GPIO_Pin_2 // PA2 - Clashes with PWM6 input. +#define UART2_RX_PIN GPIO_Pin_3 // PA3 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource2 +#define UART2_RX_PINSOURCE GPIO_PinSource3 + +// Note: PA5 and PA0 are N/C on the sparky - potentially use for ADC or LED STRIP? + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) + +#define I2C2_SCL_GPIO GPIOA +#define I2C2_SCL_GPIO_AF GPIO_AF_4 +#define I2C2_SCL_PIN GPIO_Pin_9 +#define I2C2_SCL_PIN_SOURCE GPIO_PinSource9 +#define I2C2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOA +#define I2C2_SDA_GPIO GPIOA +#define I2C2_SDA_GPIO_AF GPIO_AF_4 +#define I2C2_SDA_PIN GPIO_Pin_10 +#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10 +#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA + + +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) + +#define BLACKBOX +#define SERIAL_RX +#define GPS +#define DISPLAY + +#define LED_STRIP +#if 1 +// LED strip configuration using PWM motor output pin 5. +#define LED_STRIP_TIMER TIM16 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL3 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_TIMER TIM16 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 +#define WS2811_DMA_CHANNEL DMA1_Channel3 +#define WS2811_IRQ DMA1_Channel3_IRQn +#endif + +#if 0 +// Alternate LED strip pin +// FIXME DMA IRQ Transfer Complete is never called because the TIM17_DMA_RMP needs to be set in SYSCFG_CFGR1 +#define LED_STRIP_TIMER TIM17 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL7 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_1 +#define WS2811_PIN GPIO_Pin_7 // TIM17_CH1 +#define WS2811_PIN_SOURCE GPIO_PinSource7 +#define WS2811_TIMER TIM17 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM17 +#define WS2811_DMA_CHANNEL DMA1_Channel7 +#define WS2811_IRQ DMA1_Channel7_IRQn +#endif + + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_3 + +// Hardware bind plug at PB12 (Pin 25) +#define BINDPLUG_PORT GPIOB +#define BINDPLUG_PIN Pin_12 + +// alternative defaults for AlienWii32 F3 target +#define ALIENWII32 +#define BRUSHED_MOTORS +#define HARDWARE_BIND_PLUG diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index ffd179baaa..c2109e5c21 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -152,8 +152,10 @@ #define BIND_PORT GPIOA #define BIND_PIN Pin_3 -// alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target) +// alternative defaults for AlienWii32 F1 target #ifdef ALIENWII32 +#undef TARGET_BOARD_IDENTIFIER +#define TARGET_BOARD_IDENTIFIER "AWF1" // AlienWii32 F1. #define BRUSHED_MOTORS #define HARDWARE_BIND_PLUG // Hardware bind plug at PB5 (Pin 41) From 704a43107044f7d148d223bc39e253020b76c34a Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 26 Jan 2015 19:26:44 +0100 Subject: [PATCH 145/182] PID contoller documentation update (MultiWii23 and MultiWiiHybrid) --- docs/PID tuning.md | 18 +++++++++++++++++- src/main/config/config.c | 2 +- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 99c770d782..2caf2c8479 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -104,4 +104,20 @@ only the gyros. The default is 75% For example, at a setting of "100" for "sensitivity_horizon", 100% self-leveling strength will be applied at center stick, 50% self-leveling will be applied at 50% stick, and no self-leveling will be applied at 100% stick. If sensitivity is decreased to 75, 100% self-leveling will be applied at center stick, 50% will be applied at 63% -stick, and no self-leveling will be applied at 75% stick and onwards. \ No newline at end of file +stick, and no self-leveling will be applied at 75% stick and onwards. + +### PID controller 3, "MultiWii23" (default for the ALIENWIIF1 and ALIENWIIF3 targets) + +PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later. + +The algorithm is handling roll and pitch differently to yaw. users with problems on yaw authority should try this one. + +For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. This will provide best performance on very small multicopters with brushed motors. + +### PID controller 4, "MultiWiiHybrid" + +PID Controller 3 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm. + +This PID controller was initialy implementd for testing purposes but is also performing quite well. + +For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors. diff --git a/src/main/config/config.c b/src/main/config/config.c index a357a870df..c5e53d52b5 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -470,7 +470,7 @@ static void resetConf(void) masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; masterConfig.looptime = 2000; -// currentProfile->pidController = 3; + currentProfile->pidController = 3; currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[PITCH] = 36; currentProfile->failsafeConfig.failsafe_delay = 2; From cc65a446249a287c6cb5cc6768570046c734104f Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 26 Jan 2015 19:37:00 +0100 Subject: [PATCH 146/182] Minor documentation fix --- docs/PID tuning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 2caf2c8479..8380bd1939 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -116,7 +116,7 @@ For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. ### PID controller 4, "MultiWiiHybrid" -PID Controller 3 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm. +PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm. This PID controller was initialy implementd for testing purposes but is also performing quite well. From 8824f708e019d3123585c7d909686a89c0d3c21e Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Tue, 27 Jan 2015 11:27:08 +1300 Subject: [PATCH 147/182] Fix blackbox logging of PIDs in new PID controllers --- src/main/flight/flight.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 9b9bcdbd72..cf2e66c3ed 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -360,7 +360,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; + axisPID_D[axis] = -DTerm; #endif } @@ -390,7 +390,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; axisPID_I[FD_YAW] = ITerm; - axisPID_D[FD_YAW] = DTerm; + axisPID_D[FD_YAW] = 0; #endif } @@ -472,7 +472,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; + axisPID_D[axis] = -DTerm; #endif } //YAW @@ -502,7 +502,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; axisPID_I[FD_YAW] = ITerm; - axisPID_D[FD_YAW] = DTerm; + axisPID_D[FD_YAW] = 0; #endif } From f4ec9a2c65b18a2949a4c75bc966b475d4a8e9fe Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 27 Jan 2015 22:20:28 +0100 Subject: [PATCH 148/182] CC3D - Updating docs with reference to VCP support. --- docs/Board - CC3D.md | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md index 580bd4c353..0f725e533d 100644 --- a/docs/Board - CC3D.md +++ b/docs/Board - CC3D.md @@ -10,9 +10,6 @@ If issues are found with this board please report via the [github issue tracker] The board has a USB port directly connected to the processor. Other boards like the Naze and Flip32 have an on-board USB to uart adapter which connect to the processor's serial port instead. -Currently there is no support for virtual com port functionality on the CC3D which means that cleanflight -does not currently use the USB socket at all. Therefore, the communication with the board is achieved through a USB-UART adaptater connected to the Main port. - The board cannot currently be used for hexacopters/octocopters. Tricopter & Airplane support is untested, please report success or failure if you try it. @@ -71,13 +68,14 @@ The 6 pin RC_Output connector has the following pinouts when used in RX_PARALLEL | Value | Identifier | Board Markings | Notes | | ----- | ------------ | -------------- | -----------------------------------------| -| 1 | USART1 | MAIN PORT | Has a hardware inverter for S.BUS | -| 2 | USART3 | FLEX PORT | | -| 3 | SoftSerial | RC connector | Pins 4 and 5 (Tx and Rx respectively) | +| 1 | VCP | USB PORT | | +| 2 | USART1 | MAIN PORT | Has a hardware inverter for S.BUS | +| 3 | USART3 | FLEX PORT | | +| 4 | SoftSerial | RC connector | Pins 4 and 5 (Tx and Rx respectively) | The SoftSerial port is not available when RX_PARALLEL_PWM is used. The transmission data rate is limited to 19200 baud. -To connect the GUI to the flight controller you need additional hardware attached to the USART1 serial port (by default). +To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP). # Flex Port @@ -106,6 +104,8 @@ There are two primary ways to get Cleanflight onto a CC3D board. The entire flash ram on the target processor is flashed with a single image. +The image can be flashed by using a USB to UART adapter connected to the main port when the CC3D is put into the STM32 bootloader mode, achieved by powering on the CC3D with the SBL/3.3v pads bridged. + ## OpenPilot Bootloader compatible image mode. The initial section of flash ram on the target process is flashed with a bootloader which can then run the code in the @@ -114,4 +114,3 @@ remaining area of flash ram. The OpenPilot bootloader code also allows the remaining section of flash to be reconfigured and re-flashed by the OpenPilot Ground Station (GCS) via USB without requiring a USB to uart adapter. -In this mode a USB to uart adapter is still required to connect to via the GUI or CLI. From a2b232e805f06c9260944553ab676488b55db93a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 27 Jan 2015 22:55:41 +0100 Subject: [PATCH 149/182] Detach midrc (input) from servo center (output). This fixes incorrect servo center position when midrc was not 1500. Most likely only experienced by futaba plane or servo/tilt users. --- src/main/config/config.c | 1 + src/main/flight/mixer.c | 4 ++-- src/main/io/escservo.h | 3 +++ src/main/io/serial_cli.c | 1 + src/main/main.c | 2 +- src/main/rx/rx.h | 4 ++-- 6 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index c5e53d52b5..a7597280da 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -197,6 +197,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) escAndServoConfig->minthrottle = 1150; escAndServoConfig->maxthrottle = 1850; escAndServoConfig->mincommand = 1000; + escAndServoConfig->servoCenterPulse = 1500; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 18c9e380fc..f322374744 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -473,14 +473,14 @@ static void airplaneMixer(void) int16_t lFlap = determineServoMiddleOrForwardFromChannel(2); lFlap = constrain(lFlap, servoConf[2].min, servoConf[2].max); - lFlap = rxConfig->midrc - lFlap; + lFlap = escAndServoConfig->servoCenterPulse - lFlap; if (slow_LFlaps < lFlap) slow_LFlaps += airplaneConfig->flaps_speed; else if (slow_LFlaps > lFlap) slow_LFlaps -= airplaneConfig->flaps_speed; servo[2] = ((int32_t)servoConf[2].rate * slow_LFlaps) / 100L; - servo[2] += rxConfig->midrc; + servo[2] += escAndServoConfig->servoCenterPulse; } if (FLIGHT_MODE(PASSTHRU_MODE)) { // Direct passthru from RX diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index d6ffa49266..66cd7db59e 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -18,7 +18,10 @@ #pragma once typedef struct escAndServoConfig_s { + + // PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms) uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs + uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. } escAndServoConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b6fd89293b..47737b7a73 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -226,6 +226,7 @@ const clivalue_t valueTable[] = { { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, PWM_RANGE_ZERO, PWM_RANGE_MAX }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, PWM_RANGE_ZERO, PWM_RANGE_MAX }, // FIXME lower limit should match code in the mixer, 1500 currently, diff --git a/src/main/main.c b/src/main/main.c index 3e300b8156..3e1052b544 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -305,7 +305,7 @@ void init(void) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors - pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; + pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwmRxInit(masterConfig.inputFilteringMode); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index b88e0c5903..2e401969c2 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -68,11 +68,11 @@ typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers + uint8_t rssi_channel; + uint8_t rssi_scale; uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end - uint8_t rssi_channel; - uint8_t rssi_scale; } rxConfig_t; #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) From bcedbd77fb987b60b3e1754ba6c62894cc5a48d4 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 27 Jan 2015 23:18:05 +0100 Subject: [PATCH 150/182] Adding a FIXME regarding changing serialrx_provider at runtime. --- src/main/rx/rx.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 0fdca6d09a..17ce1f7a13 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -172,7 +172,15 @@ void serialRxInit(rxConfig_t *rxConfig) bool isSerialRxFrameComplete(rxConfig_t *rxConfig) { - + /** + * FIXME: Each of the xxxxFrameComplete() methods MUST be able to survive being called without the + * corresponding xxxInit() method having been called first. + * + * This situation arises when the cli or the msp changes the value of rxConfig->serialrx_provider + * + * A solution is for the ___Init() to configure the serialRxFrameComplete function pointer which + * should be used instead of the switch statement below. + */ switch (rxConfig->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: From b8b248827c7427182b3cefad8a33370411849f31 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 27 Jan 2015 23:32:45 +0100 Subject: [PATCH 151/182] Bump config version for new servoCenterPulse setting. --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index a7597280da..e99b0eb543 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -110,7 +110,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 89; +static const uint8_t EEPROM_CONF_VERSION = 90; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { From 0624eb50675c564a5fc4ee7fd15d55438a01b6e2 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Wed, 28 Jan 2015 20:42:51 +1300 Subject: [PATCH 152/182] Refactor blackbox IO routines out into separate file --- Makefile | 4 +- src/main/blackbox/blackbox.c | 582 +++++++------------------------- src/main/blackbox/blackbox_io.c | 437 ++++++++++++++++++++++++ src/main/blackbox/blackbox_io.h | 41 +++ 4 files changed, 612 insertions(+), 452 deletions(-) create mode 100644 src/main/blackbox/blackbox_io.c create mode 100644 src/main/blackbox/blackbox_io.h diff --git a/Makefile b/Makefile index f2d4c5951d..a62cc1ab8e 100644 --- a/Makefile +++ b/Makefile @@ -248,7 +248,8 @@ HIGHEND_SRC = flight/autotune.c \ telemetry/smartport.c \ sensors/sonar.c \ sensors/barometer.c \ - blackbox/blackbox.c + blackbox/blackbox.c \ + blackbox/blackbox_io.c VCP_SRC = \ vcp/hw_config.c \ @@ -395,6 +396,7 @@ CJMCU_SRC = \ drivers/timer.c \ drivers/timer_stm32f10x.c \ blackbox/blackbox.c \ + blackbox/blackbox_io.c \ hardware_revision.c \ $(COMMON_SRC) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 49677e062a..d88b27910d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" #include "version.h" @@ -37,8 +36,6 @@ #include "drivers/light_led.h" #include "drivers/sound_beeper.h" -#include "common/printf.h" - #include "flight/flight.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" @@ -75,9 +72,8 @@ #include "config/config_master.h" #include "blackbox.h" +#include "blackbox_io.h" -#define BLACKBOX_BAUDRATE 115200 -#define BLACKBOX_INITIAL_PORT_MODE MODE_TX #define BLACKBOX_I_INTERVAL 32 #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) @@ -255,9 +251,6 @@ extern uint8_t motorCount; //From mw.c: extern uint32_t currentTime; -// How many bytes should we transmit per loop iteration? -static uint8_t serialChunkSize = 16; - static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; static struct { @@ -281,10 +274,6 @@ STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION static uint32_t blackboxIteration; static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; -static serialPort_t *blackboxPort; -static portMode_t previousPortMode; -static uint32_t previousBaudRate; - /* * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated. * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs @@ -299,306 +288,6 @@ static blackboxValues_t blackboxHistoryRing[3]; // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old) static blackboxValues_t* blackboxHistory[3]; -static void blackboxWrite(uint8_t value) -{ - serialWrite(blackboxPort, value); -} - -static void _putc(void *p, char c) -{ - (void)p; - serialWrite(blackboxPort, c); -} - -//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!) -static void blackboxPrintf(char *fmt, ...) -{ - va_list va; - va_start(va, fmt); - tfp_format(NULL, _putc, fmt, va); - va_end(va); -} - -// Print the null-terminated string 's' to the serial port and return the number of bytes written -static int blackboxPrint(const char *s) -{ - const char *pos = s; - - while (*pos) { - serialWrite(blackboxPort, *pos); - pos++; - } - - return pos - s; -} - -/** - * Write an unsigned integer to the blackbox serial port using variable byte encoding. - */ -static void writeUnsignedVB(uint32_t value) -{ - //While this isn't the final byte (we can only write 7 bits at a time) - while (value > 127) { - blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow" - value >>= 7; - } - blackboxWrite(value); -} - -/** - * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding. - */ -static void writeSignedVB(int32_t value) -{ - //ZigZag encode to make the value always positive - writeUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); -} - -static void writeS16(int16_t value) -{ - blackboxWrite(value & 0xFF); - blackboxWrite((value >> 8) & 0xFF); -} - -/** - * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits - */ -static void writeTag2_3S32(int32_t *values) { - static const int NUM_FIELDS = 3; - - //Need to be enums rather than const ints if we want to switch on them (due to being C) - enum { - BITS_2 = 0, - BITS_4 = 1, - BITS_6 = 2, - BITS_32 = 3 - }; - - enum { - BYTES_1 = 0, - BYTES_2 = 1, - BYTES_3 = 2, - BYTES_4 = 3 - }; - - int x; - int selector = BITS_2, selector2; - - /* - * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes - * below: - * - * Selector possibilities - * - * 2 bits per field ss11 2233, - * 4 bits per field ss00 1111 2222 3333 - * 6 bits per field ss11 1111 0022 2222 0033 3333 - * 32 bits per field sstt tttt followed by fields of various byte counts - */ - for (x = 0; x < NUM_FIELDS; x++) { - //Require more than 6 bits? - if (values[x] >= 32 || values[x] < -32) { - selector = BITS_32; - break; - } - - //Require more than 4 bits? - if (values[x] >= 8 || values[x] < -8) { - if (selector < BITS_6) - selector = BITS_6; - } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits? - if (selector < BITS_4) - selector = BITS_4; - } - } - - switch (selector) { - case BITS_2: - blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03)); - break; - case BITS_4: - blackboxWrite((selector << 6) | (values[0] & 0x0F)); - blackboxWrite((values[1] << 4) | (values[2] & 0x0F)); - break; - case BITS_6: - blackboxWrite((selector << 6) | (values[0] & 0x3F)); - blackboxWrite((uint8_t)values[1]); - blackboxWrite((uint8_t)values[2]); - break; - case BITS_32: - /* - * Do another round to compute a selector for each field, assuming that they are at least 8 bits each - * - * Selector2 field possibilities - * 0 - 8 bits - * 1 - 16 bits - * 2 - 24 bits - * 3 - 32 bits - */ - selector2 = 0; - - //Encode in reverse order so the first field is in the low bits: - for (x = NUM_FIELDS - 1; x >= 0; x--) { - selector2 <<= 2; - - if (values[x] < 128 && values[x] >= -128) - selector2 |= BYTES_1; - else if (values[x] < 32768 && values[x] >= -32768) - selector2 |= BYTES_2; - else if (values[x] < 8388608 && values[x] >= -8388608) - selector2 |= BYTES_3; - else - selector2 |= BYTES_4; - } - - //Write the selectors - blackboxWrite((selector << 6) | selector2); - - //And now the values according to the selectors we picked for them - for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) { - switch (selector2 & 0x03) { - case BYTES_1: - blackboxWrite(values[x]); - break; - case BYTES_2: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - break; - case BYTES_3: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x] >> 16); - break; - case BYTES_4: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x] >> 16); - blackboxWrite(values[x] >> 24); - break; - } - } - break; - } -} - -/** - * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits. - */ -static void writeTag8_4S16(int32_t *values) { - - //Need to be enums rather than const ints if we want to switch on them (due to being C) - enum { - FIELD_ZERO = 0, - FIELD_4BIT = 1, - FIELD_8BIT = 2, - FIELD_16BIT = 3 - }; - - uint8_t selector, buffer; - int nibbleIndex; - int x; - - selector = 0; - //Encode in reverse order so the first field is in the low bits: - for (x = 3; x >= 0; x--) { - selector <<= 2; - - if (values[x] == 0) - selector |= FIELD_ZERO; - else if (values[x] < 8 && values[x] >= -8) - selector |= FIELD_4BIT; - else if (values[x] < 128 && values[x] >= -128) - selector |= FIELD_8BIT; - else - selector |= FIELD_16BIT; - } - - blackboxWrite(selector); - - nibbleIndex = 0; - buffer = 0; - for (x = 0; x < 4; x++, selector >>= 2) { - switch (selector & 0x03) { - case FIELD_ZERO: - //No-op - break; - case FIELD_4BIT: - if (nibbleIndex == 0) { - //We fill high-bits first - buffer = values[x] << 4; - nibbleIndex = 1; - } else { - blackboxWrite(buffer | (values[x] & 0x0F)); - nibbleIndex = 0; - } - break; - case FIELD_8BIT: - if (nibbleIndex == 0) - blackboxWrite(values[x]); - else { - //Write the high bits of the value first (mask to avoid sign extension) - blackboxWrite(buffer | ((values[x] >> 4) & 0x0F)); - //Now put the leftover low bits into the top of the next buffer entry - buffer = values[x] << 4; - } - break; - case FIELD_16BIT: - if (nibbleIndex == 0) { - //Write high byte first - blackboxWrite(values[x] >> 8); - blackboxWrite(values[x]); - } else { - //First write the highest 4 bits - blackboxWrite(buffer | ((values[x] >> 12) & 0x0F)); - // Then the middle 8 - blackboxWrite(values[x] >> 4); - //Only the smallest 4 bits are still left to write - buffer = values[x] << 4; - } - break; - } - } - //Anything left over to write? - if (nibbleIndex == 1) - blackboxWrite(buffer); -} - -/** - * Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is - * written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero). - * - * valueCount must be 8 or less. - */ -static void writeTag8_8SVB(int32_t *values, int valueCount) -{ - uint8_t header; - int i; - - if (valueCount > 0) { - //If we're only writing one field then we can skip the header - if (valueCount == 1) { - writeSignedVB(values[0]); - } else { - //First write a one-byte header that marks which fields are non-zero - header = 0; - - // First field should be in low bits of header - for (i = valueCount - 1; i >= 0; i--) { - header <<= 1; - - if (values[i] != 0) - header |= 0x01; - } - - blackboxWrite(header); - - for (i = 0; i < valueCount; i++) - if (values[i] != 0) - writeSignedVB(values[i]); - } - } -} - static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) { switch (condition) { @@ -660,8 +349,9 @@ static void blackboxBuildConditionCache() blackboxConditionCache = 0; for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { - if (testBlackboxConditionUncached(cond)) + if (testBlackboxConditionUncached(cond)) { blackboxConditionCache |= 1 << cond; + } } } @@ -694,6 +384,7 @@ static void blackboxSetState(BlackboxState newState) break; case BLACKBOX_STATE_SHUTTING_DOWN: xmitState.u.startTime = millis(); + blackboxDeviceFlush(); break; default: ; @@ -708,23 +399,28 @@ static void writeIntraframe(void) blackboxWrite('I'); - writeUnsignedVB(blackboxIteration); - writeUnsignedVB(blackboxCurrent->time); + blackboxWriteUnsignedVB(blackboxIteration); + blackboxWriteUnsignedVB(blackboxCurrent->time); - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_P[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_I[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_I[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) - writeSignedVB(blackboxCurrent->axisPID_D[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]); + } + } - for (x = 0; x < 3; x++) - writeSignedVB(blackboxCurrent->rcCommand[x]); + for (x = 0; x < 3; x++) { + blackboxWriteSignedVB(blackboxCurrent->rcCommand[x]); + } - writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -733,41 +429,47 @@ static void writeIntraframe(void) * * Write 14 bits even if the number is negative (which would otherwise result in 32 bits) */ - writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF); + blackboxWriteUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF); } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) { // 12bit value directly from ADC - writeUnsignedVB(blackboxCurrent->amperageLatest); + blackboxWriteUnsignedVB(blackboxCurrent->amperageLatest); } #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->magADC[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->magADC[x]); + } } #endif #ifdef BARO - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) - writeSignedVB(blackboxCurrent->BaroAlt); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) { + blackboxWriteSignedVB(blackboxCurrent->BaroAlt); + } #endif - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->gyroData[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->gyroData[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->accSmooth[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->accSmooth[x]); + } //Motors can be below minthrottle when disarmed, but that doesn't happen much - writeUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); //Motors tend to be similar to each other - for (x = 1; x < motorCount; x++) - writeSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); + for (x = 1; x < motorCount; x++) { + blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); + } - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) - writeSignedVB(blackboxHistory[0]->servo[5] - 1500); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { + blackboxWriteSignedVB(blackboxHistory[0]->servo[5] - 1500); + } //Rotate our history buffers: @@ -795,36 +497,41 @@ static void writeInterframe(void) * Since the difference between the difference between successive times will be nearly zero (due to consistent * looptime spacing), use second-order differences. */ - writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); + blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) + for (x = 0; x < XYZ_AXIS_COUNT; x++) { deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x]; + } /* * The PID I field changes very slowly, most of the time +-2, so use an encoding * that can pack all three fields into one byte in that situation. */ - writeTag2_3S32(deltas); + blackboxWriteTag2_3S32(deltas); /* * The PID D term is frequently set to zero for yaw, which makes the result from the calculation * always zero. So don't bother recording D results when PID D terms are zero. */ - for (x = 0; x < XYZ_AXIS_COUNT; x++) - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) - writeSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) { + blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]); + } + } /* * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that * can pack multiple values per byte: */ - for (x = 0; x < 4; x++) + for (x = 0; x < 4; x++) { deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x]; + } - writeTag8_4S16(deltas); + blackboxWriteTag8_4S16(deltas); //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO int optionalFieldCount = 0; @@ -839,29 +546,35 @@ static void writeInterframe(void) #ifdef MAG if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) { - for (x = 0; x < XYZ_AXIS_COUNT; x++) + for (x = 0; x < XYZ_AXIS_COUNT; x++) { deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x]; + } } #endif #ifdef BARO - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) { deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt; + } #endif - writeTag8_8SVB(deltas, optionalFieldCount); + blackboxWriteTag8_8SVB(deltas, optionalFieldCount); //Since gyros, accs and motors are noisy, base the prediction on the average of the history: - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); + } - for (x = 0; x < XYZ_AXIS_COUNT; x++) - writeSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2); + for (x = 0; x < XYZ_AXIS_COUNT; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2); + } - for (x = 0; x < motorCount; x++) - writeSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2); + for (x = 0; x < motorCount; x++) { + blackboxWriteSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2); + } - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) - writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { + blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); + } //Rotate our history buffers blackboxHistory[2] = blackboxHistory[1]; @@ -871,8 +584,10 @@ static void writeInterframe(void) static int gcd(int num, int denom) { - if (denom == 0) + if (denom == 0) { return num; + } + return gcd(denom, num % denom); } @@ -892,59 +607,12 @@ static void validateBlackboxConfig() } } -static void configureBlackboxPort(void) -{ - blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX); - if (blackboxPort) { - previousPortMode = blackboxPort->mode; - previousBaudRate = blackboxPort->baudRate; - - serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE); - serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE); - beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); - } else { - blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); - - if (blackboxPort) { - previousPortMode = blackboxPort->mode; - previousBaudRate = blackboxPort->baudRate; - } - } - - /* - * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If - * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should - * transmit with each write. - * - * 9 / 1250 = 7200 / 1000000 - */ - serialChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); -} - -static void releaseBlackboxPort(void) -{ - serialSetMode(blackboxPort, previousPortMode); - serialSetBaudRate(blackboxPort, previousBaudRate); - - endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); - - /* - * Normally this would be handled by mw.c, but since we take an unknown amount - * of time to shut down asynchronously, we're the only ones that know when to call it. - */ - if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { - mspAllocateSerialPorts(&masterConfig.serialConfig); - } -} - void startBlackbox(void) { if (blackboxState == BLACKBOX_STATE_STOPPED) { validateBlackboxConfig(); - configureBlackboxPort(); - - if (!blackboxPort) { + if (!blackboxDeviceOpen()) { blackboxSetState(BLACKBOX_STATE_DISABLED); return; } @@ -982,7 +650,7 @@ void finishBlackbox(void) * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event. * Just give the port back and stop immediately. */ - releaseBlackboxPort(); + blackboxDeviceClose(); blackboxSetState(BLACKBOX_STATE_STOPPED); } } @@ -992,8 +660,8 @@ static void writeGPSHomeFrame() { blackboxWrite('H'); - writeSignedVB(GPS_home[0]); - writeSignedVB(GPS_home[1]); + blackboxWriteSignedVB(GPS_home[0]); + blackboxWriteSignedVB(GPS_home[1]); //TODO it'd be great if we could grab the GPS current time and write that too gpsHistory.GPS_home[0] = GPS_home[0]; @@ -1012,15 +680,15 @@ static void writeGPSFrame() */ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) { // Predict the time of the last frame in the main log - writeUnsignedVB(currentTime - blackboxHistory[1]->time); + blackboxWriteUnsignedVB(currentTime - blackboxHistory[1]->time); } - writeUnsignedVB(GPS_numSat); - writeSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); - writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); - writeUnsignedVB(GPS_altitude); - writeUnsignedVB(GPS_speed); - writeUnsignedVB(GPS_ground_course); + blackboxWriteUnsignedVB(GPS_numSat); + blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); + blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); + blackboxWriteUnsignedVB(GPS_altitude); + blackboxWriteUnsignedVB(GPS_speed); + blackboxWriteUnsignedVB(GPS_ground_course); gpsHistory.GPS_numSat = GPS_numSat; gpsHistory.GPS_coord[0] = GPS_coord[0]; @@ -1038,31 +706,39 @@ static void loadBlackboxState(void) blackboxCurrent->time = currentTime; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_P[i] = axisPID_P[i]; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + } + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_I[i] = axisPID_I[i]; - for (i = 0; i < XYZ_AXIS_COUNT; i++) + } + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_D[i] = axisPID_D[i]; + } - for (i = 0; i < 4; i++) + for (i = 0; i < 4; i++) { blackboxCurrent->rcCommand[i] = rcCommand[i]; + } - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->gyroData[i] = gyroData[i]; + } - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->accSmooth[i] = accSmooth[i]; + } - for (i = 0; i < motorCount; i++) + for (i = 0; i < motorCount; i++) { blackboxCurrent->motor[i] = motor[i]; + } blackboxCurrent->vbatLatest = vbatLatestADC; blackboxCurrent->amperageLatest = amperageLatestADC; #ifdef MAG - for (i = 0; i < XYZ_AXIS_COUNT; i++) + for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->magADC[i] = magADC[i]; + } #endif #ifdef BARO @@ -1103,8 +779,9 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he * the whole header. */ if (xmitState.u.fieldIndex == -1) { - if (xmitState.headerIndex >= headerCount) + if (xmitState.headerIndex >= headerCount) { return false; //Someone probably called us again after we had already completed transmission + } charsWritten = blackboxPrint("H Field "); charsWritten += blackboxPrint(headerNames[xmitState.headerIndex]); @@ -1115,15 +792,16 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he } else charsWritten = 0; - for (; xmitState.u.fieldIndex < fieldCount && charsWritten < serialChunkSize; xmitState.u.fieldIndex++) { + for (; xmitState.u.fieldIndex < fieldCount && charsWritten < blackboxWriteChunkSize; xmitState.u.fieldIndex++) { def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * xmitState.u.fieldIndex); if (!conditions || testBlackboxCondition(conditions[conditionsStride * xmitState.u.fieldIndex])) { if (needComma) { blackboxWrite(','); charsWritten++; - } else + } else { needComma = true; + } // The first header is a field name if (xmitState.headerIndex == 0) { @@ -1169,7 +847,7 @@ static bool blackboxWriteSysinfo() } // How many bytes can we afford to transmit this loop? - xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + serialChunkSize, 64); + xmitState.u.serialBudget = MIN(xmitState.u.serialBudget + blackboxWriteChunkSize, 64); // Most headers will consume at least 20 bytes so wait until we've built up that much link budget if (xmitState.u.serialBudget < 20) { @@ -1262,8 +940,9 @@ static bool blackboxWriteSysinfo() */ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) { - if (blackboxState != BLACKBOX_STATE_RUNNING) + if (blackboxState != BLACKBOX_STATE_RUNNING) { return; + } //Shared header for event frames blackboxWrite('E'); @@ -1272,7 +951,7 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) //Now serialize the data for this specific frame type switch (event) { case FLIGHT_LOG_EVENT_SYNC_BEEP: - writeUnsignedVB(data->syncBeep.time); + blackboxWriteUnsignedVB(data->syncBeep.time); break; case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: blackboxWrite(data->autotuneCycleStart.phase); @@ -1288,11 +967,11 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWrite(data->autotuneCycleStart.d); break; case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS: - writeS16(data->autotuneTargets.currentAngle); + blackboxWriteS16(data->autotuneTargets.currentAngle); blackboxWrite((uint8_t) data->autotuneTargets.targetAngle); blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak); - writeS16(data->autotuneTargets.firstPeakAngle); - writeS16(data->autotuneTargets.secondPeakAngle); + blackboxWriteS16(data->autotuneTargets.firstPeakAngle); + blackboxWriteS16(data->autotuneTargets.secondPeakAngle); break; case FLIGHT_LOG_EVENT_LOG_END: blackboxPrint("End of log"); @@ -1333,11 +1012,13 @@ void handleBlackbox(void) * buffer. */ if (millis() > xmitState.u.startTime + 100) { - for (i = 0; i < serialChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) + for (i = 0; i < blackboxWriteChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) { blackboxWrite(blackboxHeader[xmitState.headerIndex]); + } - if (blackboxHeader[xmitState.headerIndex] == '\0') + if (blackboxHeader[xmitState.headerIndex] == '\0') { blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO); + } } break; case BLACKBOX_STATE_SEND_FIELDINFO: @@ -1345,9 +1026,9 @@ void handleBlackbox(void) if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { #ifdef GPS - if (feature(FEATURE_GPS)) + if (feature(FEATURE_GPS)) { blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS); - else + } else #endif blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); } @@ -1372,8 +1053,9 @@ void handleBlackbox(void) //On entry of this state, xmitState.headerIndex is 0 //Keep writing chunks of the system info headers until it returns true to signal completion - if (blackboxWriteSysinfo()) + if (blackboxWriteSysinfo()) { blackboxSetState(BLACKBOX_STATE_PRERUN); + } break; case BLACKBOX_STATE_PRERUN: blackboxSetState(BLACKBOX_STATE_RUNNING); @@ -1425,7 +1107,7 @@ void handleBlackbox(void) } break; case BLACKBOX_STATE_SHUTTING_DOWN: - //On entry of this state, startTime is set + //On entry of this state, startTime is set and a flush is performed /* * Wait for the log we've transmitted to make its way to the logger before we release the serial port, @@ -1433,8 +1115,8 @@ void handleBlackbox(void) * * Don't wait longer than it could possibly take if something funky happens. */ - if (millis() > xmitState.u.startTime + 200 || isSerialTransmitBufferEmpty(blackboxPort)) { - releaseBlackboxPort(); + if (millis() > xmitState.u.startTime + 200 || isBlackboxDeviceIdle()) { + blackboxDeviceClose(); blackboxSetState(BLACKBOX_STATE_STOPPED); } break; @@ -1445,16 +1127,14 @@ void handleBlackbox(void) static bool canUseBlackboxWithCurrentConfiguration(void) { - if (!feature(FEATURE_BLACKBOX)) - return false; - - return true; + return feature(FEATURE_BLACKBOX); } void initBlackbox(void) { - if (canUseBlackboxWithCurrentConfiguration()) + if (canUseBlackboxWithCurrentConfiguration()) { blackboxSetState(BLACKBOX_STATE_STOPPED); - else + } else { blackboxSetState(BLACKBOX_STATE_DISABLED); + } } diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c new file mode 100644 index 0000000000..194c03c40f --- /dev/null +++ b/src/main/blackbox/blackbox_io.c @@ -0,0 +1,437 @@ +#include +#include + +#include "blackbox_io.h" + +#include "platform.h" +#include "version.h" + +#include "common/maths.h" +#include "common/axis.h" +#include "common/color.h" + +#include "drivers/gpio.h" +#include "drivers/sensor.h" +#include "drivers/system.h" +#include "drivers/serial.h" +#include "drivers/compass.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/accgyro.h" +#include "drivers/light_led.h" +#include "drivers/sound_beeper.h" + +#include "flight/flight.h" +#include "sensors/sensors.h" +#include "sensors/boardalignment.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/gyro.h" +#include "sensors/battery.h" +#include "io/beeper.h" +#include "io/display.h" +#include "io/escservo.h" +#include "rx/rx.h" +#include "io/rc_controls.h" +#include "flight/mixer.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/autotune.h" +#include "flight/navigation.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/serial.h" +#include "io/serial_cli.h" +#include "io/serial_msp.h" +#include "io/statusindicator.h" +#include "rx/msp.h" +#include "telemetry/telemetry.h" +#include "common/printf.h" + +#include "config/runtime_config.h" +#include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#define BLACKBOX_BAUDRATE 115200 +#define BLACKBOX_INITIAL_PORT_MODE MODE_TX + +// How many bytes should we transmit per loop iteration? +uint8_t blackboxWriteChunkSize = 16; + +static serialPort_t *blackboxPort; +static portMode_t previousPortMode; +static uint32_t previousBaudRate; + +void blackboxWrite(uint8_t value) +{ + serialWrite(blackboxPort, value); +} + +static void _putc(void *p, char c) +{ + (void)p; + blackboxWrite(c); +} + +//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!) +void blackboxPrintf(char *fmt, ...) +{ + va_list va; + va_start(va, fmt); + tfp_format(NULL, _putc, fmt, va); + va_end(va); +} + +// Print the null-terminated string 's' to the serial port and return the number of bytes written +int blackboxPrint(const char *s) +{ + const char *pos = s; + + while (*pos) { + blackboxWrite(*pos); + pos++; + } + + return pos - s; +} + +/** + * Write an unsigned integer to the blackbox serial port using variable byte encoding. + */ +void blackboxWriteUnsignedVB(uint32_t value) +{ + //While this isn't the final byte (we can only write 7 bits at a time) + while (value > 127) { + blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow" + value >>= 7; + } + blackboxWrite(value); +} + +/** + * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding. + */ +void blackboxWriteSignedVB(int32_t value) +{ + //ZigZag encode to make the value always positive + blackboxWriteUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); +} + +void blackboxWriteS16(int16_t value) +{ + blackboxWrite(value & 0xFF); + blackboxWrite((value >> 8) & 0xFF); +} + +/** + * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits + */ +void blackboxWriteTag2_3S32(int32_t *values) { + static const int NUM_FIELDS = 3; + + //Need to be enums rather than const ints if we want to switch on them (due to being C) + enum { + BITS_2 = 0, + BITS_4 = 1, + BITS_6 = 2, + BITS_32 = 3 + }; + + enum { + BYTES_1 = 0, + BYTES_2 = 1, + BYTES_3 = 2, + BYTES_4 = 3 + }; + + int x; + int selector = BITS_2, selector2; + + /* + * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes + * below: + * + * Selector possibilities + * + * 2 bits per field ss11 2233, + * 4 bits per field ss00 1111 2222 3333 + * 6 bits per field ss11 1111 0022 2222 0033 3333 + * 32 bits per field sstt tttt followed by fields of various byte counts + */ + for (x = 0; x < NUM_FIELDS; x++) { + //Require more than 6 bits? + if (values[x] >= 32 || values[x] < -32) { + selector = BITS_32; + break; + } + + //Require more than 4 bits? + if (values[x] >= 8 || values[x] < -8) { + if (selector < BITS_6) { + selector = BITS_6; + } + } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits? + if (selector < BITS_4) { + selector = BITS_4; + } + } + } + + switch (selector) { + case BITS_2: + blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03)); + break; + case BITS_4: + blackboxWrite((selector << 6) | (values[0] & 0x0F)); + blackboxWrite((values[1] << 4) | (values[2] & 0x0F)); + break; + case BITS_6: + blackboxWrite((selector << 6) | (values[0] & 0x3F)); + blackboxWrite((uint8_t)values[1]); + blackboxWrite((uint8_t)values[2]); + break; + case BITS_32: + /* + * Do another round to compute a selector for each field, assuming that they are at least 8 bits each + * + * Selector2 field possibilities + * 0 - 8 bits + * 1 - 16 bits + * 2 - 24 bits + * 3 - 32 bits + */ + selector2 = 0; + + //Encode in reverse order so the first field is in the low bits: + for (x = NUM_FIELDS - 1; x >= 0; x--) { + selector2 <<= 2; + + if (values[x] < 128 && values[x] >= -128) { + selector2 |= BYTES_1; + } else if (values[x] < 32768 && values[x] >= -32768) { + selector2 |= BYTES_2; + } else if (values[x] < 8388608 && values[x] >= -8388608) { + selector2 |= BYTES_3; + } else { + selector2 |= BYTES_4; + } + } + + //Write the selectors + blackboxWrite((selector << 6) | selector2); + + //And now the values according to the selectors we picked for them + for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) { + switch (selector2 & 0x03) { + case BYTES_1: + blackboxWrite(values[x]); + break; + case BYTES_2: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + break; + case BYTES_3: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + break; + case BYTES_4: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x] >> 16); + blackboxWrite(values[x] >> 24); + break; + } + } + break; + } +} + +/** + * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits. + */ +void blackboxWriteTag8_4S16(int32_t *values) { + + //Need to be enums rather than const ints if we want to switch on them (due to being C) + enum { + FIELD_ZERO = 0, + FIELD_4BIT = 1, + FIELD_8BIT = 2, + FIELD_16BIT = 3 + }; + + uint8_t selector, buffer; + int nibbleIndex; + int x; + + selector = 0; + //Encode in reverse order so the first field is in the low bits: + for (x = 3; x >= 0; x--) { + selector <<= 2; + + if (values[x] == 0) { + selector |= FIELD_ZERO; + } else if (values[x] < 8 && values[x] >= -8) { + selector |= FIELD_4BIT; + } else if (values[x] < 128 && values[x] >= -128) { + selector |= FIELD_8BIT; + } else { + selector |= FIELD_16BIT; + } + } + + blackboxWrite(selector); + + nibbleIndex = 0; + buffer = 0; + for (x = 0; x < 4; x++, selector >>= 2) { + switch (selector & 0x03) { + case FIELD_ZERO: + //No-op + break; + case FIELD_4BIT: + if (nibbleIndex == 0) { + //We fill high-bits first + buffer = values[x] << 4; + nibbleIndex = 1; + } else { + blackboxWrite(buffer | (values[x] & 0x0F)); + nibbleIndex = 0; + } + break; + case FIELD_8BIT: + if (nibbleIndex == 0) { + blackboxWrite(values[x]); + } else { + //Write the high bits of the value first (mask to avoid sign extension) + blackboxWrite(buffer | ((values[x] >> 4) & 0x0F)); + //Now put the leftover low bits into the top of the next buffer entry + buffer = values[x] << 4; + } + break; + case FIELD_16BIT: + if (nibbleIndex == 0) { + //Write high byte first + blackboxWrite(values[x] >> 8); + blackboxWrite(values[x]); + } else { + //First write the highest 4 bits + blackboxWrite(buffer | ((values[x] >> 12) & 0x0F)); + // Then the middle 8 + blackboxWrite(values[x] >> 4); + //Only the smallest 4 bits are still left to write + buffer = values[x] << 4; + } + break; + } + } + //Anything left over to write? + if (nibbleIndex == 1) { + blackboxWrite(buffer); + } +} + +/** + * Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is + * written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero). + * + * valueCount must be 8 or less. + */ +void blackboxWriteTag8_8SVB(int32_t *values, int valueCount) +{ + uint8_t header; + int i; + + if (valueCount > 0) { + //If we're only writing one field then we can skip the header + if (valueCount == 1) { + blackboxWriteSignedVB(values[0]); + } else { + //First write a one-byte header that marks which fields are non-zero + header = 0; + + // First field should be in low bits of header + for (i = valueCount - 1; i >= 0; i--) { + header <<= 1; + + if (values[i] != 0) { + header |= 0x01; + } + } + + blackboxWrite(header); + + for (i = 0; i < valueCount; i++) { + if (values[i] != 0) { + blackboxWriteSignedVB(values[i]); + } + } + } + } +} + +/** + * If there is data waiting to be written to the blackbox device, attempt to write that now. + */ +void blackboxDeviceFlush(void) +{ + //Presently a no-op on serial +} + +/** + * Attempt to open the logging device. Returns true if successful. + */ +bool blackboxDeviceOpen(void) +{ + blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX); + if (blackboxPort) { + previousPortMode = blackboxPort->mode; + previousBaudRate = blackboxPort->baudRate; + + serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE); + serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE); + beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + } else { + blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); + + if (blackboxPort) { + previousPortMode = blackboxPort->mode; + previousBaudRate = blackboxPort->baudRate; + } + } + + /* + * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If + * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should + * transmit with each write. + * + * 9 / 1250 = 7200 / 1000000 + */ + blackboxWriteChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); + + return blackboxPort != NULL; +} + +void blackboxDeviceClose(void) +{ + serialSetMode(blackboxPort, previousPortMode); + serialSetBaudRate(blackboxPort, previousBaudRate); + + endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + + /* + * Normally this would be handled by mw.c, but since we take an unknown amount + * of time to shut down asynchronously, we're the only ones that know when to call it. + */ + if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { + mspAllocateSerialPorts(&masterConfig.serialConfig); + } +} + +bool isBlackboxDeviceIdle(void) +{ + return isSerialTransmitBufferEmpty(blackboxPort); +} diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h new file mode 100644 index 0000000000..5abacecfa5 --- /dev/null +++ b/src/main/blackbox/blackbox_io.h @@ -0,0 +1,41 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +uint8_t blackboxWriteChunkSize; + +void blackboxWrite(uint8_t value); + +void blackboxPrintf(char *fmt, ...); +int blackboxPrint(const char *s); + +void blackboxWriteUnsignedVB(uint32_t value); +void blackboxWriteSignedVB(int32_t value); +void blackboxWriteS16(int16_t value); +void blackboxWriteTag2_3S32(int32_t *values); +void blackboxWriteTag8_4S16(int32_t *values); +void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); + +void blackboxDeviceFlush(void); +bool blackboxDeviceOpen(void); +void blackboxDeviceClose(void); + +bool isBlackboxDeviceIdle(void); From e134574c8185e3128f7ad2bfb99886074185f4f3 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Wed, 28 Jan 2015 20:43:25 +1300 Subject: [PATCH 153/182] Fix bug in logging of currentMeter header --- src/main/blackbox/blackbox.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d88b27910d..2440af0a51 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -924,9 +924,10 @@ static bool blackboxWriteSysinfo() xmitState.u.serialBudget -= strlen("H vbatref:%u\n"); break; case 13: - blackboxPrintf("H currentMeter:%i,%i\n", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); + blackboxPrintf("H currentMeter:%d,%d\n", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale); - xmitState.u.serialBudget -= strlen("H currentMeter:%i,%i\n"); + xmitState.u.serialBudget -= strlen("H currentMeter:%d,%d\n"); + break; default: return true; } From db1c5d8ac4dba2b29487caedd1f93e67cde37988 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 28 Jan 2015 13:21:10 +0100 Subject: [PATCH 154/182] Updating PID controller documentation --- docs/PID tuning.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 8380bd1939..0ebfa00e3d 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -31,13 +31,13 @@ strength of the correction to be backed off in order to avoid overshooting the t ## PID controllers -Cleanflight has three built-in PID controllers which each have different flight behavior. Each controller requires +Cleanflight has 5 built-in PID controllers which each have different flight behavior. Each controller requires different PID settings for best performance, so if you tune your craft using one PID controller, those settings will likely not work well on any of the other controllers. -You can change between PID controllers by running `set pid_controller = X` on the CLI tab of the Cleanflight -Configurator, where X is the number of the controller you want to use. Please read these notes first before trying one -out! +You can change between PID controllers by running `set pid_controller=n` on the CLI tab of the Cleanflight +Configurator, where `n` is the number of the controller you want to use. Please read these notes first before trying one +out. ### PID controller 0, "MultiWii" (default) @@ -110,7 +110,7 @@ stick, and no self-leveling will be applied at 75% stick and onwards. PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later. -The algorithm is handling roll and pitch differently to yaw. users with problems on yaw authority should try this one. +The algorithm is handling roll and pitch differently to yaw. Users with problems on yaw authority should try this one. For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. This will provide best performance on very small multicopters with brushed motors. @@ -118,6 +118,6 @@ For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm. -This PID controller was initialy implementd for testing purposes but is also performing quite well. +This PID controller was initialy implemented for testing purposes but is also performing quite well. For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors. From b4803697d2ae2eb8e40f59613b4eef882b46066c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 07:56:26 +0100 Subject: [PATCH 155/182] Initial port of Harakiri PID controller some of the settings are hardcoded --- src/main/flight/flight.c | 120 +++++++++++++++++++++++++++++++++++++++ src/main/io/serial_cli.c | 2 +- 2 files changed, 121 insertions(+), 1 deletion(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index cf2e66c3ed..35cb64b3a8 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -43,6 +43,7 @@ extern uint16_t cycleTime; extern uint8_t motorCount; +extern uint32_t currentTime; int16_t heading, magHold; int16_t axisPID[3]; @@ -506,6 +507,122 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #endif } +int32_t SpecialIntegerRoundUp(float val) // If neg value just represents a change in direction rounding to next higher number is "more" negative +{ + if (val > 0) return val + 0.5f; + else if (val < 0) return val - 0.5f; + else return 0; +} + +#define RCconstPI 0.159154943092f // 0.5f / M_PI; +#define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. +#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) + +static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, +rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) +{ + UNUSED(rxConfig); + + float delta, RCfactor, rcCommandAxis, MainDptCut; + float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0; + static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0}; + float tmp0flt; + static uint32_t previousTime; + int32_t tmp0, PTermYW; + uint8_t axis; + static int32_t errorGyroI_YW = 0; + float ACCDeltaTimeINS = 0; + float FLOATcycleTime = 0; + +// MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D + MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now + FLOATcycleTime = (float)constrain(currentTime - previousTime, 1, 100000); // 1us - 100ms + previousTime = currentTime; + ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now + RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element + + if(FLIGHT_MODE(HORIZON_MODE)) prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f; + for (axis = 0; axis < 2; axis++) { + rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { + error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; + tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f; + PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt); + errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f); + ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f; + } + if (!FLIGHT_MODE(ANGLE_MODE)) { + if (ABS((int16_t)gyroData[axis]) > 2560) errorGyroI[axis] = 0.0f; + else { + error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis]; + errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); + } + ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f; + if (FLIGHT_MODE(HORIZON_MODE)) { + PTerm = PTermACC + prop * (rcCommandAxis - PTermACC); + ITerm = ITermACC + prop * (ITermGYRO - ITermACC); + } + else { + PTerm = rcCommandAxis; + ITerm = ITermGYRO; + } + } + else { + PTerm = PTermACC; + ITerm = ITermACC; + } + PTerm -= gyroData[axis] * dynP8[axis] * 0.003f; + delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS; + lastGyro[axis] = gyroData[axis]; + lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); + DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; + axisPID[axis] = SpecialIntegerRoundUp(PTerm + ITerm - DTerm); // Round up result. + + #ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = -DTerm; +#endif + } + + tmp0flt = (uint16_t)FLOATcycleTime & (uint16_t)0xFFFC; // Filter last 2 bit jitter + tmp0flt /= 3000.0f; + if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardrcoded for now + PTermYW = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; + tmp0 = SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); + axisPID[FD_YAW] = rcCommand[FD_YAW] - tmp0 * PTermYW / 80; + if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) + errorGyroI_YW = 0; + else { + error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0; + errorGyroI_YW = constrain(errorGyroI_YW + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp + axisPID[FD_YAW] += (errorGyroI_YW / 125 * pidProfile->I8[FD_YAW]) >> 6; + } + } + else { + tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; + error = tmp0 - SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better + if (ABS(tmp0) > 50) errorGyroI_YW = 0; + else errorGyroI_YW = constrain(errorGyroI_YW + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); + axisPID[FD_YAW] = constrain(errorGyroI_YW >> 13, -GYRO_I_MAX, +GYRO_I_MAX); + PTermYW = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; + if(motorCount > 3) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply + tmp0 = 300; + if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW]; + PTermYW = constrain(PTermYW, -tmp0, tmp0); + } + axisPID[FD_YAW] += PTermYW; + } + axisPID[FD_YAW] = SpecialIntegerRoundUp(axisPID[FD_YAW]); // Round up result. + +#ifdef BLACKBOX + axisPID_P[FD_YAW] = PTerm; + axisPID_I[FD_YAW] = ITerm; + axisPID_D[FD_YAW] = DTerm; +#endif +} + static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -613,6 +730,9 @@ void setPIDController(int type) break; case 4: pid_controller = pidMultiWiiHybrid; + break; + case 5: + pid_controller = pidHarakiri; } } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 47737b7a73..2ec7e39f4b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -370,7 +370,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 4 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, From ef749e822ae9923cd71c74be595ff62c4bd25076 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 08:45:37 +0100 Subject: [PATCH 156/182] Harakiri PID controller code cleanup and documentation --- docs/PID tuning.md | 16 ++++++++++++++++ src/main/flight/flight.c | 31 +++++++++++++++---------------- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 0ebfa00e3d..ab2414d3c5 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -121,3 +121,19 @@ PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and This PID controller was initialy implemented for testing purposes but is also performing quite well. For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors. + +### PID controller 5, "Harakiri" + +PID Controller 5 is an port of the PID controller from the Harakiri firmware. + +The algorithm is leveraging more floating point math. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: + + OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. + MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) + +The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly. + +Yaw authority is also quite good. Only the P value is used in the yaw algorithm. + + + diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 35cb64b3a8..e6467dc3da 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -528,9 +528,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0}; float tmp0flt; static uint32_t previousTime; - int32_t tmp0, PTermYW; + int32_t tmp0; uint8_t axis; - static int32_t errorGyroI_YW = 0; float ACCDeltaTimeINS = 0; float FLOATcycleTime = 0; @@ -579,7 +578,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; axisPID[axis] = SpecialIntegerRoundUp(PTerm + ITerm - DTerm); // Round up result. - #ifdef BLACKBOX +#ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = -DTerm; @@ -589,37 +588,37 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) tmp0flt = (uint16_t)FLOATcycleTime & (uint16_t)0xFFFC; // Filter last 2 bit jitter tmp0flt /= 3000.0f; if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardrcoded for now - PTermYW = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; + PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; tmp0 = SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); - axisPID[FD_YAW] = rcCommand[FD_YAW] - tmp0 * PTermYW / 80; + axisPID[FD_YAW] = rcCommand[FD_YAW] - tmp0 * PTerm / 80; if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) - errorGyroI_YW = 0; + errorGyroI[FD_YAW] = 0; else { error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0; - errorGyroI_YW = constrain(errorGyroI_YW + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp - axisPID[FD_YAW] += (errorGyroI_YW / 125 * pidProfile->I8[FD_YAW]) >> 6; + errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp + axisPID[FD_YAW] += (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; } } else { tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; error = tmp0 - SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better - if (ABS(tmp0) > 50) errorGyroI_YW = 0; - else errorGyroI_YW = constrain(errorGyroI_YW + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); - axisPID[FD_YAW] = constrain(errorGyroI_YW >> 13, -GYRO_I_MAX, +GYRO_I_MAX); - PTermYW = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; + if (ABS(tmp0) > 50) errorGyroI[FD_YAW] = 0; + else errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); + axisPID[FD_YAW] = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX); + PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; if(motorCount > 3) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply tmp0 = 300; if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW]; - PTermYW = constrain(PTermYW, -tmp0, tmp0); + PTerm = constrain(PTerm, -tmp0, tmp0); } - axisPID[FD_YAW] += PTermYW; + axisPID[FD_YAW] += PTerm; } axisPID[FD_YAW] = SpecialIntegerRoundUp(axisPID[FD_YAW]); // Round up result. #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; - axisPID_I[FD_YAW] = ITerm; - axisPID_D[FD_YAW] = DTerm; + axisPID_I[FD_YAW] = 0; + axisPID_D[FD_YAW] = 0; #endif } From a5c3c266866aa4e66781f4906c94f28b5088854f Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 09:23:50 +0100 Subject: [PATCH 157/182] Fix problem for non GPS targets and add code for autotuning --- src/main/flight/flight.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index e6467dc3da..460376bcde 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -544,7 +544,17 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) for (axis = 0; axis < 2; axis++) { rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { +#ifdef GPS error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#else + error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#endif + +#ifdef AUTOTUNE + if (shouldAutotune()) { + error = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(error))); + } +#endif PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f; PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt); From db8d539cbb5a617657e351b4050255490ed88c68 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 10:42:41 +0100 Subject: [PATCH 158/182] Another documentation upddate for the Harakiri PID controller --- docs/PID tuning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index ab2414d3c5..9d8a4ffcdb 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -126,7 +126,7 @@ For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This PID Controller 5 is an port of the PID controller from the Harakiri firmware. -The algorithm is leveraging more floating point math. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: +The algorithm is leveraging more floating point math. This PID controller is also compensates for different looptimes on roll and pitch. It likely don’t need retuning of the PID values. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) From 124ae1f590516c1dfb72bc474400b3bc3f6beebf Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 14:13:04 +0100 Subject: [PATCH 159/182] Finalize Blackbox yaw output for Harakiri PID controller Minor code updates and cleanup Documentation update --- docs/PID tuning.md | 4 ++-- src/main/flight/flight.c | 37 +++++++++++++------------------------ 2 files changed, 15 insertions(+), 26 deletions(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 9d8a4ffcdb..3d2900bbdd 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -126,14 +126,14 @@ For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This PID Controller 5 is an port of the PID controller from the Harakiri firmware. -The algorithm is leveraging more floating point math. This PID controller is also compensates for different looptimes on roll and pitch. It likely don’t need retuning of the PID values. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: +The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don’t need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly. -Yaw authority is also quite good. Only the P value is used in the yaw algorithm. +Yaw authority is also quite good. diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 460376bcde..97334700c9 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -43,7 +43,6 @@ extern uint16_t cycleTime; extern uint8_t motorCount; -extern uint32_t currentTime; int16_t heading, magHold; int16_t axisPID[3]; @@ -507,16 +506,9 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #endif } -int32_t SpecialIntegerRoundUp(float val) // If neg value just represents a change in direction rounding to next higher number is "more" negative -{ - if (val > 0) return val + 0.5f; - else if (val < 0) return val - 0.5f; - else return 0; -} - #define RCconstPI 0.159154943092f // 0.5f / M_PI; -#define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. #define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) +#define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) @@ -527,7 +519,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0; static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0}; float tmp0flt; - static uint32_t previousTime; int32_t tmp0; uint8_t axis; float ACCDeltaTimeINS = 0; @@ -535,8 +526,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) // MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now - FLOATcycleTime = (float)constrain(currentTime - previousTime, 1, 100000); // 1us - 100ms - previousTime = currentTime; + FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element @@ -586,7 +576,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) lastGyro[axis] = gyroData[axis]; lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; - axisPID[axis] = SpecialIntegerRoundUp(PTerm + ITerm - DTerm); // Round up result. + axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result. #ifdef BLACKBOX axisPID_P[axis] = PTerm; @@ -597,37 +587,36 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) tmp0flt = (uint16_t)FLOATcycleTime & (uint16_t)0xFFFC; // Filter last 2 bit jitter tmp0flt /= 3000.0f; - if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardrcoded for now + if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; - tmp0 = SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); - axisPID[FD_YAW] = rcCommand[FD_YAW] - tmp0 * PTerm / 80; - if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) - errorGyroI[FD_YAW] = 0; + tmp0 = lrintf(gyroData[FD_YAW] * 0.25f); + PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80; + if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) errorGyroI[FD_YAW] = 0; else { error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp - axisPID[FD_YAW] += (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; + ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; } } else { tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; - error = tmp0 - SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better + error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better if (ABS(tmp0) > 50) errorGyroI[FD_YAW] = 0; else errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); - axisPID[FD_YAW] = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX); + ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX); PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; if(motorCount > 3) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply tmp0 = 300; if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW]; PTerm = constrain(PTerm, -tmp0, tmp0); } - axisPID[FD_YAW] += PTerm; } - axisPID[FD_YAW] = SpecialIntegerRoundUp(axisPID[FD_YAW]); // Round up result. + axisPID[FD_YAW] = PTerm + ITerm; + axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; - axisPID_I[FD_YAW] = 0; + axisPID_I[FD_YAW] = ITerm; axisPID_D[FD_YAW] = 0; #endif } From 56176abe66e2f0b242942a0e128dd29c69d6458c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 14:42:49 +0100 Subject: [PATCH 160/182] Fix potential overflow. --- src/main/flight/flight.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 97334700c9..2f72ee7873 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -585,7 +585,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) #endif } - tmp0flt = (uint16_t)FLOATcycleTime & (uint16_t)0xFFFC; // Filter last 2 bit jitter + tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter tmp0flt /= 3000.0f; if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; From 25676e944a61fe51aec13560b9b9dbe141f3180e Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 28 Jan 2015 15:21:24 +0100 Subject: [PATCH 161/182] Update PID controller documentation --- docs/PID tuning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 3d2900bbdd..fd17c1c9b8 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -31,7 +31,7 @@ strength of the correction to be backed off in order to avoid overshooting the t ## PID controllers -Cleanflight has 5 built-in PID controllers which each have different flight behavior. Each controller requires +Cleanflight has 6 built-in PID controllers which each have different flight behavior. Each controller requires different PID settings for best performance, so if you tune your craft using one PID controller, those settings will likely not work well on any of the other controllers. From bf1bc864ae22818cb2f92dc745295731c90aee5b Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Wed, 28 Jan 2015 17:28:32 +0100 Subject: [PATCH 162/182] Send current meter reading as signed value Use signed value normally, truncate to 0-0xffff when multiwiiCurrentMeterOutput is active --- src/main/io/serial_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a9eff4df27..739cef67db 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -830,9 +830,9 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery serialize16(rssi); if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { - serialize16((uint16_t)constrain((ABS(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps + serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero } else - serialize16((uint16_t)constrain(ABS(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps + serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A break; case MSP_RC_TUNING: headSerialReply(7); From 34cd8f466eb01430eaea0c60e6d71ad2f38a3fd2 Mon Sep 17 00:00:00 2001 From: Krzysztof Rosinski Date: Wed, 28 Jan 2015 22:43:37 +0100 Subject: [PATCH 163/182] MSP command for sonar altitude --- src/main/flight/altitudehold.c | 12 +++++--- src/main/flight/altitudehold.h | 10 ++++--- src/main/flight/flight.h | 1 - src/main/io/rc_controls.h | 2 ++ src/main/io/serial.c | 1 - src/main/io/serial_msp.c | 52 ++++++++++++++++++++++------------ src/main/io/serial_msp.h | 5 ++++ src/main/main.c | 2 +- src/main/mw.c | 2 +- src/main/sensors/sonar.c | 26 +++++++++++------ src/main/sensors/sonar.h | 7 ++--- 11 files changed, 78 insertions(+), 42 deletions(-) diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 5fa40d624b..bfd7be91db 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -53,7 +53,6 @@ uint8_t velocityControl = 0; int32_t errorVelocityI = 0; int32_t altHoldThrottleAdjustment = 0; int32_t AltHold; -int32_t EstAlt; // in cm int32_t vario = 0; // variometer in cm/s @@ -78,7 +77,7 @@ void configureAltitudeHold( #if defined(BARO) || defined(SONAR) static int16_t initialThrottleHold; - +static int32_t EstAlt; // in cm // 40hz update rate (20hz LPF on acc) #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) @@ -230,6 +229,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) float vel_acc; int32_t vel_tmp; float accZ_tmp; + int32_t sonarAlt = -1; static float accZ_old = 0.0f; static float vel = 0.0f; static float accAlt = 0.0f; @@ -242,8 +242,6 @@ void calculateEstimatedAltitude(uint32_t currentTime) int16_t tiltAngle; #endif - - dTime = currentTime - previousTime; if (dTime < BARO_UPDATE_FREQUENCY_40HZ) return; @@ -329,5 +327,11 @@ void calculateEstimatedAltitude(uint32_t currentTime) accZ_old = accZ_tmp; } + +int32_t altitudeHoldGetEstimatedAltitude(void) +{ + return EstAlt; +} + #endif diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index fefbb75824..1e42c65efc 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,14 +15,16 @@ * along with Cleanflight. If not, see . */ -//extern int32_t errorVelocityI; -//extern int32_t altHoldThrottleAdjustment; -//extern uint8_t velocityControl; -//extern int32_t setVelocity; +#include "flight/flight.h" +#include "io/escservo.h" +#include "io/rc_controls.h" + +#include "sensors/barometer.h" void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig); void applyAltHold(airplaneConfig_t *airplaneConfig); void updateAltHoldState(void); void updateSonarAltHoldState(void); +int32_t altitudeHoldGetEstimatedAltitude(void); diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 95877badf2..51fd74a0a7 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -111,7 +111,6 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; extern int16_t heading, magHold; extern int32_t AltHold; -extern int32_t EstAlt; extern int32_t vario; void setPIDController(int type); diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 8441ea90f1..a1c2a10a50 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,6 +17,8 @@ #pragma once +#include "rx/rx.h" + typedef enum { BOXARM = 0, BOXANGLE, diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 397d4ee744..56fec19a92 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -44,7 +44,6 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate); -void mspInit(serialConfig_t *serialConfig); void cliInit(serialConfig_t *serialConfig); // this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask. diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 739cef67db..31ab095ac5 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -45,6 +45,8 @@ #include "flight/mixer.h" #include "flight/failsafe.h" #include "flight/navigation.h" +#include "flight/altitudehold.h" + #include "rx/rx.h" #include "rx/msp.h" #include "io/escservo.h" @@ -54,9 +56,11 @@ #include "io/serial.h" #include "io/ledstrip.h" #include "telemetry/telemetry.h" + #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/battery.h" +#include "sensors/sonar.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" @@ -249,6 +253,7 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SERVO_CONF 120 //out message Servo settings #define MSP_NAV_STATUS 121 //out message Returns navigation status #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters +#define MSP_SONAR_ALTITUDE 123 //out message Returns sonar altitude [cm] #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -368,7 +373,7 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; static mspPort_t *currentPort; -void serialize32(uint32_t a) +static void serialize32(uint32_t a) { static uint8_t t; t = a; @@ -385,7 +390,7 @@ void serialize32(uint32_t a) currentPort->checksum ^= t; } -void serialize16(int16_t a) +static void serialize16(int16_t a) { static uint8_t t; t = a; @@ -396,32 +401,32 @@ void serialize16(int16_t a) currentPort->checksum ^= t; } -void serialize8(uint8_t a) +static void serialize8(uint8_t a) { serialWrite(mspSerialPort, a); currentPort->checksum ^= a; } -uint8_t read8(void) +static uint8_t read8(void) { return currentPort->inBuf[currentPort->indRX++] & 0xff; } -uint16_t read16(void) +static uint16_t read16(void) { uint16_t t = read8(); t += (uint16_t)read8() << 8; return t; } -uint32_t read32(void) +static uint32_t read32(void) { uint32_t t = read16(); t += (uint32_t)read16() << 16; return t; } -void headSerialResponse(uint8_t err, uint8_t s) +static void headSerialResponse(uint8_t err, uint8_t s) { serialize8('$'); serialize8('M'); @@ -431,36 +436,36 @@ void headSerialResponse(uint8_t err, uint8_t s) serialize8(currentPort->cmdMSP); } -void headSerialReply(uint8_t s) +static void headSerialReply(uint8_t s) { headSerialResponse(0, s); } -void headSerialError(uint8_t s) +static void headSerialError(uint8_t s) { headSerialResponse(1, s); } -void tailSerialReply(void) +static void tailSerialReply(void) { serialize8(currentPort->checksum); } -void s_struct(uint8_t *cb, uint8_t siz) +static void s_struct(uint8_t *cb, uint8_t siz) { headSerialReply(siz); while (siz--) serialize8(*cb++); } -void serializeNames(const char *s) +static void serializeNames(const char *s) { const char *c; for (c = s; *c; c++) serialize8(*c); } -const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) +static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) { uint8_t boxIndex; const box_t *candidate; @@ -473,7 +478,7 @@ const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) return NULL; } -const box_t *findBoxByPermenantId(uint8_t permenantId) +static const box_t *findBoxByPermenantId(uint8_t permenantId) { uint8_t boxIndex; const box_t *candidate; @@ -486,7 +491,7 @@ const box_t *findBoxByPermenantId(uint8_t permenantId) return NULL; } -void serializeBoxNamesReply(void) +static void serializeBoxNamesReply(void) { int i, activeBoxId, j, flag = 1, count = 0, len; const box_t *box; @@ -625,7 +630,7 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXLEDLOW; } #endif - + if (feature(FEATURE_INFLIGHT_ACC_CAL)) activeBoxIds[activeBoxIdCount++] = BOXCALIB; @@ -652,7 +657,6 @@ static bool processOutCommand(uint8_t cmdMSP) { uint32_t i, tmp, junk; - #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0; @@ -821,9 +825,21 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_ALTITUDE: headSerialReply(6); - serialize32(EstAlt); +#if defined(BARO) || defined(SONAR) + serialize32(altitudeHoldGetEstimatedAltitude()); +#else + serialize32(0); +#endif serialize16(vario); break; + case MSP_SONAR_ALTITUDE: + headSerialReply(4); +#if defined(SONAR) + serialize32(sonarGetLatestAltitude()); +#else + serialize32(0); +#endif + break; case MSP_ANALOG: headSerialReply(7); serialize8((uint8_t)constrain(vbat, 0, 255)); diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 80abd02132..fd26e9981e 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -17,9 +17,14 @@ #pragma once +#include "io/serial.h" +#include "drivers/serial.h" + // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 +void mspInit(serialConfig_t *serialConfig); + void mspProcess(void); void sendMspTelemetry(void); void mspSetTelemetryPort(serialPort_t *mspTelemetryPort); diff --git a/src/main/main.c b/src/main/main.c index 3e1052b544..3b1a33e8c5 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -332,7 +332,7 @@ void init(void) #ifdef SONAR if (feature(FEATURE_SONAR)) { - Sonar_init(); + sonarInit(); } #endif diff --git a/src/main/mw.c b/src/main/mw.c index 12425863a8..0d542311c5 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -469,7 +469,7 @@ void executePeriodicTasks(void) #ifdef SONAR case UPDATE_SONAR_TASK: if (sensors(SENSOR_SONAR)) { - Sonar_update(); + sonarUpdate(); } break; #endif diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 518cb56052..842df64045 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -33,11 +33,13 @@ #include "sensors/sensors.h" #include "sensors/sonar.h" -int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range - inclination adjusted by imu +// in cm , -1 indicate sonar is not in range - inclination adjusted by imu #ifdef SONAR -void Sonar_init(void) +static int32_t calculatedAltitude; + +void sonarInit(void) { #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R) static const sonarHardware_t const sonarPWM56 = { @@ -74,25 +76,33 @@ void Sonar_init(void) #endif sensorsSet(SENSOR_SONAR); - sonarAlt = 0; + calculatedAltitude = -1; } -void Sonar_update(void) +void sonarUpdate(void) { hcsr04_start_reading(); } +int32_t sonarRead(void) +{ + return hcsr04_get_distance(); +} + int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle) { // calculate sonar altitude only if the sonar is facing downwards(<25deg) if (tiltAngle > 250) - return -1; + calculatedAltitude = -1; + else + calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f; - return sonarAlt * (900.0f - tiltAngle) / 900.0f; + return calculatedAltitude; } -int32_t sonarRead(void) { - return hcsr04_get_distance(); +int32_t sonarGetLatestAltitude(void) +{ + return calculatedAltitude; } #endif diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index a6a2a8b94d..3b8674e424 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -17,10 +17,9 @@ #pragma once -extern int32_t sonarAlt; - -void Sonar_init(void); -void Sonar_update(void); +void sonarInit(void); +void sonarUpdate(void); int32_t sonarRead(void); int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle); +int32_t sonarGetLatestAltitude(void); From 50428f2dcc0cc5730ddb33d4d5701382fe93afe6 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 15:23:38 +0100 Subject: [PATCH 164/182] Change MSP_SONAR_ALTITUDE to be in the range used for cleanflight original commands. See https://code.google.com/p/multiwii/source/browse/trunk/MultiWii_shared/Protocol.cpp#18 --- src/main/io/serial_msp.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 31ab095ac5..3c3a4aa22f 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -207,6 +207,8 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_VOLTAGE_METER_CONFIG 56 #define MSP_SET_VOLTAGE_METER_CONFIG 57 +#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] + // // Baseflight MSP commands (if enabled they exist in Cleanflight) // @@ -253,7 +255,6 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SERVO_CONF 120 //out message Servo settings #define MSP_NAV_STATUS 121 //out message Returns navigation status #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters -#define MSP_SONAR_ALTITUDE 123 //out message Returns sonar altitude [cm] #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed From 104a263533b8c341ccbb5e05205a974faf4e0262 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 17:02:10 +0100 Subject: [PATCH 165/182] Cleanup code style in Harakiri PID controller merge. --- src/main/flight/flight.c | 43 +++++++++++++++++++++++++++------------- 1 file changed, 29 insertions(+), 14 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 2f72ee7873..e0c6b4f839 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -530,7 +530,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element - if(FLIGHT_MODE(HORIZON_MODE)) prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f; + if (FLIGHT_MODE(HORIZON_MODE)) { + prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f; + } + for (axis = 0; axis < 2; axis++) { rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { @@ -551,31 +554,36 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f); ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f; } + if (!FLIGHT_MODE(ANGLE_MODE)) { - if (ABS((int16_t)gyroData[axis]) > 2560) errorGyroI[axis] = 0.0f; - else { + if (ABS((int16_t)gyroData[axis]) > 2560) { + errorGyroI[axis] = 0.0f; + } else { error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); } + ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f; + if (FLIGHT_MODE(HORIZON_MODE)) { PTerm = PTermACC + prop * (rcCommandAxis - PTermACC); ITerm = ITermACC + prop * (ITermGYRO - ITermACC); - } - else { + } else { PTerm = rcCommandAxis; ITerm = ITermGYRO; } - } - else { + } else { PTerm = PTermACC; ITerm = ITermACC; } + PTerm -= gyroData[axis] * dynP8[axis] * 0.003f; delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS; + lastGyro[axis] = gyroData[axis]; lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; + axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result. #ifdef BLACKBOX @@ -587,25 +595,32 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter tmp0flt /= 3000.0f; + if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; tmp0 = lrintf(gyroData[FD_YAW] * 0.25f); PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80; - if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) errorGyroI[FD_YAW] = 0; - else { + if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { + errorGyroI[FD_YAW] = 0; + } else { error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0; errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; } - } - else { + } else { tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better - if (ABS(tmp0) > 50) errorGyroI[FD_YAW] = 0; - else errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); + + if (ABS(tmp0) > 50) { + errorGyroI[FD_YAW] = 0; + } else { + errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); + } + ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX); PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; - if(motorCount > 3) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply + + if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply tmp0 = 300; if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW]; PTerm = constrain(PTerm, -tmp0, tmp0); From 203c7447637d1c946f645842734b828c8c661c7a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 18:17:53 +0100 Subject: [PATCH 166/182] Add MSP command to allow changing pid controller. --- src/main/io/serial_msp.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3c3a4aa22f..5622da838c 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -125,7 +125,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 4 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 5 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -209,6 +209,8 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] +#define MSP_PID_CONTROLLER 59 +#define MSP_SET_PID_CONTROLLER 60 // // Baseflight MSP commands (if enabled they exist in Cleanflight) // @@ -892,6 +894,10 @@ static bool processOutCommand(uint8_t cmdMSP) headSerialReply(sizeof(pidnames) - 1); serializeNames(pidnames); break; + case MSP_PID_CONTROLLER: + headSerialReply(1); + serialize8(currentProfile->pidController); + break; case MSP_MODE_RANGES: headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT); for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { @@ -1198,6 +1204,10 @@ static bool processInCommand(void) currentProfile->accelerometerTrims.values.pitch = read16(); currentProfile->accelerometerTrims.values.roll = read16(); break; + case MSP_SET_PID_CONTROLLER: + currentProfile->pidController = read8(); + setPIDController(currentProfile->pidController); + break; case MSP_SET_PID: if (currentProfile->pidController == 2) { for (i = 0; i < 3; i++) { From 649081a5a6bb0c9114261344393f8098328c1c35 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 18:20:10 +0100 Subject: [PATCH 167/182] Rename pidBaseflight to pidLuxFloat since it was never in official baseflight firmware. --- docs/PID tuning.md | 9 +++------ src/main/flight/flight.c | 4 ++-- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index fd17c1c9b8..4ae7fb154a 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -71,7 +71,7 @@ need to be increased in order to perform like PID controller 0. The LEVEL "D" setting is not used by this controller. -### PID controller 2, "Baseflight" +### PID controller 2, "LuxFloat" PID Controller 2 is Lux's new floating point PID controller. Both controller 0 and 1 use integer arithmetic, which was faster in the days of the slower 8-bit MultiWii controllers, but is less precise. @@ -82,10 +82,7 @@ don't have to be retuned when the looptime setting changes. There were initially some problems with horizon mode, and sluggishness in acro mode, that were recently fixed by nebbian in v1.6.0. The autotune feature does not work on this controller, so don't try to autotune it. -Even though PC2 is called "pidBaseflight" in the code, it was never in Baseflight or MultiWii. A better name might have -been pidFloatingPoint, or pidCleanflight. It is the first PID Controller designed for 32-bit processors and not derived -from MultiWii. I believe it was named pidBaseflight because it was to be the first true 32-bit processor native PID -controller, and thus the native Baseflight PC, but Timecop never accepted the code into Baseflight. +It is the first PID Controller designed for 32-bit processors and not derived from MultiWii. The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to @@ -126,7 +123,7 @@ For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This PID Controller 5 is an port of the PID controller from the Harakiri firmware. -The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don’t need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: +The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don�t need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index e0c6b4f839..26e18f618d 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -97,7 +97,7 @@ bool shouldAutotune(void) } #endif -static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, +static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { float RateError, errorAngle, AngleRate, gyroRate; @@ -736,7 +736,7 @@ void setPIDController(int type) pid_controller = pidRewrite; break; case 2: - pid_controller = pidBaseflight; + pid_controller = pidLuxFloat; break; case 3: pid_controller = pidMultiWii23; From 741f20a8bb25e04f17fb33ed793a4f2a19746fbd Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 19:02:55 +0100 Subject: [PATCH 168/182] Bump version to 1.7.0 --- src/main/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/version.h b/src/main/version.h index a31fe051fc..cb3706b0c3 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 7 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define MW_VERSION 231 From ff6aecc1c80782e153b21dfe73c0fc3ea1f4c29f Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 21:20:44 +0100 Subject: [PATCH 169/182] CC3D - Disable display feature if the USART3 serial port is used. --- src/main/config/config.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index e99b0eb543..d74f714afc 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -719,6 +719,12 @@ void validateAndFixConfig(void) } #endif +#if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3) + if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) { + featureClear(FEATURE_DISPLAY); + } +#endif + useRxConfig(&masterConfig.rxConfig); serialConfig_t *serialConfig = &masterConfig.serialConfig; From 7b8ad20af62971319de5590e8aa87569cd3cdbf7 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 21:23:41 +0100 Subject: [PATCH 170/182] Fix max baud rates on CC3D / F3 targets that were broken in merge of 6048a2e. --- src/main/io/serial.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 56fec19a92..d8e7b0a2a6 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -91,13 +91,13 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { {SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE }, #endif #ifdef USE_USART1 - {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART1, 9600, 250000, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #endif #ifdef USE_USART2 - {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART2, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #endif #ifdef USE_USART3 - {SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART3, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #endif }; @@ -117,8 +117,8 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { #ifdef USE_VCP {SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE }, #endif - {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, - {SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART1, 9600, 250000, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART3, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, {SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE} }; #else @@ -133,7 +133,7 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { }; const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { - {SERIAL_PORT_USART1, 9600, 250000, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART1, 9600, 250000, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, {SERIAL_PORT_USART2, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #if (SERIAL_PORT_COUNT > 2) {SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}, From e81cc9696eabdf03797b48d4a829fca4d410f9ae Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 21:25:50 +0100 Subject: [PATCH 171/182] CC3D - Support serial RX on UART1. --- src/main/drivers/serial_uart_stm32f10x.c | 35 ++++++++++++++---------- src/main/io/serial.c | 2 +- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index 8cc1519df1..ba9128d419 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -47,13 +47,18 @@ static uartPort_t uartPort2; static uartPort_t uartPort3; #endif +// Using RX DMA disables the use of receive callbacks +#if !defined(CC3D) // FIXME move board specific code to target.h files. +#define USE_USART1_RX_DMA +#endif + void uartStartTxDMA(uartPort_t *s); void usartIrqCallback(uartPort_t *s) { uint16_t SR = s->USARTx->SR; - if (SR & USART_FLAG_RXNE) { + if (SR & USART_FLAG_RXNE && !s->rxDMAChannel) { // If we registered a callback, pass crap there if (s->port.callback) { s->port.callback(s->USARTx->DR); @@ -98,11 +103,13 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) s->USARTx = USART1; - s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; - s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; +#ifdef USE_USART1_RX_DMA s->rxDMAChannel = DMA1_Channel5; + s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; +#endif s->txDMAChannel = DMA1_Channel4; + s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); @@ -129,6 +136,15 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); +#ifndef USE_USART1_RX_DMA + // RX/TX Interrupt + NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1); + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); +#endif + return s; } @@ -146,20 +162,11 @@ void DMA1_Channel4_IRQHandler(void) s->txDMAEmpty = true; } -// USART1 Tx IRQ Handler +// USART1 Rx/Tx IRQ Handler void USART1_IRQHandler(void) { uartPort_t *s = &uartPort1; - uint16_t SR = s->USARTx->SR; - - if (SR & USART_FLAG_TXE) { - if (s->port.txBufferTail != s->port.txBufferHead) { - s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail]; - s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize; - } else { - USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE); - } - } + usartIrqCallback(s); } #endif diff --git a/src/main/io/serial.c b/src/main/io/serial.c index d8e7b0a2a6..4c9b5c4d75 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -117,7 +117,7 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { #ifdef USE_VCP {SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE }, #endif - {SERIAL_PORT_USART1, 9600, 250000, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART1, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, {SERIAL_PORT_USART3, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, {SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE} }; From 1d4c8925cc5e7572f74489ccf0f161433be00071 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 21:27:13 +0100 Subject: [PATCH 172/182] Invert the useage of USE_USART1_RX_DMA for clarity. --- src/main/drivers/serial_uart_stm32f10x.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index ba9128d419..5369206dc3 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -48,8 +48,10 @@ static uartPort_t uartPort3; #endif // Using RX DMA disables the use of receive callbacks -#if !defined(CC3D) // FIXME move board specific code to target.h files. #define USE_USART1_RX_DMA + +#if defined(CC3D) // FIXME move board specific code to target.h files. +#undef USE_USART1_RX_DMA #endif void uartStartTxDMA(uartPort_t *s); From f16c6fc019837863c473ac7a20e36e85598d2b96 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Fri, 30 Jan 2015 15:53:45 +1300 Subject: [PATCH 173/182] Blackbox: Log PID intermediates for pidLuxFloat Previously nothing was logged since the intermediate calculations were floats. Logging them converted to integers is better than nothing. --- src/main/flight/flight.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 26e18f618d..2c4f090c9c 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -201,8 +201,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000); - } +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = -DTerm; +#endif + } } static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, From 6a6a5b99086fe3dd8aee8d6d8459d41df928978b Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Fri, 30 Jan 2015 18:58:10 +1300 Subject: [PATCH 174/182] PID tuning docs: Fix Angle level reference for PID controller 1 --- docs/PID tuning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 4ae7fb154a..a7788134bb 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -85,7 +85,7 @@ nebbian in v1.6.0. The autotune feature does not work on this controller, so don It is the first PID Controller designed for 32-bit processors and not derived from MultiWii. The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which -is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to +is labeled "LEVEL Proportional" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to Horizon mode. The default is 5.0. The strength of the auto-leveling correction applied during Horizon mode is set by the parameter "level_horizon" which From f77a762b48c10f69b252791f062ee77c17f047b8 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 20:54:34 +0100 Subject: [PATCH 175/182] Allow inflight adjustments for floating-point based PID controllers. --- src/main/config/config.c | 7 +- src/main/config/config_profile.h | 2 - src/main/flight/autotune.c | 4 +- src/main/flight/autotune.h | 2 +- src/main/flight/flight.h | 6 +- src/main/io/rc_controls.c | 75 ++++++--- src/main/io/rc_controls.h | 3 + src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 10 +- src/main/mw.c | 2 +- src/test/unit/platform.h | 1 + src/test/unit/rc_controls_unittest.cc | 215 ++++++++++++++++++++++++++ 12 files changed, 295 insertions(+), 34 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index d74f714afc..0d8c47eebf 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -110,7 +110,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 90; +static const uint8_t EEPROM_CONF_VERSION = 91; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -121,6 +121,8 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetPidProfile(pidProfile_t *pidProfile) { + pidProfile->pidController = 0; + pidProfile->P8[ROLL] = 40; pidProfile->I8[ROLL] = 30; pidProfile->D8[ROLL] = 23; @@ -386,7 +388,6 @@ static void resetConf(void) masterConfig.looptime = 3500; masterConfig.emf_avoidance = 0; - currentProfile->pidController = 0; resetPidProfile(¤tProfile->pidProfile); resetControlRateConfig(&masterConfig.controlRateProfiles[0]); @@ -603,7 +604,7 @@ void activateConfig(void) useTelemetryConfig(&masterConfig.telemetryConfig); #endif - setPIDController(currentProfile->pidController); + setPIDController(currentProfile->pidProfile.pidController); #ifdef GPS gpsUseProfile(¤tProfile->gpsProfile); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 1aafc14bdd..7968b97c04 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -18,8 +18,6 @@ #pragma once typedef struct profile_s { - uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid - pidProfile_t pidProfile; uint8_t defaultRateProfileIndex; diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 2770b6824e..4926848d88 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -415,7 +415,7 @@ void restorePids(pidProfile_t *pidProfileToTune) memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup)); } -void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse) +void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune) { phase = nextPhase; @@ -439,7 +439,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle firstPeakAngle = secondPeakAngle = 0; pidProfile = pidProfileToTune; - pidController = pidControllerInUse; + pidController = pidProfile->pidController; updatePidIndex(); updateTargetAngle(); diff --git a/src/main/flight/autotune.h b/src/main/flight/autotune.h index 095309aaf1..8d4296e3cb 100644 --- a/src/main/flight/autotune.h +++ b/src/main/flight/autotune.h @@ -18,7 +18,7 @@ #pragma once void autotuneReset(); -void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse); +void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune); float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle); void autotuneEndPhase(); diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 51fd74a0a7..cad033b4f2 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -32,12 +32,16 @@ typedef enum { PID_ITEM_COUNT } pidIndex_e; +#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) + typedef struct pidProfile_s { + uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid + uint8_t P8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; - float P_f[3]; // float p i and d factors for the new baseflight pid + float P_f[3]; // float p i and d factors for lux float pid controller float I_f[3]; float D_f[3]; float A_level; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 64a70ba40e..5a821a5cdc 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -21,6 +21,8 @@ #include +#include "platform.h" + #include "common/axis.h" #include "common/maths.h" @@ -360,6 +362,7 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { int newValue; + float newFloatValue; if (delta > 0) { queueConfirmationBeep(2); @@ -391,34 +394,70 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c break; case ADJUSTMENT_PITCH_ROLL_P: - newValue = (int)pidProfile->P8[PIDPITCH] + delta; - pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->P8[PIDROLL] + delta; - pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; + pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; + pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->P8[PIDPITCH] + delta; + pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->P8[PIDROLL] + delta; + pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_PITCH_ROLL_I: - newValue = (int)pidProfile->I8[PIDPITCH] + delta; - pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->I8[PIDROLL] + delta; - pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; + pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; + pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->I8[PIDPITCH] + delta; + pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->I8[PIDROLL] + delta; + pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_PITCH_ROLL_D: - newValue = (int)pidProfile->D8[PIDPITCH] + delta; - pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->D8[PIDROLL] + delta; - pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; + pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; + pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->D8[PIDPITCH] + delta; + pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->D8[PIDROLL] + delta; + pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_P: - newValue = (int)pidProfile->P8[PIDYAW] + delta; - pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; + pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->P8[PIDYAW] + delta; + pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_I: - newValue = (int)pidProfile->I8[PIDYAW] + delta; - pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; + pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->I8[PIDYAW] + delta; + pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_D: - newValue = (int)pidProfile->D8[PIDYAW] + delta; - pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; + pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->D8[PIDYAW] + delta; + pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; default: break; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index a1c2a10a50..dde0536f06 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -207,7 +207,10 @@ typedef struct adjustmentState_s { uint32_t timeoutAt; } adjustmentState_t; + +#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT #define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel +#endif #define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches. diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2ec7e39f4b..a8eaf45e6c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -370,7 +370,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5622da838c..97822698a9 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -865,7 +865,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); - if (currentProfile->pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid + if (currentProfile->pidProfile.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < 3; i++) { serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250)); @@ -896,7 +896,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID_CONTROLLER: headSerialReply(1); - serialize8(currentProfile->pidController); + serialize8(currentProfile->pidProfile.pidController); break; case MSP_MODE_RANGES: headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT); @@ -1205,11 +1205,11 @@ static bool processInCommand(void) currentProfile->accelerometerTrims.values.roll = read16(); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidController = read8(); - setPIDController(currentProfile->pidController); + currentProfile->pidProfile.pidController = read8(); + setPIDController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: - if (currentProfile->pidController == 2) { + if (currentProfile->pidProfile.pidController == 2) { for (i = 0; i < 3; i++) { currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f; currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; diff --git a/src/main/mw.c b/src/main/mw.c index 0d542311c5..028177515a 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -124,7 +124,7 @@ void updateAutotuneState(void) autotuneReset(); landedAfterAutoTuning = false; } - autotuneBeginNextPhase(¤tProfile->pidProfile, currentProfile->pidController); + autotuneBeginNextPhase(¤tProfile->pidProfile); ENABLE_FLIGHT_MODE(AUTOTUNE_MODE); autoTuneWasUsed = true; } else { diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 39c8093167..f74b9be29d 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -25,3 +25,4 @@ #define SERIAL_PORT_COUNT 4 +#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 54a830278e..91ec4f515f 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -17,7 +17,9 @@ #include #include + extern "C" { + #include "platform.h" #include "common/axis.h" #include "flight/flight.h" @@ -29,6 +31,8 @@ extern "C" { #include "gtest/gtest.h" extern "C" { +extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); + int constrain(int amt, int low, int high) { if (amt < low) @@ -479,6 +483,215 @@ TEST(RcControlsTest, processRcRateProfileAdjustments) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); } +static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawPAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawIAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawDAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +TEST(RcControlsTest, processPIDIncreasePidController0) +{ + // given + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; + memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); + + escAndServoConfig_t escAndServoConfig; + memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); + + pidProfile_t pidProfile; + memset(&pidProfile, 0, sizeof (pidProfile)); + pidProfile.pidController = 0; + pidProfile.P8[PIDPITCH] = 0; + pidProfile.P8[PIDROLL] = 5; + pidProfile.P8[YAW] = 7; + pidProfile.I8[PIDPITCH] = 10; + pidProfile.I8[PIDROLL] = 15; + pidProfile.I8[YAW] = 17; + pidProfile.D8[PIDPITCH] = 20; + pidProfile.D8[PIDROLL] = 25; + pidProfile.D8[YAW] = 27; + + // and + controlRateConfig_t controlRateConfig; + memset(&controlRateConfig, 0, sizeof (controlRateConfig)); + + // and + memset(&rxConfig, 0, sizeof (rxConfig)); + rxConfig.mincheck = DEFAULT_MIN_CHECK; + rxConfig.maxcheck = DEFAULT_MAX_CHECK; + rxConfig.midrc = 1500; + + adjustmentStateMask = 0; + memset(&adjustmentStates, 0, sizeof(adjustmentStates)); + + configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig); + configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig); + configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig); + configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig); + configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig); + configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); + + // and + uint8_t index; + for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + rcData[index] = PWM_RANGE_MIDDLE; + } + + // and + resetCallCounters(); + resetMillis(); + + // and + rcData[AUX1] = PWM_RANGE_MAX; + rcData[AUX2] = PWM_RANGE_MAX; + rcData[AUX3] = PWM_RANGE_MAX; + + // and + uint8_t expectedAdjustmentStateMask = + (1 << 0) | + (1 << 1) | + (1 << 2) | + (1 << 3) | + (1 << 4) | + (1 << 5); + + // when + useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); + processRcAdjustments(&controlRateConfig, &rxConfig); + + // then + EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6); + EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); + + // and + EXPECT_EQ(pidProfile.P8[PIDPITCH], 1); + EXPECT_EQ(pidProfile.P8[PIDROLL], 6); + EXPECT_EQ(pidProfile.P8[YAW], 8); + EXPECT_EQ(pidProfile.I8[PIDPITCH], 11); + EXPECT_EQ(pidProfile.I8[PIDROLL], 16); + EXPECT_EQ(pidProfile.I8[YAW], 18); + EXPECT_EQ(pidProfile.D8[PIDPITCH], 21); + EXPECT_EQ(pidProfile.D8[PIDROLL], 26); + EXPECT_EQ(pidProfile.D8[YAW], 28); +} + +TEST(RcControlsTest, processPIDIncreasePidController2) +{ + // given + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; + memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); + + escAndServoConfig_t escAndServoConfig; + memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); + + pidProfile_t pidProfile; + memset(&pidProfile, 0, sizeof (pidProfile)); + pidProfile.pidController = 2; + pidProfile.P_f[PIDPITCH] = 0.0f; + pidProfile.P_f[PIDROLL] = 5.0f; + pidProfile.P_f[PIDYAW] = 7.0f; + pidProfile.I_f[PIDPITCH] = 10.0f; + pidProfile.I_f[PIDROLL] = 15.0f; + pidProfile.I_f[PIDYAW] = 17.0f; + pidProfile.D_f[PIDPITCH] = 20.0f; + pidProfile.D_f[PIDROLL] = 25.0f; + pidProfile.D_f[PIDYAW] = 27.0f; + + // and + controlRateConfig_t controlRateConfig; + memset(&controlRateConfig, 0, sizeof (controlRateConfig)); + + // and + memset(&rxConfig, 0, sizeof (rxConfig)); + rxConfig.mincheck = DEFAULT_MIN_CHECK; + rxConfig.maxcheck = DEFAULT_MAX_CHECK; + rxConfig.midrc = 1500; + + adjustmentStateMask = 0; + memset(&adjustmentStates, 0, sizeof(adjustmentStates)); + + configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig); + configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig); + configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig); + configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig); + configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig); + configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); + + // and + uint8_t index; + for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + rcData[index] = PWM_RANGE_MIDDLE; + } + + // and + resetCallCounters(); + resetMillis(); + + // and + rcData[AUX1] = PWM_RANGE_MAX; + rcData[AUX2] = PWM_RANGE_MAX; + rcData[AUX3] = PWM_RANGE_MAX; + + // and + uint8_t expectedAdjustmentStateMask = + (1 << 0) | + (1 << 1) | + (1 << 2) | + (1 << 3) | + (1 << 4) | + (1 << 5); + + // when + useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); + processRcAdjustments(&controlRateConfig, &rxConfig); + + // then + EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6); + EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); + + // and + EXPECT_EQ(pidProfile.P_f[PIDPITCH], 1.0f); + EXPECT_EQ(pidProfile.P_f[PIDROLL], 6.0f); + EXPECT_EQ(pidProfile.P_f[PIDYAW], 8.0f); + EXPECT_EQ(pidProfile.I_f[PIDPITCH], 11.0f); + EXPECT_EQ(pidProfile.I_f[PIDROLL], 16.0f); + EXPECT_EQ(pidProfile.I_f[PIDYAW], 18.0f); + EXPECT_EQ(pidProfile.D_f[PIDPITCH], 21.0f); + EXPECT_EQ(pidProfile.D_f[PIDROLL], 26.0f); + EXPECT_EQ(pidProfile.D_f[PIDYAW], 28.0f); + +} + extern "C" { void saveConfigAndNotify(void) {} void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {} @@ -494,6 +707,8 @@ void mwDisarm(void) {} uint8_t getCurrentControlRateProfile(void) { return 0; } +void GPS_reset_home_position(void) {} +void baroSetCalibrationCycles(uint16_t) {} uint8_t armingFlags = 0; int16_t heading; From fa1894008745f47787a6dccacfd3d952ca9007f2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:25:18 +0100 Subject: [PATCH 176/182] Minor cleanups to allow CJMCU to build again. --- Makefile | 2 -- src/main/config/config_master.h | 2 ++ src/main/io/serial_cli.c | 6 ++++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index a62cc1ab8e..aadb9ab50c 100644 --- a/Makefile +++ b/Makefile @@ -395,8 +395,6 @@ CJMCU_SRC = \ drivers/system_stm32f10x.c \ drivers/timer.c \ drivers/timer_stm32f10x.c \ - blackbox/blackbox.c \ - blackbox/blackbox_io.c \ hardware_revision.c \ $(COMMON_SRC) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index bacb7b5a7b..b99770729b 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -85,8 +85,10 @@ typedef struct master_t { uint8_t current_profile_index; controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT]; +#ifdef BLACKBOX uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; +#endif uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a8eaf45e6c..0735c87bd3 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -163,7 +163,7 @@ const clicmd_t cmdTable[] = { { "color", "configure colors", cliColor }, #endif { "defaults", "reset to defaults and reboot", cliDefaults }, - { "dump", "print configurable settings in a pastable form", cliDump }, + { "dump", "dump configuration", cliDump }, { "exit", "", cliExit }, { "feature", "list or -val or val", cliFeature }, { "get", "get variable value", cliGet }, @@ -249,7 +249,7 @@ const clivalue_t valueTable[] = { { "serial_port_2_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[1], 0, SERIAL_PORT_SCENARIO_MAX }, #if (SERIAL_PORT_COUNT > 2) { "serial_port_3_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[2], 0, SERIAL_PORT_SCENARIO_MAX }, -#if !defined(CC3D) +#if (SERIAL_PORT_COUNT > 3) { "serial_port_4_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[3], 0, SERIAL_PORT_SCENARIO_MAX }, #if (SERIAL_PORT_COUNT > 4) { "serial_port_5_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[4], 0, SERIAL_PORT_SCENARIO_MAX }, @@ -408,8 +408,10 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, +#ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, +#endif }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) From ea386e6da2f15e1a7839901fa51add710c39c10d Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:27:09 +0100 Subject: [PATCH 177/182] Remove magic number usage. Fix limits for FP based pid controller PID adjustments to match those in serial_cli.c. --- src/main/flight/autotune.c | 4 ++-- src/main/io/rc_controls.c | 18 +++++++++--------- src/main/io/serial_msp.c | 4 ++-- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 4926848d88..33974931ba 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -226,8 +226,8 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin return errorAngle; } - if (pidController == 2) { - // TODO support new baseflight pid controller + if (IS_PID_CONTROLLER_FP_BASED(pidController) { + // TODO support floating point based pid controllers return errorAngle; } diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 5a821a5cdc..203ed2b928 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -396,9 +396,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_PITCH_ROLL_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; - pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; - pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDPITCH] + delta; pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -409,9 +409,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_PITCH_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; - pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; - pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDPITCH] + delta; pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -422,9 +422,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_PITCH_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; - pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; - pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDPITCH] + delta; pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -435,7 +435,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_YAW_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; - pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDYAW] + delta; pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -444,7 +444,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; - pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDYAW] + delta; pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -453,7 +453,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; - pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDYAW] + delta; pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 97822698a9..d0677566c7 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -865,7 +865,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); - if (currentProfile->pidProfile.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < 3; i++) { serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250)); @@ -1209,7 +1209,7 @@ static bool processInCommand(void) setPIDController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: - if (currentProfile->pidProfile.pidController == 2) { + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { for (i = 0; i < 3; i++) { currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f; currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; From 2ac72823142ea294c6b1fc54e1115da4cf468884 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:46:23 +0100 Subject: [PATCH 178/182] Use different scale for inflight adjustments to PIDs of FP based PID controllers. --- src/main/io/rc_controls.c | 38 ++++++++++++---------- src/test/Makefile | 1 + src/test/unit/rc_controls_unittest.cc | 47 +++++++++++---------------- 3 files changed, 40 insertions(+), 46 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 203ed2b928..b73a867f15 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -32,10 +32,12 @@ #include "drivers/system.h" #include "flight/flight.h" +#include "flight/navigation.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "sensors/barometer.h" #include "sensors/battery.h" #include "sensors/sensors.h" #include "sensors/gyro.h" @@ -395,10 +397,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; - pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; - pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDPITCH] + delta; pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -408,10 +410,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; - pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; - pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDPITCH] + delta; pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -421,10 +423,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; - pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; - pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDPITCH] + delta; pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -434,8 +436,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; - pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDYAW] + delta; pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -443,8 +445,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; - pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDYAW] + delta; pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -452,8 +454,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; - pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDYAW] + delta; pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c diff --git a/src/test/Makefile b/src/test/Makefile index 6642d98b8f..9f3c5db21e 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -263,6 +263,7 @@ $(OBJECT_DIR)/rc_controls_unittest.o : \ $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@ rc_controls_unittest : \ + $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/io/rc_controls.o \ $(OBJECT_DIR)/rc_controls_unittest.o \ $(OBJECT_DIR)/gtest_main.a diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 91ec4f515f..3e611ce1ed 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -20,6 +20,7 @@ extern "C" { #include "platform.h" + #include "common/maths.h" #include "common/axis.h" #include "flight/flight.h" @@ -32,16 +33,6 @@ extern "C" { extern "C" { extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); - -int constrain(int amt, int low, int high) -{ - if (amt < low) - return low; - else if (amt > high) - return high; - else - return amt; -} } TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde) @@ -594,15 +585,15 @@ TEST(RcControlsTest, processPIDIncreasePidController0) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // and - EXPECT_EQ(pidProfile.P8[PIDPITCH], 1); - EXPECT_EQ(pidProfile.P8[PIDROLL], 6); - EXPECT_EQ(pidProfile.P8[YAW], 8); - EXPECT_EQ(pidProfile.I8[PIDPITCH], 11); - EXPECT_EQ(pidProfile.I8[PIDROLL], 16); - EXPECT_EQ(pidProfile.I8[YAW], 18); - EXPECT_EQ(pidProfile.D8[PIDPITCH], 21); - EXPECT_EQ(pidProfile.D8[PIDROLL], 26); - EXPECT_EQ(pidProfile.D8[YAW], 28); + EXPECT_EQ(1, pidProfile.P8[PIDPITCH]); + EXPECT_EQ(6, pidProfile.P8[PIDROLL]); + EXPECT_EQ(8, pidProfile.P8[YAW]); + EXPECT_EQ(11, pidProfile.I8[PIDPITCH]); + EXPECT_EQ(16, pidProfile.I8[PIDROLL]); + EXPECT_EQ(18, pidProfile.I8[YAW]); + EXPECT_EQ(21, pidProfile.D8[PIDPITCH]); + EXPECT_EQ(26, pidProfile.D8[PIDROLL]); + EXPECT_EQ(28, pidProfile.D8[YAW]); } TEST(RcControlsTest, processPIDIncreasePidController2) @@ -680,15 +671,15 @@ TEST(RcControlsTest, processPIDIncreasePidController2) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // and - EXPECT_EQ(pidProfile.P_f[PIDPITCH], 1.0f); - EXPECT_EQ(pidProfile.P_f[PIDROLL], 6.0f); - EXPECT_EQ(pidProfile.P_f[PIDYAW], 8.0f); - EXPECT_EQ(pidProfile.I_f[PIDPITCH], 11.0f); - EXPECT_EQ(pidProfile.I_f[PIDROLL], 16.0f); - EXPECT_EQ(pidProfile.I_f[PIDYAW], 18.0f); - EXPECT_EQ(pidProfile.D_f[PIDPITCH], 21.0f); - EXPECT_EQ(pidProfile.D_f[PIDROLL], 26.0f); - EXPECT_EQ(pidProfile.D_f[PIDYAW], 28.0f); + EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]); + EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]); + EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]); + EXPECT_EQ(10.1f, pidProfile.I_f[PIDPITCH]); + EXPECT_EQ(15.1f, pidProfile.I_f[PIDROLL]); + EXPECT_EQ(17.1f, pidProfile.I_f[PIDYAW]); + EXPECT_EQ(20.1f, pidProfile.D_f[PIDPITCH]); + EXPECT_EQ(25.1f, pidProfile.D_f[PIDROLL]); + EXPECT_EQ(27.1f, pidProfile.D_f[PIDYAW]); } From de9a89ca85a2e2cf4bd7cb0cc05c26bd444adaf2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:47:29 +0100 Subject: [PATCH 179/182] Fix missing ) from ea386e6da2f15e1a7839901fa51add710c39c10d --- src/main/flight/autotune.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 33974931ba..189d9da248 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -226,7 +226,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin return errorAngle; } - if (IS_PID_CONTROLLER_FP_BASED(pidController) { + if (IS_PID_CONTROLLER_FP_BASED(pidController)) { // TODO support floating point based pid controllers return errorAngle; } From 1f3c9ab0dbf859b8fc4e05bc254ed3ac90f987d1 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 22:02:25 +0100 Subject: [PATCH 180/182] Fix failing targets. --- src/main/config/config.c | 2 +- src/main/target/OLIMEXINO/target.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 0d8c47eebf..eb5465c834 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -472,7 +472,7 @@ static void resetConf(void) masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; masterConfig.looptime = 2000; - currentProfile->pidController = 3; + currentProfile->pidProfile.pidController = 3; currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[PITCH] = 36; currentProfile->failsafeConfig.failsafe_delay = 2; diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 34ec32a7dc..87dd9aadd6 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -113,4 +113,4 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE - +#define BLACKBOX From d21a009d405262e8d8fc3fcee30dec8c46956fa0 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 22:09:36 +0100 Subject: [PATCH 181/182] Change scale for I and D for inflight adjustments to PIDs of FP based PID. --- src/main/io/rc_controls.c | 12 ++++++------ src/test/unit/rc_controls_unittest.cc | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index b73a867f15..9c79231c11 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -410,9 +410,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 10.0f); + newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f); pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 10.0f); + newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f); pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDPITCH] + delta; @@ -423,9 +423,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 10.0f); + newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f); pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 10.0f); + newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f); pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDPITCH] + delta; @@ -445,7 +445,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 10.0f); + newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f); pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDYAW] + delta; @@ -454,7 +454,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 10.0f); + newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f); pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDYAW] + delta; diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 3e611ce1ed..c97348bffe 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -674,12 +674,12 @@ TEST(RcControlsTest, processPIDIncreasePidController2) EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]); EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]); EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]); - EXPECT_EQ(10.1f, pidProfile.I_f[PIDPITCH]); - EXPECT_EQ(15.1f, pidProfile.I_f[PIDROLL]); - EXPECT_EQ(17.1f, pidProfile.I_f[PIDYAW]); - EXPECT_EQ(20.1f, pidProfile.D_f[PIDPITCH]); - EXPECT_EQ(25.1f, pidProfile.D_f[PIDROLL]); - EXPECT_EQ(27.1f, pidProfile.D_f[PIDYAW]); + EXPECT_EQ(10.01f, pidProfile.I_f[PIDPITCH]); + EXPECT_EQ(15.01f, pidProfile.I_f[PIDROLL]); + EXPECT_EQ(17.01f, pidProfile.I_f[PIDYAW]); + EXPECT_EQ(20.001f, pidProfile.D_f[PIDPITCH]); + EXPECT_EQ(25.001f, pidProfile.D_f[PIDROLL]); + EXPECT_EQ(27.001f, pidProfile.D_f[PIDYAW]); } From c85919325cc98b5fafab1204dade047835380218 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 22:43:04 +0100 Subject: [PATCH 182/182] Exclude a bit more code when USE_ADC is not defined for a target. --- src/main/drivers/adc.c | 10 ++++++++++ src/main/rx/rx.c | 8 +++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 4215df21d4..eae2433401 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -18,11 +18,14 @@ #include #include +#include "build_config.h" + #include "platform.h" #include "system.h" #include "adc.h" +#ifdef USE_ADC adc_config_t adcConfig[ADC_CHANNEL_COUNT]; volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; @@ -47,3 +50,10 @@ uint16_t adcGetChannel(uint8_t channel) return adcValues[adcConfig[channel].dmaIndex]; } +#else +uint16_t adcGetChannel(uint8_t channel) +{ + UNUSED(channel); + return 0; +} +#endif diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 17ce1f7a13..07ea188d67 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -21,6 +21,8 @@ #include +#include "build_config.h" + #include "platform.h" #include "common/maths.h" @@ -59,7 +61,7 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR const char rcChannelLetters[] = "AERT12345678abcdefgh"; -uint16_t rssi; // range: [0;1023] +uint16_t rssi = 0; // range: [0;1023] int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] @@ -377,6 +379,9 @@ void updateRSSIPWM(void) void updateRSSIADC(uint32_t currentTime) { +#ifndef USE_ADC + UNUSED(currentTime); +#else static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT]; static uint8_t adcRssiSampleIndex = 0; static uint32_t rssiUpdateAt = 0; @@ -403,6 +408,7 @@ void updateRSSIADC(uint32_t currentTime) adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT; rssi = (uint16_t)((constrain(adcRssiMean, 0, 100) / 100.0f) * 1023.0f); +#endif } void updateRSSI(uint32_t currentTime)