From 0491eae52e8b7361f0e4ea553bddd88187a40849 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 10 Nov 2017 13:49:48 +0000 Subject: [PATCH 01/32] Update PID calculations to use actual deltaT --- src/main/flight/pid.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3b87ab358a..88ce802c18 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -34,6 +34,7 @@ #include "config/parameter_group_ids.h" #include "drivers/sound_beeper.h" +#include "drivers/time.h" #include "fc/fc_core.h" #include "fc/fc_rc.h" @@ -409,6 +410,14 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an const float tpaFactor = getThrottlePIDAttenuation(); const float motorMixRange = getMotorMixRange(); static timeUs_t crashDetectedAtUs; + static timeUs_t previousTime; + + // calculate actual deltaT +#ifndef SITL + currentTimeUs = microsISR(); // re-get current time, since there can be variations in timing since scheduler invocation +#endif + const float deltaT = currentTimeUs - previousTime; + previousTime = currentTimeUs; // Dynamic ki component to gradually scale back integration when above windup point const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); @@ -491,7 +500,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate I component const float ITerm = axisPID_I[axis]; - const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator, -itermLimit, itermLimit); + const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * deltaT * dynKi * itermAccelerator, -itermLimit, itermLimit); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { // Only increase ITerm if output is not saturated @@ -509,8 +518,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); } const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y - // Divide rate change by dT to get differential (ie dr/dt) - float delta = (rD - previousRateError[axis]) / dT; + // Divide rate change by deltaT to get differential (ie dr/dt) + float delta = (rD - previousRateError[axis]) / deltaT; previousRateError[axis] = rD; From 56ed140e330b6677d1cc7b30768cc1b86da88ecd Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 12 Nov 2017 12:49:12 +0000 Subject: [PATCH 02/32] Updated to use currentTimeUs, which is time of gyro sampling --- src/main/flight/pid.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 88ce802c18..3a5beae970 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -413,9 +413,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an static timeUs_t previousTime; // calculate actual deltaT -#ifndef SITL - currentTimeUs = microsISR(); // re-get current time, since there can be variations in timing since scheduler invocation -#endif const float deltaT = currentTimeUs - previousTime; previousTime = currentTimeUs; From 03d1409fd273a6b68066e16848b46bd1466b2292 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 10 Nov 2017 07:27:35 +0000 Subject: [PATCH 03/32] Remove accgyro mpu read, write and ISR update function pointers --- src/main/drivers/accgyro/accgyro.h | 13 ++- src/main/drivers/accgyro/accgyro_mpu.c | 91 ++++++------------- src/main/drivers/accgyro/accgyro_mpu.h | 5 - src/main/drivers/accgyro/accgyro_mpu3050.c | 19 ++-- src/main/drivers/accgyro/accgyro_mpu6050.c | 16 ++-- src/main/drivers/accgyro/accgyro_mpu6500.c | 22 ++--- .../drivers/accgyro/accgyro_spi_icm20649.c | 26 +++--- .../drivers/accgyro/accgyro_spi_icm20689.c | 22 ++--- src/main/drivers/bus_spi.c | 1 + src/main/drivers/sensor.h | 1 - 10 files changed, 85 insertions(+), 131 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index f6df5875a0..5908c69d05 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -53,7 +53,6 @@ typedef struct gyroDev_s { sensorGyroInitFuncPtr initFn; // initialize function sensorGyroReadFuncPtr readFn; // read 3 axis data function sensorGyroReadDataFuncPtr temperatureFn; // read temperature if available - sensorGyroUpdateFuncPtr updateFn; extiCallbackRec_t exti; busDevice_t bus; float scale; // scalefactor @@ -66,14 +65,14 @@ typedef struct gyroDev_s { gyroRateKHz_e gyroRateKHz; uint8_t mpuDividerDrops; bool dataReady; -#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) - pthread_mutex_t lock; -#endif sensor_align_e gyroAlign; mpuDetectionResult_t mpuDetectionResult; ioTag_t mpuIntExtiTag; mpuConfiguration_t mpuConfiguration; bool gyro_high_fsr; +#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) + pthread_mutex_t lock; +#endif } gyroDev_t; typedef struct accDev_s { @@ -84,13 +83,13 @@ typedef struct accDev_s { int16_t ADCRaw[XYZ_AXIS_COUNT]; char revisionCode; // a revision code for the sensor, if known bool dataReady; -#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) - pthread_mutex_t lock; -#endif sensor_align_e accAlign; mpuDetectionResult_t mpuDetectionResult; mpuConfiguration_t mpuConfiguration; bool acc_high_fsr; +#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) + pthread_mutex_t lock; +#endif } accDev_t; static inline void accDevLock(accDev_t *acc) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index ed7e59451f..9ec2e64dd7 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -64,23 +64,18 @@ mpuResetFnPtr mpuResetFn; #define MPU_INQUIRY_MASK 0x7E +#ifdef USE_I2C static void mpu6050FindRevision(gyroDev_t *gyro) { - bool ack; - UNUSED(ack); - - uint8_t readBuffer[6]; - uint8_t revision; - uint8_t productId; - // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c - // determine product ID and accel revision - ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_XA_OFFS_H, readBuffer, 6); - revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); - if (revision) { - /* Congrats, these parts are better. */ + // determine product ID and revision + uint8_t readBuffer[6]; + bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_XA_OFFS_H, readBuffer, 6); + uint8_t revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); + if (ack && revision) { + // Congrats, these parts are better if (revision == 1) { gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; } else if (revision == 2) { @@ -91,9 +86,10 @@ static void mpu6050FindRevision(gyroDev_t *gyro) failureMode(FAILURE_ACC_INCOMPATIBLE); } } else { - ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_PRODUCT_ID, &productId, 1); + uint8_t productId; + ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_PRODUCT_ID, &productId, 1); revision = productId & 0x0F; - if (!revision) { + if (!ack || revision == 0) { failureMode(FAILURE_ACC_INCOMPATIBLE); } else if (revision == 4) { gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; @@ -102,6 +98,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) } } } +#endif /* * Gyro interrupt service routine @@ -117,19 +114,14 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb) #endif gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); gyro->dataReady = true; - if (gyro->updateFn) { - gyro->updateFn(gyro); - } #ifdef DEBUG_MPU_DATA_READY_INTERRUPT const uint32_t now2Us = micros(); debug[1] = (uint16_t)(now2Us - nowUs); #endif } -#endif static void mpuIntExtiInit(gyroDev_t *gyro) { -#if defined(MPU_INT_EXTI) if (gyro->mpuIntExtiTag == IO_TAG_NONE) { return; } @@ -156,31 +148,14 @@ static void mpuIntExtiInit(gyroDev_t *gyro) EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); EXTIEnable(mpuIntIO, true); #endif - -#else - UNUSED(gyro); -#endif -} - -static bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t* data, uint8_t length) -{ - UNUSED(bus); - const bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data); - return ack; -} - -static bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - UNUSED(bus); - const bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); - return ack; } +#endif // MPU_INT_EXTI bool mpuAccRead(accDev_t *acc) { uint8_t data[6]; - const bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, data, 6); + const bool ack = busReadRegisterBuffer(&acc->bus, MPU_RA_ACCEL_XOUT_H, data, 6); if (!ack) { return false; } @@ -192,18 +167,11 @@ bool mpuAccRead(accDev_t *acc) return true; } -void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn) -{ - ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) { - gyro->updateFn = updateFn; - } -} - bool mpuGyroRead(gyroDev_t *gyro) { uint8_t data[6]; - const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_GYRO_XOUT_H, data, 6); + const bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_GYRO_XOUT_H, data, 6); if (!ack) { return false; } @@ -250,8 +218,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) sensor = mpu6000SpiDetect(&gyro->bus); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; - gyro->mpuConfiguration.writeFn = spiBusWriteRegister; return true; } #endif @@ -267,8 +233,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; - gyro->mpuConfiguration.writeFn = spiBusWriteRegister; return true; } #endif @@ -283,8 +247,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) sensor = mpu9250SpiDetect(&gyro->bus); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; - gyro->mpuConfiguration.writeFn = spiBusWriteRegister; gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro; return true; } @@ -300,8 +262,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) sensor = icm20649SpiDetect(&gyro->bus); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; - gyro->mpuConfiguration.writeFn = spiBusWriteRegister; return true; } #endif @@ -316,8 +276,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) sensor = icm20689SpiDetect(&gyro->bus); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; - gyro->mpuConfiguration.writeFn = spiBusWriteRegister; return true; } #endif @@ -332,8 +290,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) sensor = bmi160Detect(&gyro->bus); if (sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = sensor; - gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; - gyro->mpuConfiguration.writeFn = spiBusWriteRegister; return true; } #endif @@ -347,27 +303,27 @@ void mpuDetect(gyroDev_t *gyro) // MPU datasheet specifies 30ms. delay(35); - uint8_t sig = 0; #ifdef USE_I2C + gyro->bus.bustype = BUSTYPE_I2C; gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE; gyro->bus.busdev_u.i2c.address = MPU_ADDRESS; - bool ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1); + uint8_t sig = 0; + bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1); #else bool ack = false; #endif - if (ack) { - gyro->mpuConfiguration.readFn = mpuReadRegisterI2C; - gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C; - } else { + + if (!ack) { #ifdef USE_SPI detectSPISensorsAndUpdateDetectionResult(gyro); #endif return; } +#ifdef USE_I2C // If an MPU3050 is connected sig will contain 0. uint8_t inquiryResult; - ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1); + ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1); inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { gyro->mpuDetectionResult.sensor = MPU_3050; @@ -381,9 +337,14 @@ void mpuDetect(gyroDev_t *gyro) } else if (sig == MPU6500_WHO_AM_I_CONST) { gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; } +#endif } void mpuGyroInit(gyroDev_t *gyro) { +#ifdef MPU_INT_EXTI mpuIntExtiInit(gyro); +#else + UNUSED(gyro); +#endif } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index ed2f7ca977..46247082ca 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -140,15 +140,11 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t* data, uint8_t length); -typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data); typedef void (*mpuResetFnPtr)(void); extern mpuResetFnPtr mpuResetFn; typedef struct mpuConfiguration_s { - mpuReadRegisterFnPtr readFn; - mpuWriteRegisterFnPtr writeFn; mpuResetFnPtr resetFn; } mpuConfiguration_t; @@ -213,4 +209,3 @@ bool mpuAccRead(struct accDev_s *acc); bool mpuGyroRead(struct gyroDev_s *gyro); bool mpuGyroReadSPI(struct gyroDev_s *gyro); void mpuDetect(struct gyroDev_s *gyro); -void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn); diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c index 74a684c35a..3cdd97b3e5 100644 --- a/src/main/drivers/accgyro/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro/accgyro_mpu3050.c @@ -50,25 +50,24 @@ static void mpu3050Init(gyroDev_t *gyro) { - bool ack; - delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe - ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_SMPLRT_DIV, 0); - if (!ack) + const bool ack = busWriteRegister(&gyro->bus, MPU3050_SMPLRT_DIV, 0); + if (!ack) { failureMode(FAILURE_ACC_INIT); + } - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_INT_CFG, 0); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); + busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); + busWriteRegister(&gyro->bus, MPU3050_INT_CFG, 0); + busWriteRegister(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET); + busWriteRegister(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); } static bool mpu3050GyroRead(gyroDev_t *gyro) { uint8_t data[6]; - const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_GYRO_OUT, data, 6); + const bool ack = busReadRegisterBuffer(&gyro->bus, MPU3050_GYRO_OUT, data, 6); if (!ack) { return false; } @@ -83,7 +82,7 @@ static bool mpu3050GyroRead(gyroDev_t *gyro) static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) { uint8_t buf[2]; - if (!gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_TEMP_OUT, buf, 2)) { + if (!busReadRegisterBuffer(&gyro->bus, MPU3050_TEMP_OUT, buf, 2)) { return false; } diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c index a008d201fb..1bdeb73c44 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro/accgyro_mpu6050.c @@ -77,23 +77,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 + busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. // Accel scale 8g (4096 LSB/g) - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, + busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); #endif } diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c index abec649c88..f211c98b50 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_mpu6500.c @@ -54,34 +54,34 @@ void mpu6500GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); + busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07); + busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07); delay(100); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0); + busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0); delay(100); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); + busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); + busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration #ifdef USE_MPU9250_MAG - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN + busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN #else - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR + busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR #endif delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable + busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable #endif delay(15); } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c index e6210c4bbe..c5e9ac65bc 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20649.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -95,12 +95,12 @@ void icm20649AccInit(accDev_t *acc) spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); - acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 delay(15); const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G; - acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); + spiBusWriteRegister(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); delay(15); - acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 + spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 delay(15); } @@ -123,31 +123,31 @@ void icm20649GyroInit(gyroDev_t *gyro) spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); // ensure proper speed - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); delay(100); - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL); + spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 delay(15); const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS; uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips) raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3; - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); + spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration // back to bank 0 - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); + spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN + spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01); + spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01); #endif } @@ -187,7 +187,7 @@ bool icm20649AccRead(accDev_t *acc) { uint8_t data[6]; - const bool ack = acc->mpuConfiguration.readFn(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6); + const bool ack = spiBusReadRegisterBuffer(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6); if (!ack) { return false; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 7514b7ae9f..1fe8f2fd5a 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -119,32 +119,32 @@ void icm20689GyroInit(gyroDev_t *gyro) spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); delay(100); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03); + spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03); delay(100); -// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0); +// spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0); // delay(100); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); + spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); + spiBusWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration -// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN +// spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable + spiBusWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 39777822a0..c3ac21f924 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -171,6 +171,7 @@ uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg) void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) { + bus->bustype = BUSTYPE_SPI; bus->busdev_u.spi.instance = instance; } #endif diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 0eb5723730..3f0267b607 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -42,5 +42,4 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc); struct gyroDev_s; typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro); typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro); -typedef bool (*sensorGyroUpdateFuncPtr)(struct gyroDev_s *gyro); typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data); From fb7f0e1904eb7ac4a8203d8d1a28fe053ce8092b Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 14 Nov 2017 01:29:45 +1300 Subject: [PATCH 04/32] Move default position for OSD elements away from underneath artificial horizon / crosshairs position. --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0191c0203c..1568f53ade 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -843,7 +843,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) { /* Position elements near centre of screen and disabled by default */ for (int i = 0; i < OSD_ITEM_COUNT; i++) { - osdConfig->item_pos[i] = OSD_POS(10, 6); + osdConfig->item_pos[i] = OSD_POS(10, 7); } /* Always enable warnings elements by default */ From 877ac16b6051016ad132c24705440332ce596b0b Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 13 Nov 2017 13:55:43 +0100 Subject: [PATCH 05/32] Fixed broken dispatcher. --- src/main/fc/fc_dispatch.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/fc_dispatch.c b/src/main/fc/fc_dispatch.c index 61d19d2ade..227f99bdd1 100644 --- a/src/main/fc/fc_dispatch.c +++ b/src/main/fc/fc_dispatch.c @@ -55,6 +55,7 @@ void dispatchProcess(uint32_t currentTime) void dispatchAdd(dispatchEntry_t *entry, int delayUs) { uint32_t delayedUntil = micros() + delayUs; + entry->delayedUntil = delayedUntil; dispatchEntry_t **p = &head; while (*p && cmp32((*p)->delayedUntil, delayedUntil) < 0) p = &(*p)->next; From 214ee1293378971416c9fd17c78b005a50de61bf Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 14 Nov 2017 01:07:45 +0900 Subject: [PATCH 06/32] Add hmc5883, bmp280 and ms5611, remove bmp280 on spi --- src/main/target/BETAFLIGHTF4/target.h | 7 ++++--- src/main/target/BETAFLIGHTF4/target.mk | 2 ++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 62888ad128..faa93690fa 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -49,10 +49,11 @@ #define USE_MPU_DATA_READY_SIGNAL #define USE_BARO -#define USE_BARO_SPI_BMP280 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 -#define BMP280_SPI_INSTANCE SPI2 -#define BMP280_CS_PIN PB3 +#define USE_MAG +#define USE_MAG_HMC5883 #define USE_OSD #define USE_MAX7456 diff --git a/src/main/target/BETAFLIGHTF4/target.mk b/src/main/target/BETAFLIGHTF4/target.mk index 76e0e59eec..303e78739f 100755 --- a/src/main/target/BETAFLIGHTF4/target.mk +++ b/src/main/target/BETAFLIGHTF4/target.mk @@ -6,4 +6,6 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ drivers/max7456.c From eef170573d532830cea46053ae05a6f21560e0bf Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 15 Nov 2017 00:23:28 +1300 Subject: [PATCH 07/32] Optimised FrSky FPort telemetry. --- src/main/rx/fport.c | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 9bad0cc975..91a5ab24c3 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -106,8 +106,6 @@ typedef struct fportFrame_s { fportData_t data; } fportFrame_t; -static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = 0, .data = 0 }; - #define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t) #define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t)) @@ -260,7 +258,7 @@ static uint8_t fportFrameStatus(void) } else { result = sbusChannelsDecode(&frame->data.controlData.channels); - setRssiUnfiltered(scaleRange(frame->data.controlData.rssi, 0, 100, 0, 1024)); + setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024)); } break; @@ -319,11 +317,7 @@ static uint8_t fportFrameStatus(void) processSmartPortTelemetry(mspPayload, &clearToSend, NULL); } - if (clearToSend) { - smartPortWriteFrameFport(&emptySmartPortFrame); - clearToSend = false; - } - + clearToSend = false; } } #endif From 0e1f0e89e756809b0d3a633397395469a8927ce7 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 14 Nov 2017 15:57:33 +0100 Subject: [PATCH 08/32] Spektrum, CMS over Telemetry. Rebased and squashed. --- make/source.mk | 1 + src/main/cms/cms.c | 185 +++++++++++++++++++++++--------- src/main/cms/cms.h | 2 +- src/main/cms/cms_menu_builtin.c | 6 +- src/main/fc/fc_init.c | 6 ++ src/main/io/displayport_srxl.c | 135 +++++++++++++++++++++++ src/main/io/displayport_srxl.h | 20 ++++ src/main/rx/spektrum.c | 3 +- src/main/target/common_fc_pre.h | 1 + src/main/telemetry/srxl.c | 167 +++++++++++++++++++++++----- src/main/telemetry/srxl.h | 7 ++ src/test/unit/osd_unittest.cc | 2 +- 12 files changed, 457 insertions(+), 78 deletions(-) create mode 100644 src/main/io/displayport_srxl.c create mode 100644 src/main/io/displayport_srxl.h diff --git a/make/source.mk b/make/source.mk index f2b5396fb1..8b46bf6d41 100644 --- a/make/source.mk +++ b/make/source.mk @@ -149,6 +149,7 @@ FC_SRC = \ io/displayport_msp.c \ io/displayport_oled.c \ io/displayport_rcdevice.c \ + io/displayport_srxl.c \ io/rcdevice_cam.c \ io/rcdevice.c \ io/rcdevice_osd.c \ diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 3a5687e013..a409e07cdc 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -126,10 +126,18 @@ static displayPort_t *cmsDisplayPortSelectNext(void) // HoTT Telemetry Screen // 21 cols x 8 rows // +// Spektrum SRXL Telemtry Textgenerator +// 13 cols x 9 rows, top row printed as a Bold Heading +// Needs the "smallScreen" adaptions -#define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN(p) ((p)->cols - 8) -#define MAX_MENU_ITEMS(p) ((p)->rows - 2) + + +#define NORMAL_SCREEN_MIN_COLS 18 // Less is a small screen +static bool smallScreen; +static uint8_t leftMenuColumn; +static uint8_t rightMenuColumn; +static uint8_t maxMenuItems; +static uint8_t linesPerMenuItem; bool cmsInMenu = false; @@ -181,14 +189,15 @@ static CMS_Menu menuErr = { static void cmsUpdateMaxRow(displayPort_t *instance) { + UNUSED(instance); pageMaxRow = 0; for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) { pageMaxRow++; } - if (pageMaxRow > MAX_MENU_ITEMS(instance)) { - pageMaxRow = MAX_MENU_ITEMS(instance); + if (pageMaxRow > maxMenuItems) { + pageMaxRow = maxMenuItems; } pageMaxRow--; @@ -196,13 +205,14 @@ static void cmsUpdateMaxRow(displayPort_t *instance) static uint8_t cmsCursorAbsolute(displayPort_t *instance) { - return currentCtx.cursorRow + currentCtx.page * MAX_MENU_ITEMS(instance); + UNUSED(instance); + return currentCtx.cursorRow + currentCtx.page * maxMenuItems; } static void cmsPageSelect(displayPort_t *instance, int8_t newpage) { currentCtx.page = (newpage + pageCount) % pageCount; - pageTop = ¤tCtx.menu->entries[currentCtx.page * MAX_MENU_ITEMS(instance)]; + pageTop = ¤tCtx.menu->entries[currentCtx.page * maxMenuItems]; cmsUpdateMaxRow(instance); displayClearScreen(instance); } @@ -244,7 +254,13 @@ static void cmsFormatFloat(int32_t value, char *floatString) floatString[0] = ' '; } -static void cmsPadToSize(char *buf, int size) +// CMS on OSD legacy was to use LEFT aligned values, not the RIGHT way ;-) +#define CMS_OSD_RIGHT_ALIGNED_VALUES + +#ifndef CMS_OSD_RIGHT_ALIGNED_VALUES + +// Pad buffer to the left, i.e. align left +static void cmsPadRightToSize(char *buf, int size) { int i; @@ -259,17 +275,69 @@ static void cmsPadToSize(char *buf, int size) buf[size] = 0; } +#endif + +// Pad buffer to the left, i.e. align right +static void cmsPadLeftToSize(char *buf, int size) +{ + int i,j; + int len = strlen(buf); + + for (i = size - 1, j = size - len ; i - j >= 0 ; i--) { + buf[i] = buf[i - j]; + } + + for ( ; i >= 0 ; i--) { + buf[i] = ' '; + } + + buf[size] = 0; +} + +static void cmsPadToSize(char *buf, int size) +{ + // Make absolutely sure the string terminated. + buf[size] = 0x00, + +#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES + cmsPadLeftToSize(buf, size); +#else + smallScreen ? cmsPadLeftToSize(buf, size) : cmsPadRightToSize(buf, size); +#endif +} + +static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row, uint8_t maxSize) +{ + int colpos; + int cnt; + + cmsPadToSize(buff, maxSize); +#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES + colpos = rightMenuColumn - maxSize; +#else + colpos = smallScreen ? rightMenuColumn - maxSize : rightMenuColumn; +#endif + cnt = displayWrite(pDisplay, colpos, row, buff); + return cnt; +} static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) { - #define CMS_DRAW_BUFFER_LEN 10u - char buff[CMS_DRAW_BUFFER_LEN]; + #define CMS_DRAW_BUFFER_LEN 12 + #define CMS_NUM_FIELD_LEN 5 + + char buff[CMS_DRAW_BUFFER_LEN +1]; // Make room for null terminator. int cnt = 0; + if (smallScreen) { + row++; + } + switch (p->type) { case OME_String: if (IS_PRINTVALUE(p) && p->data) { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data); + strncpy(buff, p->data, CMS_DRAW_BUFFER_LEN); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN); CLR_PRINTVALUE(p); } break; @@ -278,19 +346,19 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) case OME_Funcall: if (IS_PRINTVALUE(p)) { - int colPos = RIGHT_MENU_COLUMN(pDisplay); + buff[0]= 0x0; if ((p->type == OME_Submenu) && p->func && (p->flags & OPTSTRING)) { // Special case of sub menu entry with optional value display. char *str = ((CMSMenuOptFuncPtr)p->func)(); - cnt = displayWrite(pDisplay, colPos, row, str); - colPos += strlen(str); + strncpy( buff, str, CMS_DRAW_BUFFER_LEN); } + strncat(buff, ">", CMS_DRAW_BUFFER_LEN); - cnt += displayWrite(pDisplay, colPos, row, ">"); - + row = smallScreen ? row - 1 : row; + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, strlen(buff)); CLR_PRINTVALUE(p); } break; @@ -298,10 +366,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) case OME_Bool: if (IS_PRINTVALUE(p) && p->data) { if (*((uint8_t *)(p->data))) { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + strcpy(buff, "YES"); } else { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + strcpy(buff, "NO "); } + + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3); CLR_PRINTVALUE(p); } break; @@ -310,9 +380,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) if (IS_PRINTVALUE(p)) { OSD_TAB_t *ptr = p->data; char * str = (char *)ptr->names[*ptr->val]; - memcpy(buff, str, MAX(CMS_DRAW_BUFFER_LEN, strlen(str))); - cmsPadToSize(buff, CMS_DRAW_BUFFER_LEN); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + strncpy(buff, str, CMS_DRAW_BUFFER_LEN); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN); CLR_PRINTVALUE(p); } break; @@ -323,10 +392,11 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) uint16_t *val = (uint16_t *)p->data; if (VISIBLE(*val)) { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + strcpy(buff, "YES"); } else { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + strcpy(buff, "NO "); } + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3); CLR_PRINTVALUE(p); } break; @@ -336,8 +406,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) if (IS_PRINTVALUE(p) && p->data) { OSD_UINT8_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p); } break; @@ -346,8 +415,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) if (IS_PRINTVALUE(p) && p->data) { OSD_INT8_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p); } break; @@ -356,8 +424,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) if (IS_PRINTVALUE(p) && p->data) { OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p); } break; @@ -366,8 +433,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) if (IS_PRINTVALUE(p) && p->data) { OSD_UINT16_t *ptr = p->data; itoa(*ptr->val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p); } break; @@ -376,8 +442,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) if (IS_PRINTVALUE(p) && p->data) { OSD_FLOAT_t *ptr = p->data; cmsFormatFloat(*ptr->val * ptr->multipler, buff); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ??? + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p); } break; @@ -385,7 +450,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) case OME_Label: if (IS_PRINTVALUE(p) && p->data) { // A label with optional string, immediately following text - cnt = displayWrite(pDisplay, LEFT_MENU_COLUMN + 2 + strlen(p->text), row, p->data); + cnt = displayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, p->data); CLR_PRINTVALUE(p); } break; @@ -399,8 +464,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) // Fall through default: #ifdef CMS_MENU_DEBUG - // Shouldn't happen. Notify creator of this menu content. - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT"); + // Shouldn't happen. Notify creator of this menu content +#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES + cnt = displayWrite(pDisplay, rightMenuColumn - 6, row, "BADENT"); +#else. + cnt = displayWrite(pDisplay, rightMenuColumn, row, "BADENT"); +#endif #endif break; } @@ -415,7 +484,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) uint8_t i; OSD_Entry *p; - uint8_t top = (pDisplay->rows - pageMaxRow) / 2 - 1; + uint8_t top = smallScreen ? 1 : (pDisplay->rows - pageMaxRow)/2; // Polled (dynamic) value display denominator. @@ -450,14 +519,14 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) cmsPageDebug(); if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) { - room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, " "); + room -= displayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, " "); } if (room < 30) return; if (pDisplay->cursorRow != currentCtx.cursorRow) { - room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCtx.cursorRow + top, " >"); + room -= displayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, ">"); pDisplay->cursorRow = currentCtx.cursorRow; } @@ -465,25 +534,23 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) return; // Print text labels - for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + for (i = 0, p = pageTop; i < maxMenuItems && p->type != OME_END; i++, p++) { if (IS_PRINTLABEL(p)) { - uint8_t coloff = LEFT_MENU_COLUMN; - coloff += (p->type == OME_Label) ? 1 : 2; - room -= displayWrite(pDisplay, coloff, i + top, p->text); + uint8_t coloff = leftMenuColumn; + coloff += (p->type == OME_Label) ? 0 : 1; + room -= displayWrite(pDisplay, coloff, top + i * linesPerMenuItem, p->text); CLR_PRINTLABEL(p); if (room < 30) return; } - } // Print values // XXX Polled values at latter positions in the list may not be // XXX printed if not enough room in the middle of the list. - for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { if (IS_PRINTVALUE(p)) { - room -= cmsDrawMenuEntry(pDisplay, p, top + i); + room -= cmsDrawMenuEntry(pDisplay, p, top + i * linesPerMenuItem); if (room < 30) return; } @@ -492,9 +559,10 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) static void cmsMenuCountPage(displayPort_t *pDisplay) { + UNUSED(pDisplay); const OSD_Entry *p; for (p = currentCtx.menu->entries; p->type != OME_END; p++); - pageCount = (p - currentCtx.menu->entries - 1) / MAX_MENU_ITEMS(pDisplay) + 1; + pageCount = (p - currentCtx.menu->entries - 1) / maxMenuItems + 1; } STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay); // Forward; will be resolved after merging @@ -538,9 +606,9 @@ long cmsMenuChange(displayPort_t *pDisplay, const void *ptr) // currentCtx.cursorRow has been saved as absolute; convert it back to page + relative int8_t cursorAbs = currentCtx.cursorRow; - currentCtx.cursorRow = cursorAbs % MAX_MENU_ITEMS(pDisplay); + currentCtx.cursorRow = cursorAbs % maxMenuItems; cmsMenuCountPage(pDisplay); - cmsPageSelect(pDisplay, cursorAbs / MAX_MENU_ITEMS(pDisplay)); + cmsPageSelect(pDisplay, cursorAbs / maxMenuItems); } cmsPageDebug(); @@ -594,6 +662,25 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void) } } displayGrab(pCurrentDisplay); // grab the display for use by the CMS + + if ( pCurrentDisplay->cols < NORMAL_SCREEN_MIN_COLS) { + smallScreen = true; + linesPerMenuItem = 2; + leftMenuColumn = 0; + rightMenuColumn = pCurrentDisplay->cols; + maxMenuItems = (pCurrentDisplay->rows) / linesPerMenuItem; + } else { + smallScreen = false; + linesPerMenuItem = 1; + leftMenuColumn = 2; +#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES + rightMenuColumn = pCurrentDisplay->cols - 2; +#else + rightMenuColumn = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN; +#endif + maxMenuItems = pCurrentDisplay->rows - 2; + } + cmsMenuChange(pCurrentDisplay, currentCtx.menu); } diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index 80f37b68ed..bf952a4cb3 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -17,7 +17,7 @@ long cmsMenuChange(displayPort_t *pPort, const void *ptr); long cmsMenuExit(displayPort_t *pPort, const void *ptr); void cmsUpdate(uint32_t currentTimeUs); -#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" +#define CMS_STARTUP_HELP_TEXT1 "MENU:THR MID" #define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" #define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index f1634807c4..40984cb551 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -53,20 +53,22 @@ // Info -static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; +static char infoGitRev[GIT_SHORT_REVISION_LENGTH + 1]; static char infoTargetName[] = __TARGET__; #include "interface/msp_protocol.h" // XXX for FC identification... not available elsewhere static long cmsx_InfoInit(void) { - for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { + int i; + for ( i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; else infoGitRev[i] = shortGitRevision[i]; } + infoGitRev[i] = 0x0; // Terminate string return 0; } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index eaad12a8d1..3521a5ea84 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -107,6 +107,8 @@ #include "io/vtx_smartaudio.h" #include "io/vtx_tramp.h" +#include "io/displayport_srxl.h" + #include "scheduler/scheduler.h" #include "sensors/acceleration.h" @@ -597,6 +599,10 @@ void init(void) } #endif +#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) + // Register the srxl Textgen telemetry sensor as a displayport device + cmsDisplayPortRegister(displayPortSrxlInit()); +#endif #ifdef USE_GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/io/displayport_srxl.c b/src/main/io/displayport_srxl.c new file mode 100644 index 0000000000..684e80df8c --- /dev/null +++ b/src/main/io/displayport_srxl.c @@ -0,0 +1,135 @@ +/* + * 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 + +#include "platform.h" +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) + +#include "common/utils.h" + +#include "drivers/display.h" +#include "cms/cms.h" + +#include "telemetry/srxl.h" + +static displayPort_t srxlDisplayPort; + +static int srxlDrawScreen(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int srxlScreenSize(const displayPort_t *displayPort) +{ + return displayPort->rows * displayPort->cols; +} + +static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t c) +{ + return (spektrumTmTextGenPutChar(col, row, c)); + UNUSED(displayPort); +} + + +static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s) +{ + while (*s) { + srxlWriteChar(displayPort, col++, row, *(s++)); + } + return 0; +} + +static int srxlClearScreen(displayPort_t *displayPort) +{ + for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) { + for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) { + srxlWriteChar(displayPort, col, row, ' '); + } + } + srxlWriteString(displayPort, 1, 0, "BETAFLIGHT"); + + if ( displayPort->grabCount == 0 ) { + srxlWriteString(displayPort, 0, 3, CMS_STARTUP_HELP_TEXT1); + srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT2); + srxlWriteString(displayPort, 2, 5, CMS_STARTUP_HELP_TEXT3); + } + return 0; +} + +static bool srxlIsTransferInProgress(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return false; +} + +static int srxlHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static void srxlResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t srxlTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static int srxlGrab(displayPort_t *displayPort) +{ + return displayPort->grabCount = 1; +} + +static int srxlRelease(displayPort_t *displayPort) +{ + int cnt = displayPort->grabCount = 0; + srxlClearScreen(displayPort); + return cnt; +} + +static const displayPortVTable_t srxlVTable = { + .grab = srxlGrab, + .release = srxlRelease, + .clearScreen = srxlClearScreen, + .drawScreen = srxlDrawScreen, + .screenSize = srxlScreenSize, + .writeString = srxlWriteString, + .writeChar = srxlWriteChar, + .isTransferInProgress = srxlIsTransferInProgress, + .heartbeat = srxlHeartbeat, + .resync = srxlResync, + .txBytesFree = srxlTxBytesFree +}; + +displayPort_t *displayPortSrxlInit() +{ + srxlDisplayPort.device = NULL; + displayInit(&srxlDisplayPort, &srxlVTable); + srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; + srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; + return &srxlDisplayPort; +} + +#endif diff --git a/src/main/io/displayport_srxl.h b/src/main/io/displayport_srxl.h new file mode 100644 index 0000000000..72019e0f13 --- /dev/null +++ b/src/main/io/displayport_srxl.h @@ -0,0 +1,20 @@ +/* + * 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 + +displayPort_t *displayPortSrxlInit(); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 94d1f980ad..6ca6fe53d9 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -56,6 +56,7 @@ #define SPEKTRUM_1024_CHANNEL_COUNT 7 #define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000 +#define SPEKTRUM_TELEMETRY_FRAME_DELAY 1000 // Gap between received Rc frame and transmited TM frame, uS #define SPEKTRUM_BAUDRATE 115200 @@ -473,7 +474,7 @@ static uint8_t spektrumFrameStatus(void) /* only process if srxl enabled, some data in buffer AND servos in phase 0 */ if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80)) { - dispatchAdd(&srxlTelemetryDispatch, 100); + dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY); } return RX_FRAME_COMPLETE; } diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 013c842105..fea43db40c 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -138,6 +138,7 @@ #define USE_SPEKTRUM_FAKE_RSSI #define USE_SPEKTRUM_RSSI_PERCENT_CONVERSION #define USE_SPEKTRUM_VTX_CONTROL +#define USE_SPEKTRUM_CMS_TELEMETRY #endif #endif diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index 44597c5f04..052b682ca0 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -50,8 +50,7 @@ #include "telemetry/telemetry.h" #include "telemetry/srxl.h" - -#define SRXL_CYCLETIME_US 100000 // 100ms, 10 Hz +#define SRXL_CYCLETIME_US 33000 // 33ms, 30 Hz #define SRXL_ADDRESS_FIRST 0xA5 #define SRXL_ADDRESS_SECOND 0x80 @@ -104,8 +103,10 @@ typedef struct UINT16 rxVoltage; // Volts, 0.01V increments } STRU_TELE_QOS; */ -void srxlFrameQos(sbuf_t *dst) +bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU16BigEndian(dst, 0xFFFF); // A @@ -115,6 +116,7 @@ void srxlFrameQos(sbuf_t *dst) sbufWriteU16BigEndian(dst, 0xFFFF); // F sbufWriteU16BigEndian(dst, 0xFFFF); // H sbufWriteU16BigEndian(dst, 0xFFFF); // rxVoltage + return true; } /* @@ -130,8 +132,10 @@ typedef struct // If only 1 antenna, set B = A } STRU_TELE_RPM; */ -void srxlFrameRpm(sbuf_t *dst) +bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU16BigEndian(dst, 0xFFFF); // pulse leading edges @@ -144,6 +148,7 @@ void srxlFrameRpm(sbuf_t *dst) sbufWriteU16BigEndian(dst, 0xFFFF); sbufWriteU16BigEndian(dst, 0xFFFF); sbufWriteU16BigEndian(dst, 0xFFFF); + return true; } /* @@ -161,42 +166,156 @@ typedef struct } STRU_TELE_FP_MAH; */ -void srxlFrameFlightPackCurrent(sbuf_t *dst) +#define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s + +bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs) { - sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH); - sbufWriteU8(dst, SRXL_FRAMETYPE_SID); - sbufWriteU16(dst, getAmperage() / 10); - sbufWriteU16(dst, getMAhDrawn()); - sbufWriteU16(dst, 0x7fff); // temp A - sbufWriteU16(dst, 0xffff); - sbufWriteU16(dst, 0xffff); - sbufWriteU16(dst, 0x7fff); // temp B - sbufWriteU16(dst, 0xffff); + uint16_t amps = getAmperage() / 10; + uint16_t mah = getMAhDrawn(); + static uint16_t sentAmps; + static uint16_t sentMah; + static timeUs_t lastTimeSentFPmAh = 0; + + timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh; + + if ( (amps != sentAmps) || (mah != sentMah) || + keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) { + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU16(dst, amps); + sbufWriteU16(dst, mah); + sbufWriteU16(dst, 0x7fff); // temp A + sbufWriteU16(dst, 0xffff); + sbufWriteU16(dst, 0xffff); + sbufWriteU16(dst, 0x7fff); // temp B + sbufWriteU16(dst, 0xffff); + + sentAmps = amps; + sentMah = mah; + lastTimeSentFPmAh = currentTimeUs; + return true; + } + return false; } -// schedule array to decide how often each type of frame is sent -#define SRXL_SCHEDULE_COUNT_MAX 3 +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) -typedef void (*srxlScheduleFnPtr)(sbuf_t *dst); -const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = { +// Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display. + +#define SPEKTRUM_SRXL_DEVICE_TEXTGEN (0x0C) // Text Generator +#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS (9) // Text Generator ROWS +#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS (13) // Text Generator COLS + +/* +typedef struct +{ + UINT8 identifier; + UINT8 sID; // Secondary ID + UINT8 lineNumber; // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen) + char text[13]; // 0-terminated text when < 13 chars +} STRU_SPEKTRUM_SRXL_TEXTGEN; +*/ + +#if ( SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS > SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS ) +static char srxlTextBuff[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS][SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS]; +static bool lineSent[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS]; +#else +static char srxlTextBuff[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS][SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS]; +static bool lineSent[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS]; +#endif + +//************************************************************************** +// API Running in external client task context. E.g. in the CMS task +int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c) +{ + if (row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS && col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS) { + srxlTextBuff[row][col] = c; + lineSent[row] = false; + } + return 0; +} +//************************************************************************** + +bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + static uint8_t lineNo = 0; + int lineCount = 0; + + // Skip already sent lines... + while (lineSent[lineNo] && + lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) { + lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS; + lineCount++; + } + + sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU8(dst, lineNo); + sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS); + + lineSent[lineNo] = true; + lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS; + + // Always send something, Always one user frame after the two mandatory frames + // I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true + // too keep the "Waltz" sequence intact. + return true; +} + +#endif + +// Schedule array to decide how often each type of frame is sent +// The frames are scheduled in sets of 3 frames, 2 mandatory and 1 user frame. +// The user frame type is cycled for each set. +// Example. QOS, RPM,.CURRENT, QOS, RPM, TEXT. QOS, RPM, CURRENT, etc etc + +#define SRXL_SCHEDULE_MANDATORY_COUNT 2 // Mandatory QOS and RPM sensors + +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) +#define SRXL_SCHEDULE_CMS_COUNT 1 +#else +#define SRXL_SCHEDULE_CMS_COUNT 0 +#endif + +#define SRXL_SCHEDULE_USER_COUNT (1 + SRXL_SCHEDULE_CMS_COUNT) + +#define SRXL_SCHEDULE_COUNT_MAX (SRXL_SCHEDULE_MANDATORY_COUNT + 1) +#define SRXL_TOTAL_COUNT (SRXL_SCHEDULE_MANDATORY_COUNT + SRXL_SCHEDULE_USER_COUNT) + +typedef bool (*srxlScheduleFnPtr)(sbuf_t *dst, timeUs_t currentTimeUs); + +const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_TOTAL_COUNT] = { /* must send srxlFrameQos, Rpm and then alternating items of our own */ srxlFrameQos, srxlFrameRpm, - srxlFrameFlightPackCurrent + srxlFrameFlightPackCurrent, +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) + srxlFrameText, +#endif }; -static void processSrxl(void) +static void processSrxl(timeUs_t currentTimeUs) { static uint8_t srxlScheduleIndex = 0; + static uint8_t srxlScheduleUserIndex = 0; sbuf_t srxlPayloadBuf; sbuf_t *dst = &srxlPayloadBuf; + srxlScheduleFnPtr srxlFnPtr; + + if (srxlScheduleIndex < SRXL_SCHEDULE_MANDATORY_COUNT) { + srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex]; + } else { + srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex + srxlScheduleUserIndex]; + srxlScheduleUserIndex = (srxlScheduleUserIndex + 1) % SRXL_SCHEDULE_USER_COUNT; + } - srxlScheduleFnPtr srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex]; if (srxlFnPtr) { srxlInitializeFrame(dst); - srxlFnPtr(dst); - srxlFinalize(dst); + if (srxlFnPtr(dst, currentTimeUs)) { + srxlFinalize(dst); + } } srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX; } @@ -227,7 +346,7 @@ void handleSrxlTelemetry(timeUs_t currentTimeUs) // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz if (currentTimeUs >= srxlLastCycleTime + SRXL_CYCLETIME_US) { srxlLastCycleTime = currentTimeUs; - processSrxl(); + processSrxl(currentTimeUs); } } #endif diff --git a/src/main/telemetry/srxl.h b/src/main/telemetry/srxl.h index 33fc329e69..957e50704a 100644 --- a/src/main/telemetry/srxl.h +++ b/src/main/telemetry/srxl.h @@ -22,3 +22,10 @@ void initSrxlTelemetry(void); bool checkSrxlTelemetryState(void); void handleSrxlTelemetry(timeUs_t currentTimeUs); + +#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS 9 +#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 12 // Airware 1.20 +//#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 13 // Airware 1.21 +#define SPEKTRUM_SRXL_TEXTGEN_CLEAR_SCREEN 255 + +int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c); diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index ec19ad19d8..7c828ff07c 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -188,7 +188,7 @@ TEST(OsdTest, TestInit) // then // display buffer should contain splash screen - displayPortTestBufferSubstring(7, 8, "MENU: THR MID"); + displayPortTestBufferSubstring(7, 8, "MENU:THR MID"); displayPortTestBufferSubstring(11, 9, "+ YAW LEFT"); displayPortTestBufferSubstring(11, 10, "+ PITCH UP"); From d32b1c341d193c60528cf68b1dd8163bb8b6fdf3 Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 17 Nov 2017 17:05:47 +1300 Subject: [PATCH 09/32] Fixed FrSky SPI binding in CLI. --- src/main/interface/cli.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index b70867a8d0..e56448eac5 100755 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -3642,9 +3642,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("flash_read", NULL, "
", cliFlashRead), CLI_COMMAND_DEF("flash_write", NULL, "
", cliFlashWrite), #endif -#ifdef USE_RX_FRSKY_D - CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind), #endif +#ifdef USE_RX_FRSKY_D + CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky RX", NULL, cliFrSkyBind), #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), #ifdef USE_GPS From 0331c1f2f675d7e08a611dcd93b9cb7e7ab216e9 Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 17 Nov 2017 17:32:19 +1300 Subject: [PATCH 10/32] Fixed intentation in 'fc_core.c'. --- src/main/fc/fc_core.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index abaa7d6d63..70d6332208 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -407,7 +407,9 @@ void processRx(timeUs_t currentTimeUs) const throttleStatus_e throttleStatus = calculateThrottleStatus(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { - if (rcData[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset + if (rcData[THROTTLE] >= rxConfig()->airModeActivateThreshold) { + airmodeIsActivated = true; // Prevent Iterm from being reset + } } else { airmodeIsActivated = false; } From 9c9f02f550515c0cc9f32c7a27ad5a07a7736598 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 17 Nov 2017 06:11:53 +0000 Subject: [PATCH 11/32] Use constant dT for ITerm calcualtion to avoid windup caused by jitter --- src/main/flight/pid.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3a5beae970..1aeae117da 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -497,7 +497,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate I component const float ITerm = axisPID_I[axis]; - const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * deltaT * dynKi * itermAccelerator, -itermLimit, itermLimit); + // use dT (not deltaT) for ITerm calculation to avoid wind-up caused by jitter + const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator, -itermLimit, itermLimit); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { // Only increase ITerm if output is not saturated From 440ed4f1c2630ea76b017240e31dfe440de028a4 Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 17 Nov 2017 13:34:48 +1300 Subject: [PATCH 12/32] Reduced features on SPRACINGF3EVO and variants to make it fit flash. --- src/main/target/SPRACINGF3EVO/target.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index ddba90c893..75586105dd 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -25,10 +25,8 @@ #define TARGET_BOARD_IDENTIFIER "SPEV" #endif +#if !defined(AIORACERF3) #define USE_TARGET_CONFIG - -#ifdef AIORACERF3 -#undef USE_TARGET_CONFIG // no space left #endif #ifdef SPRACINGF3MQ @@ -38,12 +36,14 @@ #define SPRACINGF3MQ_REV 2 #endif -#undef USE_UNCOMMON_MIXERS // no space left -#endif -#undef USE_TELEMETRY_JETIEXBUS // no space left -#undef USE_SERIALRX_JETIEXBUS // no space left -#undef USE_DASHBOARD // no space left -#undef USE_RTC_TIME // no space left +#endif // SPRACINGF3MQ + +// Space reduction measures to make the firmware fit into flash: +#undef USE_TELEMETRY_JETIEXBUS +#undef USE_SERIALRX_JETIEXBUS +#undef USE_TELEMETRY_MAVLINK +#undef USE_DASHBOARD + #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT From 36957d60753deb1c434b2d6c7d0471e938ff8949 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 15 Nov 2017 09:03:39 +0000 Subject: [PATCH 13/32] When in ACRO mode use trim sticks to adjust RATE profile --- src/main/fc/rc_controls.c | 68 +++++++++++++-------- src/test/unit/arming_prevention_unittest.cc | 1 + src/test/unit/rc_controls_unittest.cc | 1 + 3 files changed, 45 insertions(+), 25 deletions(-) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 142b1ddc25..e5054dfa72 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -243,13 +243,13 @@ void processRcStickPositions(throttleStatus_e throttleStatus) return; } - // Multiple configuration profiles + // Change PID profile int newPidProfile = 0; - if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) { // ROLL left -> Profile 1 + if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) { // ROLL left -> PID profile 1 newPidProfile = 1; - } else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) { // PITCH up -> Profile 2 + } else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) { // PITCH up -> PID profile 2 newPidProfile = 2; - } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) { // ROLL right -> Profile 3 + } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) { // ROLL right -> PID profile 3 newPidProfile = 3; } if (newPidProfile) { @@ -275,28 +275,46 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } - // Accelerometer Trim - rollAndPitchTrims_t accelerometerTrimsDelta; - memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta)); + if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) { + // in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims + rollAndPitchTrims_t accelerometerTrimsDelta; + memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta)); - bool shouldApplyRollAndPitchTrimDelta = false; - if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { - accelerometerTrimsDelta.values.pitch = 2; - shouldApplyRollAndPitchTrimDelta = true; - } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { - accelerometerTrimsDelta.values.pitch = -2; - shouldApplyRollAndPitchTrimDelta = true; - } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { - accelerometerTrimsDelta.values.roll = 2; - shouldApplyRollAndPitchTrimDelta = true; - } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { - accelerometerTrimsDelta.values.roll = -2; - shouldApplyRollAndPitchTrimDelta = true; - } - if (shouldApplyRollAndPitchTrimDelta) { - applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta); - repeatAfter(STICK_AUTOREPEAT_MS); - return; + bool shouldApplyRollAndPitchTrimDelta = false; + if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { + accelerometerTrimsDelta.values.pitch = 2; + shouldApplyRollAndPitchTrimDelta = true; + } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { + accelerometerTrimsDelta.values.pitch = -2; + shouldApplyRollAndPitchTrimDelta = true; + } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { + accelerometerTrimsDelta.values.roll = 2; + shouldApplyRollAndPitchTrimDelta = true; + } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { + accelerometerTrimsDelta.values.roll = -2; + shouldApplyRollAndPitchTrimDelta = true; + } + if (shouldApplyRollAndPitchTrimDelta) { + applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta); + repeatAfter(STICK_AUTOREPEAT_MS); + return; + } + } else { + // in ACRO mode, so use sticks to change RATE profile + switch (rcSticks) { + case THR_HI + YAW_CE + PIT_HI + ROL_CE: + changeControlRateProfile(0); + return; + case THR_HI + YAW_CE + PIT_LO + ROL_CE: + changeControlRateProfile(1); + return; + case THR_HI + YAW_CE + PIT_CE + ROL_HI: + changeControlRateProfile(2); + return; + case THR_HI + YAW_CE + PIT_CE + ROL_LO: + changeControlRateProfile(3); + return; + } } #ifdef USE_DASHBOARD diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 0814908f97..26f8f8a113 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -662,6 +662,7 @@ extern "C" { void accSetCalibrationCycles(uint16_t) {} void baroSetCalibrationCycles(uint16_t) {} void changePidProfile(uint8_t) {} + void changeControlRateProfile(uint8_t) {} void dashboardEnablePageCycling(void) {} void dashboardDisablePageCycling(void) {} bool imuQuaternionHeadfreeOffsetSet(void) { return true; } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 47481adffe..98f6918cb4 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -699,6 +699,7 @@ void blackboxLogEvent(FlightLogEvent, flightLogEventData_t *) {} bool cmsInMenu = false; uint8_t armingFlags = 0; +uint16_t flightModeFlags = 0; int16_t heading; uint8_t stateFlags = 0; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; From 26b873dad90e44c9523696d30a8f23e13875ef3c Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 17 Nov 2017 13:03:35 +1300 Subject: [PATCH 14/32] Added optimisation settings for optional sources. --- make/source.mk | 41 ++++++++++++++++++++++++++++---- src/main/target/OMNIBUS/target.h | 4 +++- 2 files changed, 39 insertions(+), 6 deletions(-) diff --git a/make/source.mk b/make/source.mk index 8b46bf6d41..36f16c3b51 100644 --- a/make/source.mk +++ b/make/source.mk @@ -199,6 +199,22 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ common/filter.c \ common/maths.c \ common/typeconversion.c \ + drivers/accgyro/accgyro_adxl345.c \ + drivers/accgyro/accgyro_bma280.c \ + drivers/accgyro/accgyro_fake.c \ + drivers/accgyro/accgyro_l3g4200d.c \ + drivers/accgyro/accgyro_l3gd20.c \ + drivers/accgyro/accgyro_lsm303dlhc.c \ + drivers/accgyro/accgyro_mma845x.c \ + drivers/accgyro/accgyro_mpu3050.c \ + drivers/accgyro/accgyro_mpu6050.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_spi_bmi160.c \ + drivers/accgyro/accgyro_spi_icm20689.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu9250.c \ drivers/adc.c \ drivers/buf_writer.c \ drivers/bus.c \ @@ -237,19 +253,31 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ sensors/gyroanalyse.c \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) \ - drivers/light_ws2811strip.c \ - io/displayport_max7456.c \ - io/osd.c \ - io/osd_slave.c SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ + bus_bst_stm32f30x.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_fake.c \ + drivers/barometer/barometer_ms5611.c \ drivers/bus_i2c_config.c \ drivers/bus_spi_config.c \ drivers/bus_spi_pinconfig.c \ + drivers/compass/compass_ak8963.c \ + drivers/compass/compass_ak8975.c \ + drivers/compass/compass_fake.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.c \ + drivers/inverter.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_hal.c \ drivers/serial_escserial.c \ drivers/serial_pinconfig.c \ + drivers/serial_tcp.c \ drivers/serial_uart_init.c \ drivers/serial_uart_pinconfig.c \ + drivers/serial_usb_vcp.c \ + drivers/transponder_ir.c \ drivers/vtx_rtc6705_soft_spi.c \ drivers/vtx_rtc6705.c \ drivers/vtx_common.c \ @@ -258,12 +286,15 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ config/feature.c \ config/parameter_group.c \ config/config_streamer.c \ + i2c_bst.c \ interface/cli.c \ interface/settings.c \ + io/dashboard.c \ + io/osd.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ - io/dashboard.c \ + io/transponder_ir.c \ msp/msp_serial.c \ cms/cms.c \ cms/cms_menu_blackbox.c \ diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 886a40ed0b..c6c2b82c89 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -21,10 +21,12 @@ #undef USE_TELEMETRY_IBUS #undef USE_TELEMETRY_HOTT #undef USE_TELEMETRY_JETIEXBUS +#undef USE_SERIALRX_JETIEXBUS #undef USE_TELEMETRY_MAVLINK #undef USE_TELEMETRY_LTM #undef USE_RCDEVICE -#undef USE_RTC_TIME +#undef USE_DASHBOARD + #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus From f9de9ae1097cb456f51d9f7d142c9519207a14de Mon Sep 17 00:00:00 2001 From: Milan Krstic Date: Sat, 18 Nov 2017 01:13:48 +0100 Subject: [PATCH 15/32] add support for Winbond W25Q16 --- src/main/drivers/flash_m25p16.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 5a1b3578a0..e75b97b93f 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -48,6 +48,7 @@ #define JEDEC_ID_MICRON_M25P16 0x202015 #define JEDEC_ID_MICRON_N25Q064 0x20BA17 #define JEDEC_ID_MICRON_N25Q128 0x20ba18 +#define JEDEC_ID_WINBOND_W25Q16 0xEF4015 #define JEDEC_ID_WINBOND_W25Q64 0xEF4017 #define JEDEC_ID_WINBOND_W25Q128 0xEF4018 #define JEDEC_ID_WINBOND_W25Q256 0xEF4019 @@ -162,6 +163,7 @@ static bool m25p16_readIdentification(void) // All supported chips use the same pagesize of 256 bytes switch (chipID) { + case JEDEC_ID_WINBOND_W25Q16: case JEDEC_ID_MICRON_M25P16: geometry.sectors = 32; geometry.pagesPerSector = 256; From 60a8f3a5b9644c9c7398d7ac1320f57cc81f2001 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 18 Nov 2017 18:34:25 +1300 Subject: [PATCH 16/32] Changed MSP command name to 'MSP_SET_ARMING_DISABLED' to make it consistent. --- src/main/interface/msp.c | 2 +- src/main/interface/msp_protocol.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 73e1b1a475..85481fcb8d 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1712,7 +1712,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; #endif - case MSP_ARMING_DISABLE: + case MSP_SET_ARMING_DISABLED: { const uint8_t command = sbufReadU8(src); if (command) { diff --git a/src/main/interface/msp_protocol.h b/src/main/interface/msp_protocol.h index 63c5b4691b..b795ab1802 100644 --- a/src/main/interface/msp_protocol.h +++ b/src/main/interface/msp_protocol.h @@ -214,7 +214,7 @@ #define MSP_CAMERA_CONTROL 98 -#define MSP_ARMING_DISABLE 99 +#define MSP_SET_ARMING_DISABLED 99 // // OSD specific From 1ae797f12ff0bdffae005e3a44cdd44cc8187b1c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 18 Nov 2017 05:44:00 +0000 Subject: [PATCH 17/32] Fixed cpu overclock in systemConfig --- src/main/fc/config.c | 27 ++++++++------------------- src/main/fc/config.h | 2 -- 2 files changed, 8 insertions(+), 21 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index a30e021953..2bb86c41e9 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -124,10 +124,15 @@ PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig, .name = { 0 } ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2); -#ifndef USE_OSD_SLAVE -#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) +#ifdef USE_OSD_SLAVE +PG_RESET_TEMPLATE(systemConfig_t, systemConfig, + .debug_mode = DEBUG_MODE, + .task_statistics = true, + .boardIdentifier = TARGET_BOARD_IDENTIFIER +); +#else PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .pidProfileIndex = 0, .activeRateProfile = 0, @@ -137,24 +142,8 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .powerOnArmingGraceTime = 5, .boardIdentifier = TARGET_BOARD_IDENTIFIER ); -#else -PG_RESET_TEMPLATE(systemConfig_t, systemConfig, - .pidProfileIndex = 0, - .activeRateProfile = 0, - .debug_mode = DEBUG_MODE, - .task_statistics = true, - .boardIdentifier = TARGET_BOARD_IDENTIFIER -); -#endif #endif -#ifdef USE_OSD_SLAVE -PG_RESET_TEMPLATE(systemConfig_t, systemConfig, - .debug_mode = DEBUG_MODE, - .task_statistics = true, - .boardIdentifier = TARGET_BOARD_IDENTIFIER -); -#endif #ifdef USE_ADC PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 06bb8a93e8..53858ff99a 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -75,9 +75,7 @@ typedef struct systemConfig_s { uint8_t activeRateProfile; uint8_t debug_mode; uint8_t task_statistics; -#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) uint8_t cpu_overclock; -#endif uint8_t powerOnArmingGraceTime; // in seconds char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER) + 1]; } systemConfig_t; From ade394b6f5c6f5211fa968e52c09c030033004f4 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 18 Nov 2017 16:42:28 +1300 Subject: [PATCH 18/32] More cleanup of RSSI, sending RSSI status over MSP. --- src/main/rx/flysky.c | 8 +++- src/main/rx/fport.c | 17 ++++++- src/main/rx/frsky_d.c | 8 +++- src/main/rx/rx.c | 104 +++++++++++++++++++++++++++--------------- src/main/rx/rx.h | 18 ++++++-- 5 files changed, 109 insertions(+), 46 deletions(-) diff --git a/src/main/rx/flysky.c b/src/main/rx/flysky.c index a10a4b701a..adac610932 100644 --- a/src/main/rx/flysky.c +++ b/src/main/rx/flysky.c @@ -173,7 +173,7 @@ static void checkTimeout (void) if(countTimeout > 31) { timeout = timings->syncPacket; - setRssiFiltered(0); + setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); } else { timeout = timings->packet; countTimeout++; @@ -197,7 +197,7 @@ static void checkRSSI (void) rssi_dBm = 50 + sum / (3 * FLYSKY_RSSI_SAMPLE_COUNT); // range about [95...52], -dBm int16_t tmp = 2280 - 24 * rssi_dBm;// convert to [0...1023] - setRssiFiltered(constrain(tmp, 0, 1023)); + setRssiFiltered(constrain(tmp, 0, 1023), RSSI_SOURCE_RX_PROTOCOL); } static bool isValidPacket (const uint8_t *packet) { @@ -384,6 +384,10 @@ void flySkyInit (const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rx startRxChannel = getNextChannel(0); } + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } + A7105WriteReg(A7105_0F_CHANNEL, startRxChannel); A7105Strobe(A7105_RX); // start listening diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 91a5ab24c3..3dedc476d9 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -46,6 +46,7 @@ #define FPORT_TIME_NEEDED_PER_FRAME_US 3000 #define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 +#define FPORT_MAX_TELEMETRY_AGE_MS 500 #define FPORT_FRAME_MARKER 0x7E @@ -132,7 +133,7 @@ static serialPort_t *fportPort; static bool telemetryEnabled = false; static void reportFrameError(uint8_t errorReason) { -static volatile uint16_t frameErrors = 0; + static volatile uint16_t frameErrors = 0; DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, ++frameErrors); DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason); @@ -237,6 +238,7 @@ static uint8_t fportFrameStatus(void) static smartPortPayload_t payloadBuffer; static smartPortPayload_t *mspPayload = NULL; static bool hasTelemetryRequest = false; + static timeUs_t lastRcFrameReceivedMs = 0; uint8_t result = RX_FRAME_PENDING; @@ -258,7 +260,9 @@ static uint8_t fportFrameStatus(void) } else { result = sbusChannelsDecode(&frame->data.controlData.channels); - setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024)); + setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); + + lastRcFrameReceivedMs = millis(); } break; @@ -323,6 +327,11 @@ static uint8_t fportFrameStatus(void) #endif } + if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) { + setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); + lastRcFrameReceivedMs = 0; + } + return result; } @@ -353,6 +362,10 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) #if defined(USE_TELEMETRY_SMARTPORT) telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport); #endif + + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } } return fportPort != NULL; diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c index 107704d4c2..61eb79b567 100644 --- a/src/main/rx/frsky_d.c +++ b/src/main/rx/frsky_d.c @@ -133,7 +133,7 @@ static void compute_RSSIdbm(uint8_t *packet) RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65; } - setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024)); + setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); } #if defined(USE_TELEMETRY_FRSKY) @@ -714,11 +714,17 @@ static void frskyD_Rx_Setup(rx_spi_protocol_e protocol) RX_enable(); #endif +#if defined(USE_FRSKY_D_TELEMETRY) #if defined(USE_TELEMETRY_FRSKY) initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi, &frSkyTelemetryWriteSpi); #endif + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } +#endif + // if(!frSkySpiDetect())//detect spi working routine // return; } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 22f948b2c7..8e9093353e 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -68,11 +68,12 @@ const char rcChannelLetters[] = "AERT12345678abcdefgh"; static uint16_t rssi = 0; // range: [0;1023] -static bool useMspRssi = true; static timeUs_t lastMspRssiUpdateUs = 0; #define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec +rssiSource_t rssiSource; + static bool rxDataReceived = false; static bool rxSignalReceived = false; static bool rxSignalReceivedNotDataDriven = false; @@ -360,6 +361,15 @@ void rxInit(void) } #endif +#if defined(USE_ADC) + if (feature(FEATURE_RSSI_ADC)) { + rssiSource = RSSI_SOURCE_ADC; + } else +#endif + if (rxConfig()->rssi_channel > 0) { + rssiSource = RSSI_SOURCE_RX_CHANNEL; + } + rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeConfig.channelCount); } @@ -612,10 +622,48 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig) } } -void setRssiFiltered(uint16_t newRssi) +void setRssiFiltered(uint16_t newRssi, rssiSource_t source) { + if (source != rssiSource) { + return; + } + rssi = newRssi; - useMspRssi = false; +} + +#define RSSI_SAMPLE_COUNT 16 +#define RSSI_MAX_VALUE 1023 + +void setRssiUnfiltered(uint16_t rssiValue, rssiSource_t source) +{ + if (source != rssiSource) { + return; + } + + static uint16_t rssiSamples[RSSI_SAMPLE_COUNT]; + static uint8_t rssiSampleIndex = 0; + static unsigned sum = 0; + + sum = sum + rssiValue; + sum = sum - rssiSamples[rssiSampleIndex]; + rssiSamples[rssiSampleIndex] = rssiValue; + rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT; + + int16_t rssiMean = sum / RSSI_SAMPLE_COUNT; + + rssi = rssiMean; +} + +void setRssiMsp(uint8_t newMspRssi) +{ + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_MSP; + } + + if (rssiSource == RSSI_SOURCE_MSP) { + rssi = ((uint16_t)newMspRssi) << 2; + lastMspRssiUpdateUs = micros(); + } } static void updateRSSIPWM(void) @@ -629,7 +677,7 @@ static void updateRSSIPWM(void) } // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023]; - setRssiFiltered(constrain((uint16_t)(((pwmRssi - 1000) / 1000.0f) * 1024.0f), 0, 1023)); + setRssiFiltered(constrain((uint16_t)(((pwmRssi - 1000) / 1000.0f) * 1024.0f), 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL); } static void updateRSSIADC(timeUs_t currentTimeUs) @@ -646,54 +694,36 @@ static void updateRSSIADC(timeUs_t currentTimeUs) const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); uint16_t rssiValue = (uint16_t)((1024.0f * adcRssiSample) / (rxConfig()->rssi_scale * 100.0f)); - rssiValue = constrain(rssiValue, 0, 1023); + rssiValue = constrain(rssiValue, 0, RSSI_MAX_VALUE); // RSSI_Invert option if (rxConfig()->rssi_invert) { - rssiValue = 1024 - rssiValue; + rssiValue = RSSI_MAX_VALUE - rssiValue; } - setRssiUnfiltered((uint16_t)rssiValue); + setRssiUnfiltered((uint16_t)rssiValue, RSSI_SOURCE_ADC); #endif } -#define RSSI_SAMPLE_COUNT 16 - -void setRssiUnfiltered(uint16_t rssiValue) -{ - static uint16_t rssiSamples[RSSI_SAMPLE_COUNT]; - static uint8_t rssiSampleIndex = 0; - static unsigned sum = 0; - - sum = sum + rssiValue; - sum = sum - rssiSamples[rssiSampleIndex]; - rssiSamples[rssiSampleIndex] = rssiValue; - rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT; - - int16_t rssiMean = sum / RSSI_SAMPLE_COUNT; - - setRssiFiltered(rssiMean); -} - -void setRssiMsp(uint8_t newMspRssi) -{ - if (useMspRssi) { - rssi = ((uint16_t)newMspRssi) << 2; - lastMspRssiUpdateUs = micros(); - } -} - void updateRSSI(timeUs_t currentTimeUs) { - - if (rxConfig()->rssi_channel > 0) { + switch (rssiSource) { + case RSSI_SOURCE_RX_CHANNEL: updateRSSIPWM(); - } else if (feature(FEATURE_RSSI_ADC)) { + + break; + case RSSI_SOURCE_ADC: updateRSSIADC(currentTimeUs); - } else if (useMspRssi) { + + break; + case RSSI_SOURCE_MSP: if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) { rssi = 0; } + + break; + default: + break; } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7667a66c5e..b7de73fcab 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -159,6 +159,16 @@ typedef struct rxRuntimeConfig_s { rcFrameStatusFnPtr rcFrameStatusFn; } rxRuntimeConfig_t; +typedef enum rssiSource_e { + RSSI_SOURCE_NONE = 0, + RSSI_SOURCE_ADC, + RSSI_SOURCE_RX_CHANNEL, + RSSI_SOURCE_RX_PROTOCOL, + RSSI_SOURCE_MSP, +} rssiSource_t; + +extern rssiSource_t rssiSource; + extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount void rxInit(void); @@ -169,10 +179,10 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); void parseRcChannels(const char *input, rxConfig_t *rxConfig); -void setRssiFiltered(uint16_t newRssi); -void setRssiUnfiltered(uint16_t rssiValue); -void setRssiMsp(uint8_t newMspRssi); -void updateRSSI(timeUs_t currentTimeUs); +void setRssiFiltered(const uint16_t newRssi, const rssiSource_t source); +void setRssiUnfiltered(const uint16_t rssiValue, const rssiSource_t source); +void setRssiMsp(const uint8_t newMspRssi); +void updateRSSI(const timeUs_t currentTimeUs); uint16_t getRssi(void); void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig); From cfc87d1891abac54ec70402d0e8e10e49323baff Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 18 Nov 2017 07:21:30 +0000 Subject: [PATCH 19/32] Better separation between GPS and NAV --- src/main/blackbox/blackbox.c | 1 - src/main/fc/config.c | 2 +- src/main/fc/fc_core.c | 4 +- src/main/fc/fc_init.c | 2 + src/main/fc/rc_controls.c | 1 - src/main/flight/navigation.c | 178 +--------------------- src/main/flight/navigation.h | 15 +- src/main/flight/pid.c | 3 +- src/main/interface/msp.c | 11 +- src/main/io/gps.c | 210 ++++++++++++++++++++++++-- src/main/io/gps.h | 24 +++ src/main/target/RCEXPLORERF3/target.h | 3 + src/main/target/common_fc_pre.h | 2 + src/main/telemetry/hott.c | 1 - src/main/telemetry/ltm.c | 1 - src/main/telemetry/smartport.c | 1 - src/test/unit/osd_unittest.cc | 2 +- 17 files changed, 243 insertions(+), 218 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 39db6c9f3e..67cf8450b2 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -54,7 +54,6 @@ #include "flight/failsafe.h" #include "flight/mixer.h" -#include "flight/navigation.h" #include "flight/pid.h" #include "flight/servos.h" diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2bb86c41e9..4eff243387 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -301,7 +301,7 @@ void activateConfig(void) useRcControlsConfig(currentPidProfile); useAdjustmentConfig(currentPidProfile); -#ifdef USE_GPS +#ifdef USE_NAV gpsUsePIDs(currentPidProfile); #endif diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 70d6332208..5f02e2af49 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -553,7 +553,7 @@ void processRx(timeUs_t currentTimeUs) } #endif -#ifdef USE_GPS +#ifdef USE_NAV if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } @@ -649,7 +649,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) processRcCommand(); -#ifdef USE_GPS +#ifdef USE_NAV if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 3521a5ea84..16ce58aeb1 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -607,7 +607,9 @@ void init(void) #ifdef USE_GPS if (feature(FEATURE_GPS)) { gpsInit(); +#ifdef USE_NAV navigationInit(); +#endif } #endif diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 142b1ddc25..a7ae69ca5b 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -60,7 +60,6 @@ #include "scheduler/scheduler.h" #include "flight/pid.h" -#include "flight/navigation.h" #include "flight/failsafe.h" static pidProfile_t *pidProfile; diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 7f6f98e59e..d31572d39d 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -22,7 +22,7 @@ #include "platform.h" -#ifdef USE_GPS +#ifdef USE_NAV #include "build/debug.h" @@ -72,16 +72,11 @@ bool areSticksInApModePosition(uint16_t ap_mode); // ********************** // GPS // ********************** -int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 }; // it's the angles that must be applied for GPS correction -int32_t GPS_home[2]; int32_t GPS_hold[2]; -uint16_t GPS_distanceToHome; // distance to home point in meters -int16_t GPS_directionToHome; // direction to home or hol point in degrees static int16_t nav[2]; static int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother -navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode // When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit() void navigationInit(void) @@ -104,23 +99,17 @@ void navigationInit(void) #define NAV_TAIL_FIRST 0 // true - copter comes in with tail first #define NAV_SET_TAKEOFF_HEADING 1 // true - when copter arrives to home position it rotates it's head to takeoff direction -#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency #define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise -static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing); //static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing); -static void GPS_calc_longitude_scaling(int32_t lat); -static void GPS_calc_velocity(void); static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng); -#ifdef USE_NAV static bool check_missed_wp(void); static void GPS_calc_poshold(void); static void GPS_calc_nav_rate(uint16_t max_speed); static void GPS_update_crosstrack(void); static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow); static int32_t wrap_36000(int32_t angle); -#endif static int32_t wrap_18000(int32_t error); @@ -149,7 +138,6 @@ typedef struct { static PID posholdPID[2]; static PID poshold_ratePID[2]; -#ifdef USE_NAV static PID_PARAM navPID_PARAM; static PID navPID[2]; @@ -181,7 +169,6 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param) // add in derivative component return pid_param->kD * pid->derivative; } -#endif static void reset_PID(PID *pid) { @@ -190,8 +177,6 @@ static void reset_PID(PID *pid) pid->last_derivative = 0; } -#define GPS_X 1 -#define GPS_Y 0 /****************** PI and PID controllers for GPS ********************///32938 -> 33160 @@ -200,18 +185,13 @@ static void reset_PID(PID *pid) #define NAV_SLOW_NAV true #define NAV_BANK_MAX 3000 // 30deg max banking when navigating (just for security and testing) -static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read -static int16_t actual_speed[2] = { 0, 0 }; -static float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles static int32_t error[2]; -#ifdef USE_NAV // The difference between the desired rate of travel and the actual rate of travel // updated after GPS read - 5-10hz static int16_t rate_error[2]; // The amount of angle correction applied to target_bearing to bring the copter back on its optimum path static int16_t crosstrack_error; -#endif // Currently used WP static int32_t GPS_WP[2]; @@ -238,95 +218,14 @@ static uint32_t wp_distance; // used for slow speed wind up when start navigation; static int16_t waypoint_speed_gov; -//////////////////////////////////////////////////////////////////////////////////// -// moving average filter variables -// -#define GPS_FILTER_VECTOR_LENGTH 5 - -static uint8_t GPS_filter_index = 0; -static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH]; -static int32_t GPS_filter_sum[2]; -static int32_t GPS_read[2]; -static int32_t GPS_filtered[2]; -static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000) -static uint16_t fraction3[2]; - // This is the angle from the copter to the "next_WP" location // with the addition of Crosstrack error in degrees * 100 static int32_t nav_bearing; // saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home -static int16_t nav_takeoff_bearing; -void GPS_calculateDistanceAndDirectionToHome(void) + +void navNewGpsData(void) { - if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything - uint32_t dist; - int32_t dir; - GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir); - GPS_distanceToHome = dist / 100; - GPS_directionToHome = dir / 100; - } else { - GPS_distanceToHome = 0; - GPS_directionToHome = 0; - } -} - -void onGpsNewData(void) -{ - static uint32_t nav_loopTimer; - - if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) { - return; - } - - if (!ARMING_FLAG(ARMED)) - DISABLE_STATE(GPS_FIX_HOME); - - if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) - GPS_reset_home_position(); - - // Apply moving average filter to GPS data -#if defined(GPS_FILTERING) - GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; - for (int axis = 0; axis < 2; axis++) { - GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude - GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t - - // How close we are to a degree line ? its the first three digits from the fractions of degree - // later we use it to Check if we are close to a degree line, if yes, disable averaging, - fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000; - - GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index]; - GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000); - GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; - GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000); - if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode... - if (fraction3[axis] > 1 && fraction3[axis] < 999) { - if (axis == LAT) { - gpsSol.llh.lat = GPS_filtered[LAT]; - } else { - gpsSol.llh.lon = GPS_filtered[LON]; - } - } - } - } -#endif - - // - // Calculate time delta for navigation loop, range 0-1.0f, in seconds - // - // Time for calculating x,y speed and navigation pids - dTnav = (float)(millis() - nav_loopTimer) / 1000.0f; - nav_loopTimer = millis(); - // prevent runup from bad GPS - dTnav = MIN(dTnav, 1.0f); - - GPS_calculateDistanceAndDirectionToHome(); - - // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating - GPS_calc_velocity(); - -#ifdef USE_NAV if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) { // we are navigating @@ -367,19 +266,6 @@ void onGpsNewData(void) break; } } //end of gps calcs -#endif -} - -void GPS_reset_home_position(void) -{ - if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { - GPS_home[LAT] = gpsSol.llh.lat; - GPS_home[LON] = gpsSol.llh.lon; - GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc - nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading - // Set ground altitude - ENABLE_STATE(GPS_FIX_HOME); - } } // reset navigation (stop the navigation processor, and clear nav) @@ -393,9 +279,7 @@ void GPS_reset_nav(void) nav[i] = 0; reset_PID(&posholdPID[i]); reset_PID(&poshold_ratePID[i]); -#ifdef USE_NAV reset_PID(&navPID[i]); -#endif } } @@ -411,12 +295,10 @@ void gpsUsePIDs(pidProfile_t *pidProfile) poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f; poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; -#ifdef USE_NAV navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f; navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f; navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f; navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; -#endif } // OK here is the onboard GPS code @@ -431,12 +313,6 @@ void gpsUsePIDs(pidProfile_t *pidProfile) // this is used to offset the shrinking longitude as we go towards the poles // It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter // -static void GPS_calc_longitude_scaling(int32_t lat) -{ - float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f; - GPS_scaleLonDown = cos_approx(rads); -} - //////////////////////////////////////////////////////////////////////////////////// // Sets the waypoint to navigate, reset neccessary variables and calculate initial values // @@ -454,7 +330,6 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon) waypoint_speed_gov = navigationConfig()->nav_speed_min; } -#ifdef USE_NAV //////////////////////////////////////////////////////////////////////////////////// // Check if we missed the destination somehow // @@ -465,24 +340,7 @@ static bool check_missed_wp(void) temp = wrap_18000(temp); return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees } -#endif -#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f -#define TAN_89_99_DEGREES 5729.57795f - -//////////////////////////////////////////////////////////////////////////////////// -// Get distance between two points in cm -// Get bearing from pos1 to pos2, returns an 1deg = 100 precision -static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing) -{ - float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees - float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown; - *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS; - - *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg - if (*bearing < 0) - *bearing += 36000; -} //////////////////////////////////////////////////////////////////////////////////// // keep old calculation function for compatibility (could be removed later) distance in meters, bearing in degree @@ -495,32 +353,6 @@ static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, // *bearing = d2 / 100; //convert to degrees //} -//////////////////////////////////////////////////////////////////////////////////// -// Calculate our current speed vector from gps position data -// -static void GPS_calc_velocity(void) -{ - static int16_t speed_old[2] = { 0, 0 }; - static int32_t last_coord[2] = { 0, 0 }; - static uint8_t init = 0; - - if (init) { - float tmp = 1.0f / dTnav; - actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp; - actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp; - - actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2; - actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2; - - speed_old[GPS_X] = actual_speed[GPS_X]; - speed_old[GPS_Y] = actual_speed[GPS_Y]; - } - init = 1; - - last_coord[LON] = gpsSol.llh.lon; - last_coord[LAT] = gpsSol.llh.lat; -} - //////////////////////////////////////////////////////////////////////////////////// // Calculate a location error between two gps coordinates // Because we are using lat and lon to do our distance errors here's a quick chart: @@ -536,7 +368,6 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in error[LAT] = *target_lat - *gps_lat; // Y Error } -#ifdef USE_NAV //////////////////////////////////////////////////////////////////////////////////// // Calculate nav_lat and nav_lon from the x and y error and the speed // @@ -642,7 +473,6 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow) } return max_speed; } -#endif //////////////////////////////////////////////////////////////////////////////////// // Utilities @@ -656,7 +486,6 @@ static int32_t wrap_18000(int32_t error) return error; } -#ifdef USE_NAV static int32_t wrap_36000(int32_t angle) { if (angle > 36000) @@ -665,7 +494,6 @@ static int32_t wrap_36000(int32_t angle) angle += 36000; return angle; } -#endif void updateGpsStateForHomeAndHoldMode(void) { diff --git a/src/main/flight/navigation.h b/src/main/flight/navigation.h index 1be0b447c9..d434299cee 100644 --- a/src/main/flight/navigation.h +++ b/src/main/flight/navigation.h @@ -18,13 +18,8 @@ #pragma once #include "common/axis.h" +#include "io/gps.h" -// navigation mode -typedef enum { - NAV_MODE_NONE = 0, - NAV_MODE_POSHOLD, - NAV_MODE_WP -} navigationMode_e; // FIXME ap_mode is badly named, it's a value that is compared to rcCommand, not a flag at it's name implies. @@ -40,22 +35,14 @@ typedef struct navigationConfig_s { PG_DECLARE(navigationConfig_t, navigationConfig); -extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction -extern int32_t GPS_home[2]; extern int32_t GPS_hold[2]; -extern uint16_t GPS_distanceToHome; // distance to home point in meters -extern int16_t GPS_directionToHome; // direction to home or hol point in degrees - -extern navigationMode_e nav_mode; // Navigation mode void navigationInit(void); -void GPS_reset_home_position(void); void GPS_reset_nav(void); void GPS_set_next_wp(int32_t* lat, int32_t* lon); void gpsUsePIDs(struct pidProfile_s *pidProfile); void updateGpsStateForHomeAndHoldMode(void); void updateGpsWaypointsAndMode(void); -void onGpsNewData(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1aeae117da..3c04c1ae41 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -45,7 +45,8 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" -#include "flight/navigation.h" + +#include "io/gps.h" #include "sensors/gyro.h" #include "sensors/acceleration.h" diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 85481fcb8d..9e0683719f 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1251,7 +1251,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sb } #endif // USE_OSD_SLAVE -#ifdef USE_GPS +#ifdef USE_NAV static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src) { uint8_t wp_no; @@ -1312,7 +1312,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) uint32_t i; uint8_t value; const unsigned int dataSize = sbufBytesRemaining(src); -#ifdef USE_GPS +#ifdef USE_NAV uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; #endif @@ -1743,7 +1743,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) gpsSol.groundSpeed = sbufReadU16(src); GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers break; - +#endif // USE_GPS +#ifdef USE_NAV case MSP_SET_WP: wp_no = sbufReadU8(src); //get the wp number lat = sbufReadU32(src); @@ -1768,7 +1769,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); } break; -#endif +#endif // USE_NAV case MSP_SET_FEATURE_CONFIG: featureClearAll(); @@ -2177,7 +2178,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro mspFc4waySerialCommand(dst, src, mspPostProcessFn); ret = MSP_RESULT_ACK; #endif -#ifdef USE_GPS +#ifdef USE_NAV } else if (cmdMSP == MSP_WP) { mspFcWpCommand(dst, src); ret = MSP_RESULT_ACK; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index b52266b2b7..cdb1deea7d 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -48,7 +48,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" -#include "flight/navigation.h" +#include "flight/imu.h" #include "flight/pid.h" #include "sensors/sensors.h" @@ -71,6 +71,28 @@ static char *gpsPacketLogChar = gpsPacketLog; // ********************** // GPS // ********************** +int32_t GPS_home[2]; +uint16_t GPS_distanceToHome; // distance to home point in meters +int16_t GPS_directionToHome; // direction to home or hol point in degrees +int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 }; // it's the angles that must be applied for GPS correction +float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read +int16_t actual_speed[2] = { 0, 0 }; +int16_t nav_takeoff_bearing; +navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode + +// moving average filter variables +#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency +#ifdef GPS_FILTERING +#define GPS_FILTER_VECTOR_LENGTH 5 +static uint8_t GPS_filter_index = 0; +static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH]; +static int32_t GPS_filter_sum[2]; +static int32_t GPS_read[2]; +static int32_t GPS_filtered[2]; +static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000) +static uint16_t fraction3[2]; +#endif + gpsSolutionData_t gpsSol; uint32_t GPS_packetCount = 0; uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received. @@ -111,6 +133,7 @@ static const gpsInitData_t gpsInitData[] = { #define DEFAULT_BAUD_RATE_INDEX 0 +#ifdef USE_GPS_UBLOX static const uint8_t ubloxInit[] = { //Preprocessor Pedestrian Dynamic Platform Model Option #if defined(GPS_UBLOX_MODE_PEDESTRIAN) @@ -179,7 +202,7 @@ static const ubloxSbas_t ubloxSbas[] = { { SBAS_MSAS, { 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}}, { SBAS_GAGAN, { 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}} }; - +#endif // USE_GPS_UBLOX typedef enum { GPS_UNKNOWN, @@ -212,8 +235,12 @@ static void shiftPacketLog(void) } static void gpsNewData(uint16_t c); +#ifdef USE_GPS_NMEA static bool gpsNewFrameNMEA(char c); +#endif +#ifdef USE_GPS_UBLOX static bool gpsNewFrameUBLOX(uint8_t data); +#endif static void gpsSetState(gpsState_e state) { @@ -268,6 +295,7 @@ void gpsInit(void) gpsSetState(GPS_INITIALIZING); } +#ifdef USE_GPS_NMEA void gpsInitNmea(void) { #if defined(COLIBRI_RACE) || defined(LUX_RACE) @@ -316,7 +344,9 @@ void gpsInitNmea(void) break; } } +#endif // USE_GPS_NMEA +#ifdef USE_GPS_UBLOX void gpsInitUblox(void) { uint32_t now; @@ -400,17 +430,22 @@ void gpsInitUblox(void) break; } } +#endif // USE_GPS_UBLOX void gpsInitHardware(void) { switch (gpsConfig()->provider) { - case GPS_NMEA: - gpsInitNmea(); - break; + case GPS_NMEA: +#ifdef USE_GPS_NMEA + gpsInitNmea(); +#endif + break; - case GPS_UBLOX: - gpsInitUblox(); - break; + case GPS_UBLOX: +#ifdef USE_GPS_UBLOX + gpsInitUblox(); +#endif + break; } } @@ -494,12 +529,17 @@ static void gpsNewData(uint16_t c) bool gpsNewFrame(uint8_t c) { switch (gpsConfig()->provider) { - case GPS_NMEA: // NMEA - return gpsNewFrameNMEA(c); - case GPS_UBLOX: // UBX binary - return gpsNewFrameUBLOX(c); + case GPS_NMEA: // NMEA +#ifdef USE_GPS_NMEA + return gpsNewFrameNMEA(c); +#endif + break; + case GPS_UBLOX: // UBX binary +#ifdef USE_GPS_UBLOX + return gpsNewFrameUBLOX(c); +#endif + break; } - return false; } @@ -566,6 +606,7 @@ static uint32_t GPS_coord_to_degrees(char *coordinateString) */ // helper functions +#ifdef USE_GPS_NMEA static uint32_t grab_fields(char *src, uint8_t mult) { // convert string to uint32 uint32_t i; @@ -764,7 +805,9 @@ static bool gpsNewFrameNMEA(char c) } return frameOK; } +#endif // USE_GPS_NMEA +#ifdef USE_GPS_UBLOX // UBX support typedef struct { uint8_t preamble1; @@ -1086,9 +1129,10 @@ static bool gpsNewFrameUBLOX(uint8_t data) } return parsed; } +#endif // USE_GPS_UBLOX static void gpsHandlePassthrough(uint8_t data) - { +{ gpsNewData(data); #ifdef USE_DASHBOARD if (feature(FEATURE_DASHBOARD)) { @@ -1114,4 +1158,142 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL); } + +float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles + +void GPS_calc_longitude_scaling(int32_t lat) +{ + float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f; + GPS_scaleLonDown = cos_approx(rads); +} + + +void GPS_reset_home_position(void) +{ + if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { + GPS_home[LAT] = gpsSol.llh.lat; + GPS_home[LON] = gpsSol.llh.lon; + GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc +#ifdef USE_NAV + nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading +#endif + // Set ground altitude + ENABLE_STATE(GPS_FIX_HOME); + } +} + +//////////////////////////////////////////////////////////////////////////////////// +#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f +#define TAN_89_99_DEGREES 5729.57795f +// Get distance between two points in cm +// Get bearing from pos1 to pos2, returns an 1deg = 100 precision +void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing) +{ + float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees + float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown; + *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS; + + *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg + if (*bearing < 0) + *bearing += 36000; +} + +void GPS_calculateDistanceAndDirectionToHome(void) +{ + if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything + uint32_t dist; + int32_t dir; + GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir); + GPS_distanceToHome = dist / 100; + GPS_directionToHome = dir / 100; + } else { + GPS_distanceToHome = 0; + GPS_directionToHome = 0; + } +} + +//////////////////////////////////////////////////////////////////////////////////// +// Calculate our current speed vector from gps position data +// +static void GPS_calc_velocity(void) +{ + static int16_t speed_old[2] = { 0, 0 }; + static int32_t last_coord[2] = { 0, 0 }; + static uint8_t init = 0; + + if (init) { + float tmp = 1.0f / dTnav; + actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp; + actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp; + + actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2; + actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2; + + speed_old[GPS_X] = actual_speed[GPS_X]; + speed_old[GPS_Y] = actual_speed[GPS_Y]; + } + init = 1; + + last_coord[LON] = gpsSol.llh.lon; + last_coord[LAT] = gpsSol.llh.lat; +} + +void onGpsNewData(void) +{ + if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) { + return; + } + + if (!ARMING_FLAG(ARMED)) + DISABLE_STATE(GPS_FIX_HOME); + + if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) + GPS_reset_home_position(); + + // Apply moving average filter to GPS data +#if defined(GPS_FILTERING) + GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; + for (int axis = 0; axis < 2; axis++) { + GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude + GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t + + // How close we are to a degree line ? its the first three digits from the fractions of degree + // later we use it to Check if we are close to a degree line, if yes, disable averaging, + fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000; + + GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index]; + GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000); + GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; + GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000); + if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode... + if (fraction3[axis] > 1 && fraction3[axis] < 999) { + if (axis == LAT) { + gpsSol.llh.lat = GPS_filtered[LAT]; + } else { + gpsSol.llh.lon = GPS_filtered[LON]; + } + } + } + } +#endif + + // + // Calculate time delta for navigation loop, range 0-1.0f, in seconds + // + // Time for calculating x,y speed and navigation pids + static uint32_t nav_loopTimer; + dTnav = (float)(millis() - nav_loopTimer) / 1000.0f; + nav_loopTimer = millis(); + // prevent runup from bad GPS + dTnav = MIN(dTnav, 1.0f); + + GPS_calculateDistanceAndDirectionToHome(); + // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating + GPS_calc_velocity(); + +#ifdef USE_NAV + navNewGpsData(); +#endif +} + #endif diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 8ac1c6ab72..7a5bf622e0 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -17,6 +17,7 @@ #pragma once +#include "common/axis.h" #include "common/time.h" #include "config/parameter_group.h" @@ -25,6 +26,8 @@ #define LON 1 #define GPS_DEGREES_DIVIDER 10000000L +#define GPS_X 1 +#define GPS_Y 0 typedef enum { GPS_NMEA = 0, @@ -114,6 +117,22 @@ typedef struct gpsData_s { #define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display. extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; +extern int32_t GPS_home[2]; +extern uint16_t GPS_distanceToHome; // distance to home point in meters +extern int16_t GPS_directionToHome; // direction to home or hol point in degrees +extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction +extern float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read +extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles +extern int16_t actual_speed[2]; +extern int16_t nav_takeoff_bearing; +// navigation mode +typedef enum { + NAV_MODE_NONE = 0, + NAV_MODE_POSHOLD, + NAV_MODE_WP +} navigationMode_e; +extern navigationMode_e nav_mode; // Navigation mode + extern gpsData_t gpsData; extern gpsSolutionData_t gpsSol; @@ -134,4 +153,9 @@ void gpsUpdate(timeUs_t currentTimeUs); bool gpsNewFrame(uint8_t c); struct serialPort_s; void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort); +void onGpsNewData(void); +void GPS_reset_home_position(void); +void GPS_calc_longitude_scaling(int32_t lat); +void navNewGpsData(void); +void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing); diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 603b7cfc5c..6ede4690f6 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -104,6 +104,9 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART1 +#define USE_GPS +#define USE_GPS_UBLOX +#define USE_GPS_NMEA #define USE_NAV #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index fea43db40c..8ef1ec2778 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -145,6 +145,8 @@ #if (FLASH_SIZE > 256) // Temporarily moved GPS here because of overflowing flash size on F3 #define USE_GPS +#define USE_GPS_UBLOX +#define USE_GPS_NMEA #define USE_NAV #define USE_UNCOMMON_MIXERS #define USE_OSD_ADJUSTMENTS diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 36f39cc53a..833207f813 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -74,7 +74,6 @@ #include "flight/altitude.h" #include "flight/pid.h" -#include "flight/navigation.h" #include "io/gps.h" diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index b378cf0ba8..233e7ee11f 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -70,7 +70,6 @@ #include "flight/imu.h" #include "flight/failsafe.h" #include "flight/altitude.h" -#include "flight/navigation.h" #include "telemetry/telemetry.h" #include "telemetry/ltm.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index cc5e44f58e..7ccddee3ad 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -36,7 +36,6 @@ #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" -#include "flight/navigation.h" #include "interface/msp.h" diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 7c828ff07c..e23ab36001 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -57,7 +57,7 @@ extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint8_t GPS_numSat; uint16_t GPS_distanceToHome; - uint16_t GPS_directionToHome; + int16_t GPS_directionToHome; int32_t GPS_coord[2]; gpsSolutionData_t gpsSol; From 5eefc768d1e29354762041f50380666a949703ab Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 19 Nov 2017 10:22:17 +1300 Subject: [PATCH 20/32] Removed features from BETAFLIGHTF3 target to make it fit the flash space. --- src/main/target/BETAFLIGHTF3/target.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index a45cc4c3dd..8e6e75c172 100644 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -20,6 +20,12 @@ #define TARGET_BOARD_IDENTIFIER "BFF3" +// Removing some features to make the firmware fit the flash space +#undef USE_TELEMETRY_HOTT +#undef USE_TELEMETRY_JETIEXBUS +#undef USE_TELEMETRY_LTM + + #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define BEEPER PC15 From 86373122a1ed708f90052cb28fbf81d3113447b1 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 18 Nov 2017 18:18:23 +1300 Subject: [PATCH 21/32] Added data for lua TX scripts to MSP. --- src/main/interface/msp.c | 63 ++++++++++++++++++++++++------- src/main/interface/msp_protocol.h | 4 +- 2 files changed, 52 insertions(+), 15 deletions(-) diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 85481fcb8d..5e9577c666 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -150,6 +150,8 @@ typedef enum { #define RATEPROFILE_MASK (1 << 7) #endif //USE_OSD_SLAVE +#define RTC_NOT_SUPPORTED 0xff + #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #define ESC_4WAY 0xff @@ -1220,9 +1222,41 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected } } -#endif - break; + break; +#endif + + case MSP_TX_INFO: + sbufWriteU8(dst, rssiSource); + uint8_t rtcDateTimeIsSet = 0; +#ifdef USE_RTC_TIME + dateTime_t dt; + if (rtcGetDateTime(&dt)) { + rtcDateTimeIsSet = 1; + } +#else + rtcDateTimeIsSet = RTC_NOT_SUPPORTED; +#endif + sbufWriteU8(dst, rtcDateTimeIsSet); + + break; +#ifdef USE_RTC_TIME + case MSP_RTC: + { + dateTime_t dt; + if (rtcGetDateTime(&dt)) { + sbufWriteU16(dst, dt.year); + sbufWriteU8(dst, dt.month); + sbufWriteU8(dst, dt.day); + sbufWriteU8(dst, dt.hours); + sbufWriteU8(dst, dt.minutes); + sbufWriteU8(dst, dt.seconds); + sbufWriteU16(dst, dt.millis); + } + } + + break; +#endif default: return false; } @@ -1943,18 +1977,19 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef USE_RTC_TIME case MSP_SET_RTC: - { - dateTime_t dt; - dt.year = sbufReadU16(src); - dt.month = sbufReadU8(src); - dt.day = sbufReadU8(src); - dt.hours = sbufReadU8(src); - dt.minutes = sbufReadU8(src); - dt.seconds = sbufReadU8(src); - dt.millis = 0; - rtcSetDateTime(&dt); - } - break; + { + dateTime_t dt; + dt.year = sbufReadU16(src); + dt.month = sbufReadU8(src); + dt.day = sbufReadU8(src); + dt.hours = sbufReadU8(src); + dt.minutes = sbufReadU8(src); + dt.seconds = sbufReadU8(src); + dt.millis = 0; + rtcSetDateTime(&dt); + } + + break; #endif case MSP_TX_INFO: diff --git a/src/main/interface/msp_protocol.h b/src/main/interface/msp_protocol.h index b795ab1802..61f5b66b20 100644 --- a/src/main/interface/msp_protocol.h +++ b/src/main/interface/msp_protocol.h @@ -230,7 +230,8 @@ #define MSP_BEEPER_CONFIG 184 #define MSP_SET_BEEPER_CONFIG 185 -#define MSP_TX_INFO 186 +#define MSP_SET_TX_INFO 186 // in message Used to send runtime information from TX lua scripts to the firmware +#define MSP_TX_INFO 187 // out message Used by TX lua scripts to read information from the firmware // // Multwii original MSP commands @@ -324,3 +325,4 @@ #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration #define MSP_SET_4WAY_IF 245 //in message Sets 4way interface #define MSP_SET_RTC 246 //in message Sets the RTC clock +#define MSP_RTC 247 //out message Gets the RTC clock From 34b48badb918efc63daf4ef6bb0fb3e1ffb3d817 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 19 Nov 2017 04:24:40 +0000 Subject: [PATCH 22/32] Scaled PID deltaT calculations from us to seconds --- src/main/flight/pid.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1aeae117da..b2279e36d9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -410,11 +410,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an const float tpaFactor = getThrottlePIDAttenuation(); const float motorMixRange = getMotorMixRange(); static timeUs_t crashDetectedAtUs; - static timeUs_t previousTime; + static timeUs_t previousTimeUs; - // calculate actual deltaT - const float deltaT = currentTimeUs - previousTime; - previousTime = currentTimeUs; + // calculate actual deltaT in seconds + const float deltaT = (currentTimeUs - previousTimeUs) * 0.000001f; + previousTimeUs = currentTimeUs; // Dynamic ki component to gradually scale back integration when above windup point const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); From 2bb561aa61e1883c43ca70d6f16aab9a0bab570c Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Sun, 19 Nov 2017 10:13:41 +0000 Subject: [PATCH 23/32] Add RTC date/time to OSD stats Fixes #4575 --- src/main/io/osd.c | 15 ++++++++++++++- src/main/io/osd.h | 1 + src/test/Makefile | 4 +++- src/test/unit/osd_unittest.cc | 21 +++++++++++++++++++++ 4 files changed, 39 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1568f53ade..5a4d165ee7 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -861,6 +861,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = true; osdConfig->enabled_stats[OSD_STAT_TIMER_1] = false; osdConfig->enabled_stats[OSD_STAT_TIMER_2] = true; + osdConfig->enabled_stats[OSD_STAT_RTC_DATE_TIME] = false; osdConfig->units = OSD_UNIT_METRIC; @@ -1103,11 +1104,23 @@ static bool isSomeStatEnabled(void) { static void osdShowStats(void) { uint8_t top = 2; - char buff[10]; + char buff[OSD_ELEMENT_BUFFER_LENGTH]; displayClearScreen(osdDisplayPort); displayWrite(osdDisplayPort, 2, top++, " --- STATS ---"); + if (osdConfig()->enabled_stats[OSD_STAT_RTC_DATE_TIME]) { + bool success = false; +#ifdef USE_RTC_TIME + success = printRtcDateTime(&buff[0]); +#endif + if (!success) { + tfp_sprintf(buff, "NO RTC"); + } + + displayWrite(osdDisplayPort, 2, top++, buff); + } + if (osdConfig()->enabled_stats[OSD_STAT_TIMER_1]) { osdFormatTimer(buff, false, OSD_TIMER_1); osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index c79ccc2133..2118fbc020 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -102,6 +102,7 @@ typedef enum { OSD_STAT_TIMER_2, OSD_STAT_MAX_DISTANCE, OSD_STAT_BLACKBOX_NUMBER, + OSD_STAT_RTC_DATE_TIME, OSD_STAT_COUNT // MUST BE LAST } osd_stats_e; diff --git a/src/test/Makefile b/src/test/Makefile index 6e5690db02..ec0243f4e5 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -155,10 +155,12 @@ osd_unittest_SRC := \ $(USER_DIR)/drivers/display.c \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/common/printf.c \ + $(USER_DIR)/common/time.c \ $(USER_DIR)/fc/runtime_config.c osd_unittest_DEFINES := \ - USE_OSD + USE_OSD \ + USE_RTC_TIME parameter_groups_unittest_SRC := \ diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index e23ab36001..718f752966 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -29,6 +29,8 @@ extern "C" { #include "config/parameter_group_ids.h" + #include "common/time.h" + #include "drivers/max7456_symbols.h" #include "fc/config.h" @@ -291,6 +293,7 @@ TEST(OsdTest, TestStatsImperial) osdConfigMutable()->enabled_stats[OSD_STAT_END_BATTERY] = true; osdConfigMutable()->enabled_stats[OSD_STAT_TIMER_1] = true; osdConfigMutable()->enabled_stats[OSD_STAT_TIMER_2] = true; + osdConfigMutable()->enabled_stats[OSD_STAT_RTC_DATE_TIME] = true; osdConfigMutable()->enabled_stats[OSD_STAT_MAX_DISTANCE] = true; osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = false; @@ -310,6 +313,18 @@ TEST(OsdTest, TestStatsImperial) // a GPS fix is present stateFlags |= GPS_FIX | GPS_FIX_HOME; + // and + // this RTC time + dateTime_t dateTime; + dateTime.year = 2017; + dateTime.month = 11; + dateTime.day = 19; + dateTime.hours = 10; + dateTime.minutes = 12; + dateTime.seconds = 0; + dateTime.millis = 0; + rtcSetDateTime(&dateTime); + // when // the craft is armed doTestArm(); @@ -347,6 +362,7 @@ TEST(OsdTest, TestStatsImperial) // then // statistics screen should display the following int row = 3; + displayPortTestBufferSubstring(2, row++, "2017-11-19 10:12:"); displayPortTestBufferSubstring(2, row++, "TOTAL ARM : 00:05.00"); displayPortTestBufferSubstring(2, row++, "LAST ARM : 00:03"); displayPortTestBufferSubstring(2, row++, "MAX SPEED : 28"); @@ -397,6 +413,7 @@ TEST(OsdTest, TestStatsMetric) // then // statistics screen should display the following int row = 3; + displayPortTestBufferSubstring(2, row++, "2017-11-19 10:12:"); displayPortTestBufferSubstring(2, row++, "TOTAL ARM : 00:07.50"); displayPortTestBufferSubstring(2, row++, "LAST ARM : 00:02"); displayPortTestBufferSubstring(2, row++, "MAX SPEED : 28"); @@ -873,6 +890,10 @@ extern "C" { return simulationTime; } + uint32_t millis() { + return micros() / 1000; + } + bool isBeeperOn() { return false; } From 5924eb83cfa70ce100896c2a163214fb2b2d9dbf Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 19 Nov 2017 13:14:58 +0000 Subject: [PATCH 24/32] Reserve DSHOT commands --- src/main/drivers/pwm_output.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 5e42ea9047..387aef4119 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -61,6 +61,8 @@ typedef enum { DSHOT_CMD_LED1_OFF, // BLHeli32 only DSHOT_CMD_LED2_OFF, // BLHeli32 only DSHOT_CMD_LED3_OFF, // BLHeli32 only + DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off + DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off DSHOT_CMD_MAX = 47 } dshotCommands_e; From d22b01b5ecf7aa30868f2ba1d58bf92ab4c2dbf5 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 19 Nov 2017 10:10:47 +0000 Subject: [PATCH 25/32] Added USE_ALT_HOLD build flag --- src/main/fc/fc_core.c | 2 +- src/main/fc/fc_tasks.c | 12 +++++++----- src/main/flight/altitude.c | 4 ++-- src/main/interface/settings.c | 9 +++++---- src/main/target/common_fc_post.h | 5 +++++ src/main/target/common_fc_pre.h | 1 + 6 files changed, 21 insertions(+), 12 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 5f02e2af49..13e13e63fc 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -617,7 +617,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) } #endif -#if defined(USE_BARO) || defined(USE_SONAR) +#if defined(USE_ALT_HOLD) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index c576cfc00e..ecc585a160 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -140,12 +140,13 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) processRx(currentTimeUs); isRXDataNew = true; -#if !defined(USE_BARO) && !defined(USE_SONAR) +#if !defined(USE_ALT_HOLD) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); #endif updateArmingStatus(); +#ifdef USE_ALT_HOLD #ifdef USE_BARO if (sensors(SENSOR_BARO)) { updateAltHoldState(); @@ -157,6 +158,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) updateSonarAltHoldState(); } #endif +#endif // USE_ALT_HOLD } #endif @@ -183,7 +185,7 @@ static void taskUpdateBaro(timeUs_t currentTimeUs) } #endif -#if defined(USE_BARO) || defined(USE_SONAR) +#if defined(USE_ALT_HOLD) static void taskCalculateAltitude(timeUs_t currentTimeUs) { if (false @@ -196,7 +198,7 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs) ) { calculateEstimatedAltitude(currentTimeUs); }} -#endif +#endif // USE_ALT_HOLD #ifdef USE_TELEMETRY static void taskTelemetry(timeUs_t currentTimeUs) @@ -295,7 +297,7 @@ void fcTasksInit(void) #ifdef USE_SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif -#if defined(USE_BARO) || defined(USE_SONAR) +#ifdef USE_ALT_HOLD setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif #ifdef USE_DASHBOARD @@ -501,7 +503,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#if defined(USE_BARO) || defined(USE_SONAR) +#if defined(USE_ALT_HOLD) [TASK_ALTITUDE] = { .taskName = "ALTITUDE", .taskFunc = taskCalculateAltitude, diff --git a/src/main/flight/altitude.c b/src/main/flight/altitude.c index 2ddb331441..c8b55db7cd 100644 --- a/src/main/flight/altitude.c +++ b/src/main/flight/altitude.c @@ -52,7 +52,7 @@ static int32_t estimatedVario = 0; // variometer in cm/s static int32_t estimatedAltitude = 0; // in cm -#if defined(USE_BARO) || defined(USE_SONAR) +#if defined(USE_ALT_HOLD) enum { DEBUG_ALTITUDE_ACC, @@ -294,7 +294,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old); accZ_old = accZ_tmp; } -#endif // defined(USE_BARO) || defined(USE_SONAR) +#endif // USE_ALT_HOLD int32_t getEstimatedAltitude(void) { diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index ce6bc2c456..4d306c5bbb 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -616,18 +616,19 @@ const clivalue_t valueTable[] = { { "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) }, +#ifdef USE_ALT_HOLD { "p_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].P) }, { "i_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].I) }, { "d_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].D) }, + { "p_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) }, + { "i_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) }, + { "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) }, +#endif { "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) }, { "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) }, { "d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) }, - { "p_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) }, - { "i_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) }, - { "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) }, - { "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) }, { "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) }, diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index bf20956a17..ac7cb26ebb 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_fc_post.h @@ -61,6 +61,11 @@ #endif #endif +// undefine USE_ALT_HOLD if there is no baro or sonar to support it +#if defined(USE_ALT_HOLD) && !defined(USE_BARO) && !defined(USE_SONAR) +#undef USE_ALT_HOLD +#endif + /* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */ #if !defined(VTX_COMMON) || !defined(VTX_CONTROL) #undef VTX_COMMON diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 8ef1ec2778..60327c1042 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -148,6 +148,7 @@ #define USE_GPS_UBLOX #define USE_GPS_NMEA #define USE_NAV +#define USE_ALT_HOLD #define USE_UNCOMMON_MIXERS #define USE_OSD_ADJUSTMENTS #endif From 9564fab82303902f4a6050ee0c7341935c6a1ad9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 19 Nov 2017 13:38:43 +0000 Subject: [PATCH 26/32] Changed some CLI settings from USE_GPS to USE_NAV --- src/main/interface/settings.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index ce6bc2c456..a712e99887 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -554,7 +554,7 @@ const clivalue_t valueTable[] = { #endif // PG_NAVIGATION_CONFIG -#ifdef USE_GPS +#ifdef USE_NAV { "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, gps_wp_radius) }, { "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_controls_heading) }, { "nav_speed_min", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_speed_min) }, @@ -632,7 +632,7 @@ const clivalue_t valueTable[] = { { "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) }, { "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) }, -#ifdef USE_GPS +#ifdef USE_NAV { "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].P) }, { "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].I) }, { "gps_pos_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].D) }, From 417acc8f013950a64b28d58fa300190860ac956c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 19 Nov 2017 13:13:14 +0000 Subject: [PATCH 27/32] Retain altitude calculation --- src/main/fc/fc_tasks.c | 8 ++++---- src/main/flight/altitude.c | 17 ++++++++++++----- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index ecc585a160..dd6d48b4cb 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -185,7 +185,7 @@ static void taskUpdateBaro(timeUs_t currentTimeUs) } #endif -#if defined(USE_ALT_HOLD) +#if defined(USE_BARO) || defined(USE_SONAR) static void taskCalculateAltitude(timeUs_t currentTimeUs) { if (false @@ -198,7 +198,7 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs) ) { calculateEstimatedAltitude(currentTimeUs); }} -#endif // USE_ALT_HOLD +#endif // USE_BARO || USE_SONAR #ifdef USE_TELEMETRY static void taskTelemetry(timeUs_t currentTimeUs) @@ -297,7 +297,7 @@ void fcTasksInit(void) #ifdef USE_SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif -#ifdef USE_ALT_HOLD +#if defined(USE_BARO) || defined(USE_SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif #ifdef USE_DASHBOARD @@ -503,7 +503,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#if defined(USE_ALT_HOLD) +#if defined(USE_BARO) || defined(USE_SONAR) [TASK_ALTITUDE] = { .taskName = "ALTITUDE", .taskFunc = taskCalculateAltitude, diff --git a/src/main/flight/altitude.c b/src/main/flight/altitude.c index c8b55db7cd..209b708fdc 100644 --- a/src/main/flight/altitude.c +++ b/src/main/flight/altitude.c @@ -52,13 +52,16 @@ static int32_t estimatedVario = 0; // variometer in cm/s static int32_t estimatedAltitude = 0; // in cm -#if defined(USE_ALT_HOLD) enum { DEBUG_ALTITUDE_ACC, DEBUG_ALTITUDE_VEL, DEBUG_ALTITUDE_HEIGHT }; +// 40hz update rate (20hz LPF on acc) +#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) + +#if defined(USE_ALT_HOLD) PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0); @@ -72,8 +75,6 @@ static int32_t errorVelocityI = 0; static int32_t altHoldThrottleAdjustment = 0; static int16_t initialThrottleHold; -// 40hz update rate (20hz LPF on acc) -#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) #define DEGREES_80_IN_DECIDEGREES 800 @@ -202,7 +203,9 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa return result; } +#endif // USE_ALT_HOLD +#if defined(USE_BARO) || defined(USE_SONAR) void calculateEstimatedAltitude(timeUs_t currentTimeUs) { static timeUs_t previousTimeUs = 0; @@ -280,7 +283,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero } -#endif // BARO +#endif // USE_BARO // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay @@ -290,11 +293,15 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // set vario estimatedVario = applyDeadband(vel_tmp, 5); +#ifdef USE_ALT_HOLD static float accZ_old = 0.0f; altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old); accZ_old = accZ_tmp; +#else + UNUSED(accZ_tmp); +#endif } -#endif // USE_ALT_HOLD +#endif // USE_BARO || USE_SONAR int32_t getEstimatedAltitude(void) { From 5c0bba565ad41c2e1b9ae3c03c4cddad6fee6835 Mon Sep 17 00:00:00 2001 From: Harpalyke Date: Sun, 19 Nov 2017 22:36:27 +0000 Subject: [PATCH 28/32] adding support for an external magnetometer to matek F405 target --- src/main/target/MATEKF405/target.h | 3 +++ src/main/target/MATEKF405/target.mk | 1 + 2 files changed, 4 insertions(+) diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 83bc28d3fd..aab61407ed 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -58,6 +58,9 @@ #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG +#define MAG +#define USE_MAG_HMC5883 + // *************** Baro ************************** #define USE_I2C diff --git a/src/main/target/MATEKF405/target.mk b/src/main/target/MATEKF405/target.mk index 277dee3d24..cbb55c9a74 100644 --- a/src/main/target/MATEKF405/target.mk +++ b/src/main/target/MATEKF405/target.mk @@ -8,4 +8,5 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ drivers/max7456.c From 89702650e2281b2b2034f5efd47c41525c238140 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 20 Nov 2017 08:27:17 +0900 Subject: [PATCH 29/32] NOX support on 3.2.x --- src/main/target/NOX/config.c | 30 +++++++++ src/main/target/NOX/target.c | 40 ++++++++++++ src/main/target/NOX/target.h | 118 ++++++++++++++++++++++++++++++++++ src/main/target/NOX/target.mk | 7 ++ 4 files changed, 195 insertions(+) create mode 100644 src/main/target/NOX/config.c create mode 100644 src/main/target/NOX/target.c create mode 100644 src/main/target/NOX/target.h create mode 100644 src/main/target/NOX/target.mk diff --git a/src/main/target/NOX/config.c b/src/main/target/NOX/config.c new file mode 100644 index 0000000000..1826abc92c --- /dev/null +++ b/src/main/target/NOX/config.c @@ -0,0 +1,30 @@ +/* + * 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 + +#ifdef TARGET_CONFIG + +#include "io/serial.h" + +void targetConfiguration(void) +{ +} +#endif diff --git a/src/main/target/NOX/target.c b/src/main/target/NOX/target.c new file mode 100644 index 0000000000..d4fd85b223 --- /dev/null +++ b/src/main/target/NOX/target.c @@ -0,0 +1,40 @@ +/* + * 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 "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), // T2C3 + + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // T2C2(), T5C2() + DEF_TIM(TIM1, CH1N, PA7, TIM_USE_MOTOR, 0, 0), // T1C1N(2,3,6), T3C2(1,5,5) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // T4C3(1,7,2), T10C1(X) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // T1C3N(2,6,6), T3C4(1,7,5) + + DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), // T2C1(1,5,3) + + // Backdoor timers on UART2 (Too bad that UART1 collides with TIM4) + DEF_TIM(TIM9, CH1, PA2, TIM_USE_NONE, 0, 0), // UART2_TX + DEF_TIM(TIM9, CH2, PA3, TIM_USE_NONE, 0, 0), // UART2_RX +}; diff --git a/src/main/target/NOX/target.h b/src/main/target/NOX/target.h new file mode 100644 index 0000000000..6b4d5e4239 --- /dev/null +++ b/src/main/target/NOX/target.h @@ -0,0 +1,118 @@ +/* + * This 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. + * + * This software 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 this software. If not, see . + */ + +#pragma once + +#define TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "NOX1" +#define USBD_PRODUCT_STRING "NoxF4V1" + +#define LED0_PIN PA4 + +#define BEEPER PC13 +#define BEEPER_INVERTED + +#define INVERTER_PIN_UART2 PC14 + +#define ACC +#define USE_ACC_SPI_MPU6500 +#define USE_ACC_SPI_MPU6000 + +#define GYRO +#define USE_GYRO_SPI_MPU6500 +#define USE_GYRO_SPI_MPU6000 + +#define MPU6500_CS_PIN PB12 +#define MPU6500_SPI_INSTANCE SPI2 + +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 + +#define USE_EXTI +#define MPU_INT_EXTI PA8 +#define USE_MPU_DATA_READY_SIGNAL + +#define BARO +#define USE_BARO_BMP280 +#define USE_BARO_SPI_BMP280 +#define BMP280_SPI_INSTANCE SPI2 +#define BMP280_CS_PIN PA9 + +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN PA10 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + +#define USE_VCP +//#define VBUS_SENSING_PIN PC5 + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL1_RX_PIN NONE +#define SOFTSERIAL1_TX_PIN NONE + +#define USE_SOFTSERIAL2 +#define SOFTSERIAL2_RX_PIN PA2 // Backdoor timer on UART2_TX +#define SOFTSERIAL2_TX_PIN NONE + +#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART2, SOFTSERIAL1, SOFTSERIAL2 + +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PPM + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_ADC +#define CURRENT_METER_ADC_PIN NONE // PA6 Available from TP33 +#define VBAT_ADC_PIN PA5 // 11:1 (10K + 1K) divider + +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ESC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ESC + +#define SERIALRX_UART SERIAL_PORT_USART2 + +#define TRANSPONDER + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_SOFTSERIAL | FEATURE_ESC_SENSOR) +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) +#define TARGET_IO_PORTB (0xffff & ~(BIT(2)|BIT(11))) +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) + +#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(9) ) diff --git a/src/main/target/NOX/target.mk b/src/main/target/NOX/target.mk new file mode 100644 index 0000000000..ce5b31004c --- /dev/null +++ b/src/main/target/NOX/target.mk @@ -0,0 +1,7 @@ +F411_TARGETS += $(TARGET) +FEATURES += VCP +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/max7456.c From b3a0d0e6cf142852e36f9b92965d04b5d440cde2 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 20 Nov 2017 14:02:42 +0900 Subject: [PATCH 30/32] Revert 8970265 --- src/main/target/NOX/config.c | 30 --------- src/main/target/NOX/target.c | 40 ------------ src/main/target/NOX/target.h | 118 ---------------------------------- src/main/target/NOX/target.mk | 7 -- 4 files changed, 195 deletions(-) delete mode 100644 src/main/target/NOX/config.c delete mode 100644 src/main/target/NOX/target.c delete mode 100644 src/main/target/NOX/target.h delete mode 100644 src/main/target/NOX/target.mk diff --git a/src/main/target/NOX/config.c b/src/main/target/NOX/config.c deleted file mode 100644 index 1826abc92c..0000000000 --- a/src/main/target/NOX/config.c +++ /dev/null @@ -1,30 +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 . - */ - -#include -#include - -#include - -#ifdef TARGET_CONFIG - -#include "io/serial.h" - -void targetConfiguration(void) -{ -} -#endif diff --git a/src/main/target/NOX/target.c b/src/main/target/NOX/target.c deleted file mode 100644 index d4fd85b223..0000000000 --- a/src/main/target/NOX/target.c +++ /dev/null @@ -1,40 +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 . - */ - -#include - -#include -#include "drivers/io.h" - -#include "drivers/dma.h" -#include "drivers/timer.h" -#include "drivers/timer_def.h" - -const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), // T2C3 - - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // T2C2(), T5C2() - DEF_TIM(TIM1, CH1N, PA7, TIM_USE_MOTOR, 0, 0), // T1C1N(2,3,6), T3C2(1,5,5) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // T4C3(1,7,2), T10C1(X) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // T1C3N(2,6,6), T3C4(1,7,5) - - DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), // T2C1(1,5,3) - - // Backdoor timers on UART2 (Too bad that UART1 collides with TIM4) - DEF_TIM(TIM9, CH1, PA2, TIM_USE_NONE, 0, 0), // UART2_TX - DEF_TIM(TIM9, CH2, PA3, TIM_USE_NONE, 0, 0), // UART2_RX -}; diff --git a/src/main/target/NOX/target.h b/src/main/target/NOX/target.h deleted file mode 100644 index 6b4d5e4239..0000000000 --- a/src/main/target/NOX/target.h +++ /dev/null @@ -1,118 +0,0 @@ -/* - * This 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. - * - * This software 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 this software. If not, see . - */ - -#pragma once - -#define TARGET_CONFIG - -#define TARGET_BOARD_IDENTIFIER "NOX1" -#define USBD_PRODUCT_STRING "NoxF4V1" - -#define LED0_PIN PA4 - -#define BEEPER PC13 -#define BEEPER_INVERTED - -#define INVERTER_PIN_UART2 PC14 - -#define ACC -#define USE_ACC_SPI_MPU6500 -#define USE_ACC_SPI_MPU6000 - -#define GYRO -#define USE_GYRO_SPI_MPU6500 -#define USE_GYRO_SPI_MPU6000 - -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 - -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 - -#define USE_EXTI -#define MPU_INT_EXTI PA8 -#define USE_MPU_DATA_READY_SIGNAL - -#define BARO -#define USE_BARO_BMP280 -#define USE_BARO_SPI_BMP280 -#define BMP280_SPI_INSTANCE SPI2 -#define BMP280_CS_PIN PA9 - -#define OSD -#define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI2 -#define MAX7456_SPI_CS_PIN PA10 -#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) -#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) - -#define USE_VCP -//#define VBUS_SENSING_PIN PC5 - -#define USE_UART1 -#define UART1_RX_PIN PB7 -#define UART1_TX_PIN PB6 - -#define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 - -#define USE_SOFTSERIAL1 -#define SOFTSERIAL1_RX_PIN NONE -#define SOFTSERIAL1_TX_PIN NONE - -#define USE_SOFTSERIAL2 -#define SOFTSERIAL2_RX_PIN PA2 // Backdoor timer on UART2_TX -#define SOFTSERIAL2_TX_PIN NONE - -#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART2, SOFTSERIAL1, SOFTSERIAL2 - -#define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PPM - -#define USE_SPI -#define USE_SPI_DEVICE_1 -#define SPI1_SCK_PIN PB3 -#define SPI1_MISO_PIN PB4 -#define SPI1_MOSI_PIN PB5 - -#define USE_SPI_DEVICE_2 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define USE_ADC -#define CURRENT_METER_ADC_PIN NONE // PA6 Available from TP33 -#define VBAT_ADC_PIN PA5 // 11:1 (10K + 1K) divider - -#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ESC -#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ESC - -#define SERIALRX_UART SERIAL_PORT_USART2 - -#define TRANSPONDER - -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS - -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_SOFTSERIAL | FEATURE_ESC_SENSOR) -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) -#define TARGET_IO_PORTB (0xffff & ~(BIT(2)|BIT(11))) -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) - -#define USABLE_TIMER_CHANNEL_COUNT 9 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(9) ) diff --git a/src/main/target/NOX/target.mk b/src/main/target/NOX/target.mk deleted file mode 100644 index ce5b31004c..0000000000 --- a/src/main/target/NOX/target.mk +++ /dev/null @@ -1,7 +0,0 @@ -F411_TARGETS += $(TARGET) -FEATURES += VCP -TARGET_SRC = \ - drivers/accgyro/accgyro_mpu6500.c \ - drivers/accgyro/accgyro_spi_mpu6500.c \ - drivers/barometer/barometer_bmp280.c \ - drivers/max7456.c From d4d342ef4a53c4e6f456ee96802a59af07dee45f Mon Sep 17 00:00:00 2001 From: MJ666 Date: Mon, 20 Nov 2017 20:09:46 +0100 Subject: [PATCH 31/32] Fix AlienFlightNG F7 boot issue --- src/main/drivers/bus_spi_ll.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c index 8bb1ea0477..6e6e5b2993 100644 --- a/src/main/drivers/bus_spi_ll.c +++ b/src/main/drivers/bus_spi_ll.c @@ -365,6 +365,7 @@ uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg) void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) { + bus->bustype = BUSTYPE_SPI; bus->busdev_u.spi.instance = instance; } From fd7663ed07bfdf3f17f40fadf14eb965ca5ec6ab Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 23 Nov 2017 11:17:22 +0900 Subject: [PATCH 32/32] White space tidy --- src/main/drivers/bus.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 444f4b54d1..43681f7a16 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -42,11 +42,11 @@ typedef struct busDevice_s { struct deviceI2C_s { I2CDevice device; uint8_t address; - } i2c; - struct deviceMpuSlave_s { + } i2c; + struct deviceMpuSlave_s { const struct busDevice_s *master; uint8_t address; - } mpuSlave; + } mpuSlave; } busdev_u; } busDevice_t;