From 84729747b0de49bf5129d4f079d5f928a57fa28e Mon Sep 17 00:00:00 2001 From: dongie Date: Wed, 11 Jun 2014 12:52:03 +0900 Subject: [PATCH 1/5] allow controlling external XOR gate connected to PB2 (BOOT1) to invert USART2_RX signal on F1. Currently used by sbus driver. --- src/board.h | 14 ++++++++++++++ src/cli.c | 6 ++++++ src/config.c | 10 ++++++++-- src/drv_system.c | 9 +++++++++ src/mw.h | 12 +++++++++--- src/sbus.c | 11 +++++++++++ 6 files changed, 57 insertions(+), 5 deletions(-) diff --git a/src/board.h b/src/board.h index cfc16333dd..a2a42e3e16 100755 --- a/src/board.h +++ b/src/board.h @@ -1,3 +1,7 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ #pragma once // for roundf() @@ -208,6 +212,8 @@ typedef struct baro_t { #define BEEP_PIN Pin_12 // PA12 (Buzzer) #define BARO_GPIO GPIOC #define BARO_PIN Pin_13 +#define INV_PIN Pin_2 // PB2 (BOOT1) abused as inverter select GPIO +#define INV_GPIO GPIOB #define GYRO #define ACC @@ -255,6 +261,14 @@ typedef struct baro_t { #define BEEP_ON ; #endif +#ifdef INV_GPIO +#define INV_OFF digitalLo(INV_GPIO, INV_PIN); +#define INV_ON digitalHi(INV_GPIO, INV_PIN); +#else +#define INV_OFF ; +#define INV_ON ; +#endif + // #define SOFT_I2C // enable to test software i2c #include "utils.h" diff --git a/src/cli.c b/src/cli.c index a8f9141928..159c4ed9a8 100644 --- a/src/cli.c +++ b/src/cli.c @@ -1,3 +1,8 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ + #include "board.h" #include "mw.h" @@ -131,6 +136,7 @@ const clivalue_t valueTable[] = { { "fixedwing_althold_dir", VAR_INT8, &mcfg.fixedwing_althold_dir, -1, 1 }, { "reboot_character", VAR_UINT8, &mcfg.reboot_character, 48, 126 }, { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 }, + { "serial2_rx_inverted", VAR_UINT8, &mcfg.serial2_rx_inverted, 0, 1 }, { "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 1200, 19200 }, { "softserial_1_inverted", VAR_UINT8, &mcfg.softserial_1_inverted, 0, 1 }, { "softserial_2_inverted", VAR_UINT8, &mcfg.softserial_2_inverted, 0, 1 }, diff --git a/src/config.c b/src/config.c index 7703528826..08437efdab 100755 --- a/src/config.c +++ b/src/config.c @@ -1,3 +1,8 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ + #include "board.h" #include "mw.h" #include @@ -13,7 +18,7 @@ master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static const uint8_t EEPROM_CONF_VERSION = 64; +static const uint8_t EEPROM_CONF_VERSION = 65; static uint32_t enabledSensors = 0; static void resetConf(void); @@ -125,7 +130,8 @@ void writeEEPROM(uint8_t b, uint8_t updateProfile) while (tries--) { FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); - status = FLASH_ErasePage(FLASH_WRITE_ADDR); + FLASH_ErasePage(FLASH_WRITE_ADDR); + status = FLASH_ErasePage(FLASH_WRITE_ADDR + FLASH_PAGE_SIZE); for (i = 0; i < sizeof(master_t) && status == FLASH_COMPLETE; i += 4) status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *)((char *)&mcfg + i)); if (status == FLASH_COMPLETE) diff --git a/src/drv_system.c b/src/drv_system.c index 4607170921..37faf1092d 100755 --- a/src/drv_system.c +++ b/src/drv_system.c @@ -1,3 +1,8 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ + #include "board.h" // cycles per microsecond @@ -68,6 +73,10 @@ void systemInit(bool overclock) .cfg = { BEEP_PIN, Mode_Out_OD, Speed_2MHz } }, #endif + { + .gpio = INV_GPIO, + .cfg = { INV_PIN, Mode_Out_PP, Speed_2MHz } + }, }; gpio_config_t gpio; uint32_t i; diff --git a/src/mw.h b/src/mw.h index 53946ec8a9..5de2811e9f 100755 --- a/src/mw.h +++ b/src/mw.h @@ -1,3 +1,8 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ + #pragma once /* for VBAT monitoring frequency */ @@ -281,10 +286,11 @@ typedef struct master_t { int8_t gps_baudrate; // See GPSBaudRates enum. uint32_t serial_baudrate; + uint8_t serial2_rx_inverted; // invert RX only for USART2 (for Sbus, etc) - uint32_t softserial_baudrate; // shared by both soft serial ports - uint8_t softserial_1_inverted; // use inverted softserial input and output signals on port 1 - uint8_t softserial_2_inverted; // use inverted softserial input and output signals on port 2 + uint32_t softserial_baudrate; // shared by both soft serial ports + uint8_t softserial_1_inverted; // use inverted softserial input and output signals on port 1 + uint8_t softserial_2_inverted; // use inverted softserial input and output signals on port 2 uint8_t telemetry_provider; // See TelemetryProvider enum. uint8_t telemetry_port; // See TelemetryPort enum. diff --git a/src/sbus.c b/src/sbus.c index 1449e225ea..9620231b8a 100644 --- a/src/sbus.c +++ b/src/sbus.c @@ -1,3 +1,8 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ + #include "board.h" #include "mw.h" @@ -22,6 +27,12 @@ void sbusInit(rcReadRawDataPtr *callback) int b; for (b = 0; b < SBUS_MAX_CHANNEL; b++) sbusChannelData[b] = 2 * (mcfg.midrc - SBUS_OFFSET); + // Configure hardware inverter on PB2. If not available, this has no effect. + if (mcfg.serial2_rx_inverted) { + INV_ON; + } else { + INV_OFF; + } core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS)); if (callback) *callback = sbusReadRawRC; From ab87a992389c1dd6d997d7a1189d8f624e26b64a Mon Sep 17 00:00:00 2001 From: dongie Date: Wed, 11 Jun 2014 14:13:45 +0900 Subject: [PATCH 2/5] On second thought, no need to make inverter configurable. only sbus will use it, and the driver can set inversion automatically. --- src/cli.c | 1 - src/config.c | 2 +- src/mw.h | 3 +-- src/sbus.c | 6 +----- 4 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/cli.c b/src/cli.c index 159c4ed9a8..8bdf524ebc 100644 --- a/src/cli.c +++ b/src/cli.c @@ -136,7 +136,6 @@ const clivalue_t valueTable[] = { { "fixedwing_althold_dir", VAR_INT8, &mcfg.fixedwing_althold_dir, -1, 1 }, { "reboot_character", VAR_UINT8, &mcfg.reboot_character, 48, 126 }, { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 }, - { "serial2_rx_inverted", VAR_UINT8, &mcfg.serial2_rx_inverted, 0, 1 }, { "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 1200, 19200 }, { "softserial_1_inverted", VAR_UINT8, &mcfg.softserial_1_inverted, 0, 1 }, { "softserial_2_inverted", VAR_UINT8, &mcfg.softserial_2_inverted, 0, 1 }, diff --git a/src/config.c b/src/config.c index 08437efdab..5de9b5c05d 100755 --- a/src/config.c +++ b/src/config.c @@ -18,7 +18,7 @@ master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static const uint8_t EEPROM_CONF_VERSION = 65; +static const uint8_t EEPROM_CONF_VERSION = 64; static uint32_t enabledSensors = 0; static void resetConf(void); diff --git a/src/mw.h b/src/mw.h index 5de2811e9f..54a29e4c2d 100755 --- a/src/mw.h +++ b/src/mw.h @@ -285,8 +285,7 @@ typedef struct master_t { uint8_t gps_type; // See GPSHardware enum. int8_t gps_baudrate; // See GPSBaudRates enum. - uint32_t serial_baudrate; - uint8_t serial2_rx_inverted; // invert RX only for USART2 (for Sbus, etc) + uint32_t serial_baudrate; // primary serial (MSP) port baudrate uint32_t softserial_baudrate; // shared by both soft serial ports uint8_t softserial_1_inverted; // use inverted softserial input and output signals on port 1 diff --git a/src/sbus.c b/src/sbus.c index 9620231b8a..7a38f84eb8 100644 --- a/src/sbus.c +++ b/src/sbus.c @@ -28,11 +28,7 @@ void sbusInit(rcReadRawDataPtr *callback) for (b = 0; b < SBUS_MAX_CHANNEL; b++) sbusChannelData[b] = 2 * (mcfg.midrc - SBUS_OFFSET); // Configure hardware inverter on PB2. If not available, this has no effect. - if (mcfg.serial2_rx_inverted) { - INV_ON; - } else { - INV_OFF; - } + INV_ON; core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS)); if (callback) *callback = sbusReadRawRC; From 2fef9d5fa97e957aa8731e42186deb2c9fbaca8e Mon Sep 17 00:00:00 2001 From: dongie Date: Wed, 11 Jun 2014 15:10:17 +0900 Subject: [PATCH 3/5] Fixing gimbal_flags not working when feature SERVO_TILT was enabled. This was done by properly calculating servo offset instead of depending on enabled features/mixers. --- src/drv_pwm.c | 11 ++++++++++- src/drv_pwm.h | 7 +++++++ src/main.c | 5 +++++ src/mixer.c | 11 ++++++----- src/mw.c | 6 ++++-- src/mw.h | 1 + 6 files changed, 33 insertions(+), 8 deletions(-) diff --git a/src/drv_pwm.c b/src/drv_pwm.c index a0ebc4fe9f..c2ee5c28b5 100755 --- a/src/drv_pwm.c +++ b/src/drv_pwm.c @@ -1,3 +1,7 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ #include "board.h" /* @@ -361,7 +365,8 @@ bool pwmInit(drv_pwm_config_t *init) } if (init->extraServos && !init->airplane) { - // remap PWM5..8 as servos when used in extended servo mode + // remap PWM5..8 as servos when used in extended servo mode. + // condition for airplane because airPPM already has these as servos if (port >= PWM5 && port <= PWM8) mask = TYPE_S; } @@ -390,6 +395,10 @@ bool pwmInit(drv_pwm_config_t *init) pwmWritePtr = pwmWriteStandard; if (init->motorPwmRate > 500) pwmWritePtr = pwmWriteBrushed; + + // set return values in init struct + init->numServos = numServos; + return false; } diff --git a/src/drv_pwm.h b/src/drv_pwm.h index 918d88f401..e7d7d98216 100755 --- a/src/drv_pwm.h +++ b/src/drv_pwm.h @@ -1,3 +1,7 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ #pragma once #define MAX_MOTORS 12 @@ -22,6 +26,9 @@ typedef struct drv_pwm_config_t { // some higher value (used by 3d mode), or 0, for brushed pwm drivers. uint16_t servoCenterPulse; uint16_t failsafeThreshold; + + // OUT parameters, filled by driver + uint8_t numServos; } drv_pwm_config_t; // This indexes into the read-only hardware definition structure in drv_pwm.c, as well as into pwmPorts[] structure with dynamic data. diff --git a/src/main.c b/src/main.c index 31e7274972..fac6f84664 100755 --- a/src/main.c +++ b/src/main.c @@ -1,3 +1,7 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ #include "board.h" #include "mw.h" @@ -126,6 +130,7 @@ int main(void) } pwmInit(&pwm_params); + core.numServos = pwm_params.numServos; // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled for (i = 0; i < RC_CHANS; i++) diff --git a/src/mixer.c b/src/mixer.c index cfbff6e07f..a565ce8e56 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -1,3 +1,7 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ #include "board.h" #include "mw.h" @@ -468,13 +472,10 @@ void mixTable(void) // forward AUX1-4 to servo outputs (not constrained) if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { - int offset = 0; + int offset = core.numServos - 4; // offset servos based off number already used in mixer types // airplane and servo_tilt together can't be used - if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) - offset = 4; - else if (mixers[mcfg.mixerConfiguration].useServo) - offset = 2; + // calculate offset by taking 4 from core.numServos for (i = 0; i < 4; i++) pwmWriteServo(i + offset, rcData[AUX1 + i]); } diff --git a/src/mw.c b/src/mw.c index 4c73235984..4ca0a0db89 100755 --- a/src/mw.c +++ b/src/mw.c @@ -1,3 +1,7 @@ +/* + * This file is part of baseflight + * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md + */ #include "board.h" #include "mw.h" @@ -97,8 +101,6 @@ void annexCode(void) static int64_t mAhdrawnRaw = 0; static int32_t vbatCycleTime = 0; - int i; - // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < cfg.tpa_breakpoint) { prop2 = 100; diff --git a/src/mw.h b/src/mw.h index 54a29e4c2d..7d27750cf1 100755 --- a/src/mw.h +++ b/src/mw.h @@ -311,6 +311,7 @@ typedef struct core_t { uint8_t mpu6050_scale; // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. automatically set by mpu6050 driver. uint8_t numRCChannels; // number of rc channels as reported by current input driver bool useServo; // feature SERVO_TILT or wing/airplane mixers will enable this + uint8_t numServos; // how many total hardware servos we have. used by mixer } core_t; typedef struct flags_t { From 6eeb86fe68abbc8a7a1278298fb1e34f84a29152 Mon Sep 17 00:00:00 2001 From: Trey Marc Date: Thu, 12 Jun 2014 03:42:49 +0200 Subject: [PATCH 4/5] remove unnecessary variable --- src/imu.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/imu.c b/src/imu.c index 11cadfb949..7c6d341712 100644 --- a/src/imu.c +++ b/src/imu.c @@ -121,7 +121,7 @@ void rotateV(struct fp_vector *v, float *delta) // 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, coszcosy, sinzcosx, coszsinx, sinzsinx; + float coszcosx, sinzcosx, coszsinx, sinzsinx; cosx = cosf(delta[ROLL]); sinx = sinf(delta[ROLL]); @@ -131,12 +131,11 @@ void rotateV(struct fp_vector *v, float *delta) sinz = sinf(delta[YAW]); coszcosx = cosz * cosx; - coszcosy = cosz * cosy; sinzcosx = sinz * cosx; coszsinx = sinx * cosz; sinzsinx = sinx * sinz; - mat[0][0] = coszcosy; + mat[0][0] = cosz * cosy; mat[0][1] = -cosy * sinz; mat[0][2] = siny; mat[1][0] = sinzcosx + (coszsinx * siny); From 4ccf31d5149d02f3871138bbdc9dd5d9f1e747b8 Mon Sep 17 00:00:00 2001 From: Trey Marc Date: Thu, 12 Jun 2014 03:50:16 +0200 Subject: [PATCH 5/5] unroll loop for better code size gcc remove 29 bytes here --- src/imu.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/imu.c b/src/imu.c index 11cadfb949..5896622e37 100644 --- a/src/imu.c +++ b/src/imu.c @@ -62,12 +62,11 @@ void computeIMU(void) if (mcfg.mixerConfiguration == MULTITYPE_TRI) { gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3; gyroYawSmooth = gyroData[YAW]; - gyroData[ROLL] = gyroADC[ROLL]; - gyroData[PITCH] = gyroADC[PITCH]; } else { - for (axis = 0; axis < 3; axis++) - gyroData[axis] = gyroADC[axis]; + gyroData[YAW] = gyroADC[YAW]; } + gyroData[ROLL] = gyroADC[ROLL]; + gyroData[PITCH] = gyroADC[PITCH]; } // **************************************************