From de21b5ae3e422e31bbe075cec21087e3671ac72e Mon Sep 17 00:00:00 2001 From: IVData Date: Thu, 3 Sep 2020 18:38:10 +1200 Subject: [PATCH 001/143] Added option to output servos on PWM and SBUS --- src/main/drivers/pwm_mapping.c | 3 ++- src/main/drivers/pwm_mapping.h | 3 ++- src/main/drivers/pwm_output.c | 11 +++++++++++ src/main/fc/config.c | 2 +- src/main/fc/fc_tasks.c | 2 +- src/main/fc/settings.yaml | 2 +- 6 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 8c6eaea463b..a7a27b4ede1 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -268,7 +268,8 @@ static bool motorsUseHardwareTimers(void) static bool servosUseHardwareTimers(void) { - return servoConfig()->servo_protocol == SERVO_TYPE_PWM; + return servoConfig()->servo_protocol == SERVO_TYPE_PWM || + servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM; } static void pwmInitMotors(timMotorServoHardware_t * timOutputs) diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 2c8b8f83a52..7efbf394318 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -52,7 +52,8 @@ typedef enum { typedef enum { SERVO_TYPE_PWM = 0, SERVO_TYPE_SERVO_DRIVER, - SERVO_TYPE_SBUS + SERVO_TYPE_SBUS, + SERVO_TYPE_SBUS_PWM } servoProtocolType_e; typedef enum { diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 6472741a78a..0f4e0caf183 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -501,6 +501,12 @@ static void pwmServoWriteStandard(uint8_t index, uint16_t value) } } +static void sbusPwmWriteStandard(uint8_t index, uint16_t value) +{ + pwmServoWriteStandard(index, value); + sbusServoUpdate(index, value); +} + #ifdef USE_PWM_SERVO_DRIVER static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) { @@ -532,6 +538,11 @@ void pwmServoPreconfigure(void) sbusServoInitialize(); servoWritePtr = sbusServoUpdate; break; + + case SERVO_TYPE_SBUS_PWM: + sbusServoInitialize(); + servoWritePtr = sbusPwmWriteStandard; + break; #endif } } diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 627fd350b5a..5fcf8a6d11f 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -217,7 +217,7 @@ void validateAndFixConfig(void) #endif #ifndef USE_SERVO_SBUS - if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) { + if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS || servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) { servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM; } #endif diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 190a299d9d6..4ff3e6b9b08 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -346,7 +346,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_STACK_CHECK, true); #endif #if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS) - setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS)); + setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM)); #endif #ifdef USE_CMS #ifdef USE_MSP_DISPLAYPORT diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 65ac689cdda..d4246fc4711 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -34,7 +34,7 @@ tables: - name: motor_pwm_protocol values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"] - name: servo_protocol - values: ["PWM", "SERVO_DRIVER", "SBUS"] + values: ["PWM", "SERVO_DRIVER", "SBUS", "SBUS_PWM"] - name: failsafe_procedure values: ["SET-THR", "DROP", "RTH", "NONE"] - name: current_sensor From 64f3d70002e2890e0f8d8f3c066ff342bb6e3b90 Mon Sep 17 00:00:00 2001 From: IVData Date: Wed, 16 Sep 2020 12:30:17 +1200 Subject: [PATCH 002/143] Fix builds for targets without USE_SERVO_SBUS --- src/main/drivers/pwm_output.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 0f4e0caf183..ba147cc5623 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -501,11 +501,13 @@ static void pwmServoWriteStandard(uint8_t index, uint16_t value) } } +#ifdef USE_SERVO_SBUS static void sbusPwmWriteStandard(uint8_t index, uint16_t value) { pwmServoWriteStandard(index, value); sbusServoUpdate(index, value); } +#endif #ifdef USE_PWM_SERVO_DRIVER static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) From baa834fcee49598ac74158ddad85ee54e2dab02c Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sun, 4 Oct 2020 18:50:03 +0200 Subject: [PATCH 003/143] Add driver for BMI088 IMU --- src/main/CMakeLists.txt | 2 + src/main/drivers/accgyro/accgyro_bmi088.c | 245 ++++++++++++++++++++++ src/main/drivers/accgyro/accgyro_bmi088.h | 30 +++ src/main/drivers/bus.h | 2 + src/main/fc/settings.yaml | 2 +- src/main/sensors/acceleration.c | 14 ++ src/main/sensors/acceleration.h | 3 +- src/main/sensors/gyro.c | 10 + src/main/sensors/gyro.h | 1 + src/main/target/common_hardware.c | 9 + 10 files changed, 316 insertions(+), 2 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_bmi088.c create mode 100644 src/main/drivers/accgyro/accgyro_bmi088.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 674b766d0ba..018f1604b81 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -71,6 +71,8 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_adxl345.h drivers/accgyro/accgyro_bma280.c drivers/accgyro/accgyro_bma280.h + drivers/accgyro/accgyro_bmi088.c + drivers/accgyro/accgyro_bmi088.h drivers/accgyro/accgyro_bmi160.c drivers/accgyro/accgyro_bmi160.h drivers/accgyro/accgyro_fake.c diff --git a/src/main/drivers/accgyro/accgyro_bmi088.c b/src/main/drivers/accgyro/accgyro_bmi088.c new file mode 100644 index 00000000000..abe89af1df2 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bmi088.c @@ -0,0 +1,245 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include "platform.h" +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/system.h" +#include "drivers/time.h" +#include "drivers/io.h" +#include "drivers/exti.h" +#include "drivers/bus.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_bmi088.h" + + +#if defined(USE_IMU_BMI088) + +/* + device registers, names follow datasheet conventions, with REGA_ + prefix for accel, and REGG_ prefix for gyro + */ +#define REGA_CHIPID 0x00 +#define REGA_ERR_REG 0x02 +#define REGA_STATUS 0x03 +#define REGA_X_LSB 0x12 +#define REGA_INT_STATUS_1 0x1D +#define REGA_TEMP_LSB 0x22 +#define REGA_TEMP_MSB 0x23 +#define REGA_CONF 0x40 +#define REGA_RANGE 0x41 +#define REGA_PWR_CONF 0x7C +#define REGA_PWR_CTRL 0x7D +#define REGA_SOFTRESET 0x7E +#define REGA_FIFO_CONFIG0 0x48 +#define REGA_FIFO_CONFIG1 0x49 +#define REGA_FIFO_DOWNS 0x45 +#define REGA_FIFO_DATA 0x26 +#define REGA_FIFO_LEN0 0x24 +#define REGA_FIFO_LEN1 0x25 + +#define REGG_CHIPID 0x00 +#define REGG_RATE_X_LSB 0x02 +#define REGG_INT_CTRL 0x15 +#define REGG_INT_STATUS_1 0x0A +#define REGG_INT_STATUS_2 0x0B +#define REGG_INT_STATUS_3 0x0C +#define REGG_FIFO_STATUS 0x0E +#define REGG_RANGE 0x0F +#define REGG_BW 0x10 +#define REGG_LPM1 0x11 +#define REGG_RATE_HBW 0x13 +#define REGG_BGW_SOFTRESET 0x14 +#define REGG_FIFO_CONFIG_1 0x3E +#define REGG_FIFO_DATA 0x3F + + +static void bmi088GyroInit(gyroDev_t *gyro) +{ + gyroIntExtiInit(gyro); + + busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION); + + // Soft reset + busWrite(gyro->busDev, REGG_BGW_SOFTRESET, 0xB6); + delay(100); + + // ODR 2kHz, BW 532Hz + busWrite(gyro->busDev, REGG_BW, 0x81); + delay(1); + + // Enable sampling + busWrite(gyro->busDev, REGG_INT_CTRL, 0x80); + delay(1); + + busSetSpeed(gyro->busDev, BUS_SPEED_FAST); +} + +static void bmi088AccInit(accDev_t *acc) +{ + busSetSpeed(acc->busDev, BUS_SPEED_INITIALIZATION); + + // Soft reset + busWrite(acc->busDev, REGA_SOFTRESET, 0xB6); + delay(100); + + // Active mode + busWrite(acc->busDev, REGA_PWR_CONF, 0); + delay(100); + + // ACC ON + busWrite(acc->busDev, REGA_PWR_CTRL, 0x04); + delay(100); + + // OSR4, ODR 1600Hz + busWrite(acc->busDev, REGA_CONF, 0x8C); + delay(1); + + // Range 12g + busWrite(acc->busDev, REGA_RANGE, 0x02); + delay(1); + + busSetSpeed(acc->busDev, BUS_SPEED_STANDARD); + + acc->acc_1G = 2048; +} + +static bool bmi088GyroRead(gyroDev_t *gyro) +{ + uint8_t gyroRaw[6]; + + if (busReadBuf(gyro->busDev, REGG_RATE_X_LSB, gyroRaw, 6)) { + gyro->gyroADCRaw[X] = (int16_t)((gyroRaw[1] << 8) | gyroRaw[0]); + gyro->gyroADCRaw[Y] = (int16_t)((gyroRaw[3] << 8) | gyroRaw[2]); + gyro->gyroADCRaw[Z] = (int16_t)((gyroRaw[5] << 8) | gyroRaw[4]); + return true; + } + + return false; +} + +static bool bmi088AccRead(accDev_t *acc) +{ + uint8_t buffer[7]; + + if (busReadBuf(acc->busDev, REGA_STATUS, buffer, 2) && (buffer[1] & 0x80) && busReadBuf(acc->busDev, REGA_X_LSB, buffer, 7)) { + // first byte is discarded, see datasheet + acc->ADCRaw[X] = (int32_t)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4); + acc->ADCRaw[Y] = (int32_t)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4); + acc->ADCRaw[Z] = (int32_t)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4); + return true; + } + + return false; +} + +static bool gyroDeviceDetect(busDevice_t * busDev) +{ + uint8_t attempts; + + busSetSpeed(busDev, BUS_SPEED_INITIALIZATION); + + for (attempts = 0; attempts < 5; attempts++) { + uint8_t chipId; + + delay(100); + busRead(busDev, REGG_CHIPID, &chipId); + + if (chipId == 0x0F) { + return true; + } + } + + return false; +} + +static bool accDeviceDetect(busDevice_t * busDev) +{ + uint8_t attempts; + + busSetSpeed(busDev, BUS_SPEED_INITIALIZATION); + + for (attempts = 0; attempts < 5; attempts++) { + uint8_t chipId; + + delay(100); + busRead(busDev, REGA_CHIPID, &chipId); + + if (chipId == 0x1E) { + return true; + } + } + + return false; +} + +bool bmi088AccDetect(accDev_t *acc) +{ + acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_ACC, acc->imuSensorToUse, OWNER_MPU); + if (acc->busDev == NULL) { + return false; + } + + if (!accDeviceDetect(acc->busDev)) { + busDeviceDeInit(acc->busDev); + return false; + } + + acc->initFn = bmi088AccInit; + acc->readFn = bmi088AccRead; + + return true; +} + +bool bmi088GyroDetect(gyroDev_t *gyro) +{ + gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_GYRO, gyro->imuSensorToUse, OWNER_MPU); + if (gyro->busDev == NULL) { + return false; + } + + if (!gyroDeviceDetect(gyro->busDev)) { + busDeviceDeInit(gyro->busDev); + return false; + } + + gyro->initFn = bmi088GyroInit; + gyro->readFn = bmi088GyroRead; + gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb + gyro->intStatusFn = gyroCheckDataReady; + gyro->gyroAlign = gyro->busDev->param; + + return true; +} + +#endif /* USE_IMU_BMI088 */ diff --git a/src/main/drivers/accgyro/accgyro_bmi088.h b/src/main/drivers/accgyro/accgyro_bmi088.h new file mode 100644 index 00000000000..4ffa4acc3d1 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bmi088.h @@ -0,0 +1,30 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "drivers/sensor.h" + +bool bmi088AccDetect(accDev_t *acc); +bool bmi088GyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 6e7dc2f48b2..725de13fb2d 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -91,6 +91,8 @@ typedef enum { DEVHW_MPU6050, DEVHW_MPU6500, DEVHW_BMI160, + DEVHW_BMI088_GYRO, + DEVHW_BMI088_ACC, DEVHW_ICM20689, /* Combined ACC/GYRO/MAG chips */ diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 662ed10c2e8..68c14dc054f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4,7 +4,7 @@ tables: - name: gyro_lpf values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"] - name: acc_hardware - values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"] + values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"] diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 9ff00c45297..241f1c417a0 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -50,6 +50,7 @@ FILE_COMPILE_FOR_SPEED #include "drivers/accgyro/accgyro_adxl345.h" #include "drivers/accgyro/accgyro_mma845x.h" #include "drivers/accgyro/accgyro_bma280.h" +#include "drivers/accgyro/accgyro_bmi088.h" #include "drivers/accgyro/accgyro_bmi160.h" #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_fake.h" @@ -240,6 +241,19 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) FALLTHROUGH; #endif +#if defined(USE_IMU_BMI088) + case ACC_BMI088: + if (bmi088AccDetect(dev)) { + accHardware = ACC_BMI088; + break; + } + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (accHardwareToUse != ACC_AUTODETECT) { + break; + } + FALLTHROUGH; +#endif + #ifdef USE_IMU_ICM20689 case ACC_ICM20689: if (icm20689AccDetect(dev)) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 47f669b83a5..a650b4be959 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -45,7 +45,8 @@ typedef enum { ACC_MPU9250 = 9, ACC_BMI160 = 10, ACC_ICM20689 = 11, - ACC_FAKE = 12, + ACC_BMI088 = 12, + ACC_FAKE = 13, ACC_MAX = ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d9cd15891d9..1ce3c169ba8 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -52,6 +52,7 @@ FILE_COMPILE_FOR_SPEED #include "drivers/accgyro/accgyro_adxl345.h" #include "drivers/accgyro/accgyro_mma845x.h" #include "drivers/accgyro/accgyro_bma280.h" +#include "drivers/accgyro/accgyro_bmi088.h" #include "drivers/accgyro/accgyro_bmi160.h" #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_fake.h" @@ -202,6 +203,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard FALLTHROUGH; #endif +#ifdef USE_IMU_BMI088 + case GYRO_BMI088: + if (bmi088GyroDetect(dev)) { + gyroHardware = GYRO_BMI088; + break; + } + FALLTHROUGH; +#endif + #ifdef USE_IMU_ICM20689 case GYRO_ICM20689: if (icm20689GyroDetect(dev)) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b709294b553..d47a02f6f68 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -36,6 +36,7 @@ typedef enum { GYRO_MPU9250, GYRO_BMI160, GYRO_ICM20689, + GYRO_BMI088, GYRO_FAKE } gyroSensor_e; diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index ad8736d0db6..b445a131936 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -80,6 +80,15 @@ BUSDEV_REGISTER_I2C(busdev_bmi160, DEVHW_BMI160, BMI160_I2C_BUS, 0x68, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI160_ALIGN); #endif #endif + + #if defined(USE_IMU_BMI088) + #if defined(BMI088_SPI_BUS) + BUSDEV_REGISTER_SPI(busdev_bmi088_gyro, DEVHW_BMI088_GYRO, BMI088_SPI_BUS, BMI088_GYRO_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI088_ALIGN); + BUSDEV_REGISTER_SPI(busdev_bmi088_acc, DEVHW_BMI088_ACC, BMI088_SPI_BUS, BMI088_ACC_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI088_ALIGN); + #elif defined(BMI088_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_bmi088, DEVHW_BMI088, BMI088_I2C_BUS, 0x68, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI088_ALIGN); + #endif + #endif #endif From 94f14286d6a65a3094e629e254a6c4f941dc58d8 Mon Sep 17 00:00:00 2001 From: scavanger Date: Sat, 21 Nov 2020 20:52:04 +0100 Subject: [PATCH 004/143] Let only values blink. --- src/main/io/osd.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d7e035919c..a5c375b6138 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2419,21 +2419,21 @@ static bool osdDrawSingleElement(uint8_t item) char buff[4]; textAttributes_t attr; - displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA BP"); - + displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID); + tfp_sprintf(buff, "%3d", currentControlRateProfile->throttle.dynPID); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, attr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 5, elemPosY, buff, attr); + displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "BP %4d", currentControlRateProfile->throttle.pa_breakpoint); + tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.pa_breakpoint); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, attr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY + 1, buff, attr); return true; } From b190ffa6b5c009472229233e5da23b8a8810dd7c Mon Sep 17 00:00:00 2001 From: scavanger Date: Thu, 26 Nov 2020 19:12:32 +0100 Subject: [PATCH 005/143] Enable softserial1 RX on LED pin, disable LED_STRIP. --- src/main/target/MATEKF411SE/CMakeLists.txt | 1 + src/main/target/MATEKF411SE/target.c | 2 +- src/main/target/MATEKF411SE/target.h | 6 ++++++ 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index df67c6f00b7..3f2dd908bf5 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f411xe(MATEKF411SE) +target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) \ No newline at end of file diff --git a/src/main/target/MATEKF411SE/target.c b/src/main/target/MATEKF411SE/target.c index be71e58f2d3..879acd61318 100755 --- a/src/main/target/MATEKF411SE/target.c +++ b/src/main/target/MATEKF411SE/target.c @@ -35,7 +35,7 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad -softserial_tx2 DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM - DEF_TIM(TIM2, CH3, PB10, TIM_USE_LED, 0, 0), //LED 2812 D(1,1,3) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_ANY, 0, 0), // softserial_rx1 - LED 2812 D(1,1,3) }; diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index a91f0eb1777..7e322e8401a 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -70,7 +70,11 @@ #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_TX_PIN PB9 // ST1 pad +#ifdef MATEKF411SE_FD_SFTSRL1 +#define SOFTSERIAL_1_RX_PIN PB10 // LED pad +#else #define SOFTSERIAL_1_RX_PIN PB9 +#endif #define USE_SOFTSERIAL2 #define SOFTSERIAL_2_TX_PIN PA2 // TX2 pad @@ -131,8 +135,10 @@ #define AIRSPEED_ADC_CHANNEL ADC_CHN_4 // *************** LED2812 ************************ +#ifndef MATEKF411SE_FD_SFTSRL1 #define USE_LED_STRIP #define WS2811_PIN PB10 +#endif // *************** PINIO *************************** #define USE_PINIO From 565f41cf1c609130d87edbf51ca2b032d21866c6 Mon Sep 17 00:00:00 2001 From: scavanger Date: Thu, 26 Nov 2020 19:14:03 +0100 Subject: [PATCH 006/143] New line --- src/main/target/MATEKF411SE/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index 3f2dd908bf5..fa289c23ffa 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1,2 +1,2 @@ target_stm32f411xe(MATEKF411SE) -target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) \ No newline at end of file +target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) From 7516617ec004bb9dafacadaba27e7d8c5bed65cf Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 1 Dec 2020 21:31:02 +0100 Subject: [PATCH 007/143] [MATEKF405] Add the possibility of MC servo on S7 --- src/main/target/MATEKF405/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 1b442177567..d0543ad77e8 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -40,7 +40,7 @@ const timerHardware_t timerHardware[] = { #else DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) #endif - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S7 D(1,7)!S5 UP(2,6) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 D(1,7)!S5 UP(2,6) DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 From d6e3677455830185c263d19e24c340dafa3007f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Thu, 24 Sep 2020 19:40:11 +0200 Subject: [PATCH 008/143] MATEKF411SE_PINIO target LED pad repurposed as 2nd PINIO (USER2) --- src/main/target/MATEKF411SE/CMakeLists.txt | 1 + src/main/target/MATEKF411SE/config.c | 3 +++ src/main/target/MATEKF411SE/target.c | 2 ++ src/main/target/MATEKF411SE/target.h | 5 +++++ 4 files changed, 11 insertions(+) diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index df67c6f00b7..919b98fc17f 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f411xe(MATEKF411SE) +target_stm32f411xe(MATEKF411SE_PINIO) diff --git a/src/main/target/MATEKF411SE/config.c b/src/main/target/MATEKF411SE/config.c index 07f6de46971..38bf22dd8e0 100755 --- a/src/main/target/MATEKF411SE/config.c +++ b/src/main/target/MATEKF411SE/config.c @@ -25,4 +25,7 @@ void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +#ifdef MATEKF411SE_PINIO + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +#endif } diff --git a/src/main/target/MATEKF411SE/target.c b/src/main/target/MATEKF411SE/target.c index be71e58f2d3..72392c9fa94 100755 --- a/src/main/target/MATEKF411SE/target.c +++ b/src/main/target/MATEKF411SE/target.c @@ -35,7 +35,9 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad -softserial_tx2 DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM +#ifndef MATEKF411SE_PINIO DEF_TIM(TIM2, CH3, PB10, TIM_USE_LED, 0, 0), //LED 2812 D(1,1,3) +#endif }; diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index a91f0eb1777..46f5c05c992 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -131,13 +131,18 @@ #define AIRSPEED_ADC_CHANNEL ADC_CHN_4 // *************** LED2812 ************************ +#ifndef MATEKF411SE_PINIO #define USE_LED_STRIP #define WS2811_PIN PB10 +#endif // *************** PINIO *************************** #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PA13 // Camera switcher +#ifdef MATEKF411SE_PINIO +#define PINIO2_PIN PB10 // External PINIO (LED pad) +#endif // *************** OTHERS ************************* #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL ) From 40ed910bfe3c1c378de03a859414449c066ff751 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 7 Dec 2020 13:06:34 +0100 Subject: [PATCH 009/143] First try --- src/main/flight/pid.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 00cd8ac9c5c..b4755491718 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -611,7 +611,15 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float newFFTerm = pidState->rateTarget * pidState->kFF; // Calculate integral - pidState->errorGyroIf += rateError * pidState->kI * dT; + // If bank angle is more than 20 degrees do not update yaw and pitch I-term + float bankAngle = attitude.values.roll; + if (bankAngle > 100 && axis == FD_YAW) { + pidState->errorGyroIf += 0; + } else + { + pidState->errorGyroIf += rateError * pidState->kI * dT; + } + applyItermLimiting(pidState); From dc0b9be9459d583793ccfd01e90ba24c6b870bd7 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 7 Dec 2020 14:13:06 +0100 Subject: [PATCH 010/143] Create CLI command. --- src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 10 +++++----- src/main/flight/pid.h | 1 + 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4316e29f7ad..a66b07bab3c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1569,6 +1569,12 @@ groups: field: fixedWingItermLimitOnStickPosition min: 0 max: 1 + - name: fw_iterm_limit_bank_angle + description: "Iterm is not allowed to grow when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 180 (the default) disables this feature." + default_value: "180" + field: fixedWingItermBankLimit + min: 0 + max: 180 - name: pidsum_limit description: "A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help" default_value: "500" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b4755491718..8d9f4b17c99 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -258,6 +258,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, + .fixedWingItermBankLimit = 180, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, @@ -611,15 +612,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float newFFTerm = pidState->rateTarget * pidState->kFF; // Calculate integral - // If bank angle is more than 20 degrees do not update yaw and pitch I-term - float bankAngle = attitude.values.roll; - if (bankAngle > 100 && axis == FD_YAW) { + // Freeze yaw Iterm when bank angle is above threshold to avoid rudder counteracting turns + float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); + if (fabsf(bankAngle) > pidProfile()->fixedWingItermBankLimit && axis == FD_YAW) { pidState->errorGyroIf += 0; } else { pidState->errorGyroIf += rateError * pidState->kI * dT; } - applyItermLimiting(pidState); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cc4b20ae599..aa7670f5ebf 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -128,6 +128,7 @@ typedef struct pidProfile_s { float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point + uint16_t fixedWingItermBankLimit; // Freeze yaw Iterm when bank angle is more than this many degrees uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) float navVelXyDTermLpfHz; From 36124be148132c15204086d2e24634e14ada0a33 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 7 Dec 2020 14:17:12 +0100 Subject: [PATCH 011/143] Rename variables to make more clear that this only affects yaw stabilization. --- src/main/fc/settings.yaml | 6 +++--- src/main/flight/pid.c | 4 ++-- src/main/flight/pid.h | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a66b07bab3c..afa2506a01c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1569,10 +1569,10 @@ groups: field: fixedWingItermLimitOnStickPosition min: 0 max: 1 - - name: fw_iterm_limit_bank_angle - description: "Iterm is not allowed to grow when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 180 (the default) disables this feature." + - name: fw_yaw_iterm_limit_bank_angle + description: "Yaw Iterm is not allowed to grow when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 180 (the default) disables this feature." default_value: "180" - field: fixedWingItermBankLimit + field: fixedWingYawItermBankLimit min: 0 max: 180 - name: pidsum_limit diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8d9f4b17c99..d83e3cd09e5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -258,7 +258,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, - .fixedWingItermBankLimit = 180, + .fixedWingYawItermBankLimit = 180, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, @@ -614,7 +614,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh // Calculate integral // Freeze yaw Iterm when bank angle is above threshold to avoid rudder counteracting turns float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); - if (fabsf(bankAngle) > pidProfile()->fixedWingItermBankLimit && axis == FD_YAW) { + if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && axis == FD_YAW) { pidState->errorGyroIf += 0; } else { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index aa7670f5ebf..9f8090ef6c4 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -128,7 +128,7 @@ typedef struct pidProfile_s { float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point - uint16_t fixedWingItermBankLimit; // Freeze yaw Iterm when bank angle is more than this many degrees + uint16_t fixedWingYawItermBankLimit; // Freeze yaw Iterm when bank angle is more than this many degrees uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) float navVelXyDTermLpfHz; From 3c8deadb3e2b95ffa6cf7cfc3c222877f31e2dd9 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 7 Dec 2020 15:41:13 +0100 Subject: [PATCH 012/143] Don't freeze I-term when in AUTO TUNE (not sure if necessary, just to be safe). --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d83e3cd09e5..cc0bbc5f954 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -614,7 +614,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh // Calculate integral // Freeze yaw Iterm when bank angle is above threshold to avoid rudder counteracting turns float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); - if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && axis == FD_YAW) { + if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && axis == FD_YAW && !FLIGHT_MODE(AUTO_TUNE)) { pidState->errorGyroIf += 0; } else { From 51b2a49cb4a1aa2bf04595137ea65a59995f50aa Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 9 Dec 2020 09:55:28 +0100 Subject: [PATCH 013/143] Implement suggestions. Stops I-term from growing instead of freezing it. --- src/main/flight/pid.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cc0bbc5f954..ae6910dcf44 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -612,14 +612,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float newFFTerm = pidState->rateTarget * pidState->kFF; // Calculate integral - // Freeze yaw Iterm when bank angle is above threshold to avoid rudder counteracting turns - float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); - if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && axis == FD_YAW && !FLIGHT_MODE(AUTO_TUNE)) { - pidState->errorGyroIf += 0; - } else - { - pidState->errorGyroIf += rateError * pidState->kI * dT; - } + pidState->errorGyroIf += rateError * pidState->kI * dT; applyItermLimiting(pidState); @@ -928,11 +921,18 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); } -void checkItermLimitingActive(pidState_t *pidState) +void checkItermLimitingActive(pidState_t *pidState, flight_dynamics_index_t axis) { bool shouldActivate; + float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (usedPidControllerType == PID_TYPE_PIFF) { - shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); + // Do not allow yaw I-term to grow when bank angle is too large + if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit){ + shouldActivate = 1; + } else + { + shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); + } } else { shouldActivate = mixerIsOutputSaturated(); @@ -1010,7 +1010,7 @@ void FAST_CODE pidController(float dT) pidApplySetpointRateLimiting(&pidState[axis], axis, dT); // Step 4: Run gyro-driven control - checkItermLimitingActive(&pidState[axis]); + checkItermLimitingActive(&pidState[axis], axis); pidControllerApplyFn(&pidState[axis], axis, dT); } } From aac48f4868eb454aff2c31046d2d46b737a25526 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 9 Dec 2020 09:56:39 +0100 Subject: [PATCH 014/143] Again forgot AUTO TUNE --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ae6910dcf44..f908a03fe24 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -927,7 +927,7 @@ void checkItermLimitingActive(pidState_t *pidState, flight_dynamics_index_t axis float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (usedPidControllerType == PID_TYPE_PIFF) { // Do not allow yaw I-term to grow when bank angle is too large - if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit){ + if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !FLIGHT_MODE(AUTO_TUNE)){ shouldActivate = 1; } else { From 53f9c9f5ee2900fb48cb1cade1b0ef010f597ab8 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 9 Dec 2020 11:26:45 +0100 Subject: [PATCH 015/143] Freezing I-term instead, using new functions. --- src/main/flight/pid.c | 45 ++++++++++++++++++++++++++++++++----------- 1 file changed, 34 insertions(+), 11 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f908a03fe24..5547273abe4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -81,6 +81,7 @@ typedef struct { // Rate integrator float errorGyroIf; float errorGyroIfLimit; + float errorGyroIfFrozen; // Used for ANGLE filtering (PT1, we don't need super-sharpness here) pt1Filter_t angleFilterState; @@ -103,6 +104,7 @@ typedef struct { uint16_t pidSumLimit; filterApply4FnPtr ptermFilterApplyFn; bool itermLimitActive; + bool itermFreezeActive; biquadFilter_t rateTargetFilter; } pidState_t; @@ -351,6 +353,7 @@ void pidResetErrorAccumulators(void) for (int axis = 0; axis < 3; axis++) { pidState[axis].errorGyroIf = 0.0f; pidState[axis].errorGyroIfLimit = 0.0f; + pidState[axis].errorGyroIfFrozen = 0.0f; } } @@ -594,11 +597,21 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) { static void applyItermLimiting(pidState_t *pidState) { if (pidState->itermLimitActive) { pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); - } else { + } else + { pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); } } +static void applyItermFreezing(pidState_t *pidState) { + if (pidState->itermFreezeActive) { + pidState->errorGyroIf = pidState->errorGyroIfFrozen; + } else + { + pidState->errorGyroIfFrozen = pidState->errorGyroIf; + } +} + static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { UNUSED(pidState); UNUSED(axis); @@ -615,6 +628,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf += rateError * pidState->kI * dT; applyItermLimiting(pidState); + applyItermFreezing(pidState); if (pidProfile()->fixedWingItermThrowLimit != 0) { pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); @@ -921,18 +935,11 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); } -void checkItermLimitingActive(pidState_t *pidState, flight_dynamics_index_t axis) +void checkItermLimitingActive(pidState_t *pidState) { bool shouldActivate; - float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (usedPidControllerType == PID_TYPE_PIFF) { - // Do not allow yaw I-term to grow when bank angle is too large - if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !FLIGHT_MODE(AUTO_TUNE)){ - shouldActivate = 1; - } else - { - shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); - } + shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { shouldActivate = mixerIsOutputSaturated(); @@ -941,6 +948,20 @@ void checkItermLimitingActive(pidState_t *pidState, flight_dynamics_index_t axis pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; } +void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) +{ + if (usedPidControllerType == PID_TYPE_PIFF) { + // Do not allow yaw I-term to grow when bank angle is too large + float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); + if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !FLIGHT_MODE(AUTO_TUNE)){ + pidState->itermFreezeActive = true; + } else + { + pidState->itermFreezeActive = false; + } + } +} + void FAST_CODE pidController(float dT) { if (!pidFiltersConfigured) { @@ -1010,7 +1031,9 @@ void FAST_CODE pidController(float dT) pidApplySetpointRateLimiting(&pidState[axis], axis, dT); // Step 4: Run gyro-driven control - checkItermLimitingActive(&pidState[axis], axis); + checkItermLimitingActive(&pidState[axis]); + checkItermFreezingActive(&pidState[axis], axis); + pidControllerApplyFn(&pidState[axis], axis, dT); } } From 4b9d388a72a27afe59593cbe09cdb50fcfdebab7 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 9 Dec 2020 17:39:07 +0100 Subject: [PATCH 016/143] Disable in all cases where TURN ASSIST is on --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5547273abe4..2bb4b1e9a3d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -953,7 +953,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis if (usedPidControllerType == PID_TYPE_PIFF) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); - if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !FLIGHT_MODE(AUTO_TUNE)){ + if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ pidState->itermFreezeActive = true; } else { From 7c5b15e6215420ebd0f730debb6cab2784100692 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 9 Dec 2020 23:09:24 +0100 Subject: [PATCH 017/143] Changed settings and fixed possible issue on multirotors. --- src/main/fc/settings.yaml | 6 +++--- src/main/flight/pid.c | 6 +++++- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index afa2506a01c..3557cdd886e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1570,11 +1570,11 @@ groups: min: 0 max: 1 - name: fw_yaw_iterm_limit_bank_angle - description: "Yaw Iterm is not allowed to grow when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 180 (the default) disables this feature." - default_value: "180" + description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled." + default_value: "0" field: fixedWingYawItermBankLimit min: 0 - max: 180 + max: 90 - name: pidsum_limit description: "A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help" default_value: "500" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2bb4b1e9a3d..e69c99b3cb0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -950,7 +950,7 @@ void checkItermLimitingActive(pidState_t *pidState) void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { - if (usedPidControllerType == PID_TYPE_PIFF) { + if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankLimit != 0) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ @@ -959,7 +959,11 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis { pidState->itermFreezeActive = false; } + } else + { + pidState->itermFreezeActive = false; } + } void FAST_CODE pidController(float dT) From 8e45887bc41396837792feb08ee09972648cfa55 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 10 Dec 2020 09:56:23 +0100 Subject: [PATCH 018/143] Move yaw axis check to outer if-statement --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e69c99b3cb0..058988ea763 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -950,10 +950,10 @@ void checkItermLimitingActive(pidState_t *pidState) void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { - if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankLimit != 0) { + if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankLimit != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); - if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ + if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ pidState->itermFreezeActive = true; } else { From ca9600a8a12cdd153603b159532560c919c2ff22 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 10 Dec 2020 10:11:01 +0100 Subject: [PATCH 019/143] Fix default value issue --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 058988ea763..21190bb74c0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -260,7 +260,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, - .fixedWingYawItermBankLimit = 180, + .fixedWingYawItermBankLimit = 0, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, From 51c0b95cbff5201480ffa1b0f1f9815ba99424d6 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 10:40:29 +0100 Subject: [PATCH 020/143] Make TURN ASSIST axis aware, use target bank angle, and disable in ACRO mode --- src/main/flight/pid.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 00cd8ac9c5c..e9a3c3859d4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -843,13 +843,13 @@ float pidHeadingHold(float dT) * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more * and keeping ROLL and PITCH attitude though the turn. */ -static void NOINLINE pidTurnAssistant(pidState_t *pidState) +static void NOINLINE pidTurnAssistant(pidState_t *pidState, flight_dynamics_index_t axis) { fpVector3_t targetRates; targetRates.x = 0.0f; targetRates.y = 0.0f; - if (STATE(AIRPLANE)) { + if (STATE(AIRPLANE) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { if (calculateCosTiltAngle() >= 0.173648f) { // Ideal banked turn follow the equations: // forward_vel^2 / radius = Gravity * tan(roll_angle) @@ -871,8 +871,8 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn; + float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[axis]); + float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); } @@ -986,7 +986,9 @@ void FAST_CODE pidController(float dT) } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - pidTurnAssistant(pidState); + for (int axis = 0; axis < 3; axis++) { + pidTurnAssistant(pidState, axis); + } canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 7ec6afd1f3f9d4a385af8126252093f4ef7c5f1e Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 10:54:01 +0100 Subject: [PATCH 021/143] Change target bank angle code --- src/main/flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e9a3c3859d4..40eebf7a8e1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -843,7 +843,7 @@ float pidHeadingHold(float dT) * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more * and keeping ROLL and PITCH attitude though the turn. */ -static void NOINLINE pidTurnAssistant(pidState_t *pidState, flight_dynamics_index_t axis) +static void NOINLINE pidTurnAssistant(pidState_t *pidState) { fpVector3_t targetRates; targetRates.x = 0.0f; @@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, flight_dynamics_inde airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[axis]); + float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[ROLL]); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); @@ -986,9 +986,9 @@ void FAST_CODE pidController(float dT) } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - for (int axis = 0; axis < 3; axis++) { - pidTurnAssistant(pidState, axis); - } + //for (int axis = 0; axis < 3; axis++) { + pidTurnAssistant(pidState); + //} canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 232d526ab12549e8d1553505aba8af3c5cd4c952 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 11:31:46 +0100 Subject: [PATCH 022/143] no message --- src/main/flight/pid.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 40eebf7a8e1..f3909660230 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[ROLL]); + float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); @@ -986,9 +986,7 @@ void FAST_CODE pidController(float dT) } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - //for (int axis = 0; axis < 3; axis++) { - pidTurnAssistant(pidState); - //} + pidTurnAssistant(pidState); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 214f939a1f06e51cb402fde9eacfaccf81da1a9a Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 14:21:45 +0100 Subject: [PATCH 023/143] Attempted fix (does not work) --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f3909660230..a21ae688d36 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); + float bankAngleTarget = pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); From cf2e35908adbfbe4a6bbc726c896c86f68c1f83b Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 21:42:19 +0100 Subject: [PATCH 024/143] Convert bank angle to radians --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a21ae688d36..0623c826b60 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float bankAngleTarget = pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); + float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); From 95ab3fe22b1ad336e6898e2d9a5bc2339d467acf Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 13 Dec 2020 11:02:42 +0100 Subject: [PATCH 025/143] Take target bank angle calculation out of pidTurnAssistant --- src/main/flight/pid.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0623c826b60..000e33c9faa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -843,7 +843,7 @@ float pidHeadingHold(float dT) * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more * and keeping ROLL and PITCH attitude though the turn. */ -static void NOINLINE pidTurnAssistant(pidState_t *pidState) +static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget) { fpVector3_t targetRates; targetRates.x = 0.0f; @@ -871,7 +871,6 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); @@ -986,7 +985,8 @@ void FAST_CODE pidController(float dT) } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - pidTurnAssistant(pidState); + float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); + pidTurnAssistant(pidState, bankAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 2270a8c3df9a926e74378de05b962cdbc7283537 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 13 Dec 2020 11:15:09 +0100 Subject: [PATCH 026/143] Modes check outside of pidTurnAssistant --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 000e33c9faa..8d94c3b4fcd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -849,7 +849,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge targetRates.x = 0.0f; targetRates.y = 0.0f; - if (STATE(AIRPLANE) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { + if (STATE(AIRPLANE)) { if (calculateCosTiltAngle() >= 0.173648f) { // Ideal banked turn follow the equations: // forward_vel^2 / radius = Gravity * tan(roll_angle) @@ -984,7 +984,7 @@ void FAST_CODE pidController(float dT) levelingEnabled = false; } - if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { + if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); pidTurnAssistant(pidState, bankAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT From a1f42fcfe2cc287a8ad37a04fc89d5542fadb0e5 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 14 Dec 2020 15:24:50 +0100 Subject: [PATCH 027/143] Take pitch angle into account for turn rate calculation (technically correct but doesn't make huge difference) --- src/main/flight/pid.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8d94c3b4fcd..e733bc43bfc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -843,7 +843,7 @@ float pidHeadingHold(float dT) * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more * and keeping ROLL and PITCH attitude though the turn. */ -static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget) +static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget, float pitchAngleTarget) { fpVector3_t targetRates; targetRates.x = 0.0f; @@ -871,7 +871,8 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; + float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); + float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); } @@ -986,7 +987,8 @@ void FAST_CODE pidController(float dT) if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); - pidTurnAssistant(pidState, bankAngleTarget); + float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH])); + pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 37f2954ad1c33902e6c4a9362d5538e4646d038a Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 15 Dec 2020 14:41:01 +0100 Subject: [PATCH 028/143] Constain bank angle for calculation to 60 degrees. --- src/main/flight/pid.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e733bc43bfc..1f27af2b6f5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -871,6 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge + bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60)); float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; From 750d271ddff50a2f71f27b26d11dda2c100bd8ea Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 29 Dec 2020 02:21:33 +0200 Subject: [PATCH 029/143] added vtx_smartaudio_early_akk_workaround option --- src/main/fc/settings.yaml | 5 +++++ src/main/io/vtx_control.h | 1 + src/main/io/vtx_smartaudio.c | 3 ++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6ac12bbb41d..c7d766440b9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3075,6 +3075,11 @@ groups: default_value: "ON" field: halfDuplex type: bool + - name: vtx_smartaudio_early_akk_workaround + description: "Enable workaround for early AKK SAudio-enabled VTX bug." + default_value: "ON" + field: smartAudioEarlyAkkWorkaroundEnable + type: bool - name: PG_VTX_SETTINGS_CONFIG type: vtxSettingsConfig_t diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index e39e447a9f0..1e0d4c96f13 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -31,6 +31,7 @@ typedef struct vtxChannelActivationCondition_s { typedef struct vtxConfig_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; uint8_t halfDuplex; + uint8_t smartAudioEarlyAkkWorkaroundEnable; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index cb60d447adc..cb057f0fc5e 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -506,7 +506,8 @@ static void saSendFrame(uint8_t *buf, int len) // XXX: Workaround for early AKK SAudio-enabled VTX bug, // shouldn't cause any problems with VTX with properly // implemented SAudio. - serialWrite(smartAudioSerialPort, 0x00); + //Update: causes problem with new AKK AIO camera connected to SoftUART + if (vtxConfig()->smartAudioEarlyAkkWorkaroundEnable) serialWrite(smartAudioSerialPort, 0x00); sa_lastTransmissionMs = millis(); saStat.pktsent++; From 71a4e32bf1d9cd2471a48a1a9581174a451329e5 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 29 Dec 2020 02:49:21 +0200 Subject: [PATCH 030/143] fixed: vtxSAIsReady() returns true before vtx is initialized fixed: vtxSAGetPowerIndex() is trying to decode power index from mW --- src/main/io/vtx_smartaudio.c | 6 ++++-- src/main/io/vtx_smartaudio.h | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index cb057f0fc5e..2f12f3b58bc 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -195,6 +195,7 @@ static void saPrintSettings(void) LOG_D(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no"); } +/* int saDacToPowerIndex(int dac) { for (int idx = saPowerCount - 1 ; idx >= 0 ; idx--) { @@ -204,6 +205,7 @@ int saDacToPowerIndex(int dac) } return 0; } +*/ int saDbiToMw(uint16_t dbi) { @@ -794,7 +796,7 @@ vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) static bool vtxSAIsReady(const vtxDevice_t *vtxDevice) { - return vtxDevice != NULL && !(saDevice.power == 0); + return vtxDevice != NULL && (saDevice.power >= 0 ); //wait until power reading exists } @@ -912,7 +914,7 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) return false; } - *pIndex = ((saDevice.version == SA_1_0) ? saDacToPowerIndex(saDevice.power) : saDevice.power); + *pIndex = saDevice.power; return true; } diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index c817f05a79f..914995f1f22 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -72,7 +72,7 @@ typedef enum { typedef struct smartAudioDevice_s { smartAudioVersion_e version; int8_t channel; - int8_t power; + int8_t power; // -1 - not initialized, 0 - power level unknown, or power level index 1..n. For SA_2_1 device you can get power in mW: saPowerTable[index-1].mW int8_t mode; uint16_t freq; uint16_t orfreq; From 895a7625e81bd5e8e51a80f77b752163713cfac3 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 29 Dec 2020 03:00:31 +0200 Subject: [PATCH 031/143] Revert "fixed: vtxSAIsReady() returns true before vtx is initialized fixed: vtxSAGetPowerIndex() is trying to decode power index from mW" This reverts commit 71a4e32bf1d9cd2471a48a1a9581174a451329e5. --- src/main/io/vtx_smartaudio.c | 6 ++---- src/main/io/vtx_smartaudio.h | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 2f12f3b58bc..cb057f0fc5e 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -195,7 +195,6 @@ static void saPrintSettings(void) LOG_D(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no"); } -/* int saDacToPowerIndex(int dac) { for (int idx = saPowerCount - 1 ; idx >= 0 ; idx--) { @@ -205,7 +204,6 @@ int saDacToPowerIndex(int dac) } return 0; } -*/ int saDbiToMw(uint16_t dbi) { @@ -796,7 +794,7 @@ vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) static bool vtxSAIsReady(const vtxDevice_t *vtxDevice) { - return vtxDevice != NULL && (saDevice.power >= 0 ); + return vtxDevice != NULL && !(saDevice.power == 0); //wait until power reading exists } @@ -914,7 +912,7 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) return false; } - *pIndex = saDevice.power; + *pIndex = ((saDevice.version == SA_1_0) ? saDacToPowerIndex(saDevice.power) : saDevice.power); return true; } diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 914995f1f22..c817f05a79f 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -72,7 +72,7 @@ typedef enum { typedef struct smartAudioDevice_s { smartAudioVersion_e version; int8_t channel; - int8_t power; // -1 - not initialized, 0 - power level unknown, or power level index 1..n. For SA_2_1 device you can get power in mW: saPowerTable[index-1].mW + int8_t power; int8_t mode; uint16_t freq; uint16_t orfreq; From 1ff79a9f155ad4fdc378c93a842e492af16f9856 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 14:35:51 +0100 Subject: [PATCH 032/143] Change CLI and variable names to align with functionality --- src/main/fc/settings.yaml | 4 ++-- src/main/flight/pid.c | 6 +++--- src/main/flight/pid.h | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3557cdd886e..baf7e48a15c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1569,10 +1569,10 @@ groups: field: fixedWingItermLimitOnStickPosition min: 0 max: 1 - - name: fw_yaw_iterm_limit_bank_angle + - name: fw_yaw_iterm_freeze_bank_angle description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled." default_value: "0" - field: fixedWingYawItermBankLimit + field: fixedWingYawItermBankFreeze min: 0 max: 90 - name: pidsum_limit diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 21190bb74c0..493c2192f7c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -260,7 +260,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, - .fixedWingYawItermBankLimit = 0, + .fixedWingYawItermBankFreeze = 0, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, @@ -950,10 +950,10 @@ void checkItermLimitingActive(pidState_t *pidState) void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { - if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankLimit != 0 && axis == FD_YAW) { + if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); - if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ + if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ pidState->itermFreezeActive = true; } else { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9f8090ef6c4..7a9560c86ae 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -128,7 +128,7 @@ typedef struct pidProfile_s { float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point - uint16_t fixedWingYawItermBankLimit; // Freeze yaw Iterm when bank angle is more than this many degrees + uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) float navVelXyDTermLpfHz; From 5613059137e106f57c2fa180538e379b211cca05 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 16:11:07 +0100 Subject: [PATCH 033/143] Update docs --- docs/Settings.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Settings.md b/docs/Settings.md index 7c53b289d4b..e5481fbf84b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -130,6 +130,7 @@ | fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups | | fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | +| fw_yaw_iterm_freeze_bank_angle | 0 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. | | gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | | gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. | | gps_dyn_model | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. | From 9311c423bf5c989b1f41dde47310c0c2ee0c546e Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 10 Jan 2021 01:19:01 +0200 Subject: [PATCH 034/143] added vtx_smartaudio_early_akk_workaround options to settings.md --- docs/Settings.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Settings.md b/docs/Settings.md index 547e03cacde..55bdc14b827 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -494,6 +494,7 @@ | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | | vbat_scale | 1100 | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | | vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | +| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | | vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | | vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | | vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | From a06f59ba828b02730f02f194915909a6d727fd5b Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 10 Jan 2021 01:31:59 +0200 Subject: [PATCH 035/143] added vtx_smartaudio_early_akk_workaround options to settings.md in alphabetical order --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 55bdc14b827..5c66266b765 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -494,7 +494,6 @@ | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | | vbat_scale | 1100 | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | | vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | -| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | | vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | | vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | | vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | @@ -502,6 +501,7 @@ | vtx_max_power_override | 0 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities | | vtx_pit_mode_chan | | | | vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | +| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | | yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | From 3305d536bcae745d4594028743f4d39b98710efe Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 13 Jan 2021 13:53:05 +0100 Subject: [PATCH 036/143] pan servo home dir offset --- src/main/fc/settings.yaml | 14 ++++++++++++++ src/main/io/osd.c | 19 +++++++++++++++++-- src/main/io/osd.h | 3 ++- 3 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f5fde879efe..f520de2d177 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3015,6 +3015,20 @@ groups: default_value: "ON" description: Should home position coordinates be displayed on the arming screen. + - name: osd_pan_servo_index + type: uint8_t + field: pan_servo_index + min: 0 + max: 10 + + - name: osd_pan_servo_us2centideg + type: uint16_t + field: pan_servo_us2centideg + default_value: 9 + min: -36 + max: 36 + + - name: PG_SYSTEM_CONFIG type: systemConfig_t headers: ["fc/config.h"] diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ae077441bed..b9f1f507bea 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -87,6 +87,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/pid.h" #include "flight/rth_estimator.h" #include "flight/wind_estimator.h" +#include "flight/servos.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -185,7 +186,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); static int digitCount(int32_t value) @@ -1128,6 +1129,14 @@ static int16_t osdGet3DSpeed(void) return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); } +static int16_t osdPanServoHomeDirectionOffset(void) +{ + int8_t servoIndex = osdConfig()->pan_servo_index; + int16_t servoPosition = servo[servoIndex]; + int16_t servoMiddle = servoParams(servoIndex)->middle; + return (int16_t)((servoPosition - servoMiddle) * CENTIDEGREES_TO_DEGREES(osdConfig()->pan_servo_us2centideg)); +} + #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { @@ -1337,7 +1346,11 @@ static bool osdDrawSingleElement(uint8_t item) } else { - int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); + int16_t panHomeDirOffset = 0; + if (!osdConfig()->pan_servo_us2centideg){ + panHomeDirOffset = osdPanServoHomeDirectionOffset(); + } + int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset; osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection); } } else { @@ -2591,6 +2604,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, .sidebar_scroll_arrows = 0, .osd_home_position_arm_screen = true, + .pan_servo_index = 0, + .pan_servo_us2centideg = 0, .units = OSD_UNIT_METRIC, .main_voltage_decimals = 1, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d740b5de521..6dce63dadff 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -348,7 +348,8 @@ typedef struct osdConfig_s { uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. bool osd_home_position_arm_screen; - + uint8_t pan_servo_index; // Index of the pan servo used for home direction offset + uint16_t pan_servo_us2centideg; // cenitdegrees per us pwm uint8_t crsf_lq_format; } osdConfig_t; From 4eaa18b928edabdcf3a2372171caed566945c0da Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 13 Jan 2021 14:28:27 +0100 Subject: [PATCH 037/143] changed variable type --- src/main/fc/settings.yaml | 2 +- src/main/io/osd.c | 4 ++-- src/main/io/osd.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f520de2d177..44d739e5363 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3022,7 +3022,7 @@ groups: max: 10 - name: osd_pan_servo_us2centideg - type: uint16_t + type: float field: pan_servo_us2centideg default_value: 9 min: -36 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b9f1f507bea..50cf3947550 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1134,7 +1134,7 @@ static int16_t osdPanServoHomeDirectionOffset(void) int8_t servoIndex = osdConfig()->pan_servo_index; int16_t servoPosition = servo[servoIndex]; int16_t servoMiddle = servoParams(servoIndex)->middle; - return (int16_t)((servoPosition - servoMiddle) * CENTIDEGREES_TO_DEGREES(osdConfig()->pan_servo_us2centideg)); + return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_us2centideg); } #endif @@ -1347,7 +1347,7 @@ static bool osdDrawSingleElement(uint8_t item) else { int16_t panHomeDirOffset = 0; - if (!osdConfig()->pan_servo_us2centideg){ + if (!(osdConfig()->pan_servo_us2centideg == 0)){ panHomeDirOffset = osdPanServoHomeDirectionOffset(); } int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 6dce63dadff..07a861e9c9e 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -349,7 +349,7 @@ typedef struct osdConfig_s { uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. bool osd_home_position_arm_screen; uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - uint16_t pan_servo_us2centideg; // cenitdegrees per us pwm + float pan_servo_us2centideg; // cenitdegrees per us pwm uint8_t crsf_lq_format; } osdConfig_t; From 74dbdc9bb0a6c066a1630ccfdd1dcf05c98b5ffd Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 13 Jan 2021 14:29:25 +0100 Subject: [PATCH 038/143] For bench testing --- 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 50cf3947550..14841212a52 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1340,7 +1340,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { // && isImuHeadingValid() if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); } From 0c42ae1f62f7f9c3e032fec8be27e147b8a62a18 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 13 Jan 2021 15:00:09 +0100 Subject: [PATCH 039/143] Some improvements. --- src/main/fc/settings.yaml | 6 +++--- src/main/io/osd.c | 18 +++++++++--------- src/main/io/osd.h | 2 +- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 44d739e5363..ef3dc38c128 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3016,15 +3016,15 @@ groups: description: Should home position coordinates be displayed on the arming screen. - name: osd_pan_servo_index - type: uint8_t + description: Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. field: pan_servo_index min: 0 max: 10 - name: osd_pan_servo_us2centideg - type: float + description: Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. field: pan_servo_us2centideg - default_value: 9 + default_value: 0 min: -36 max: 36 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 14841212a52..5f97bea74e2 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -949,6 +949,14 @@ int16_t osdGetHeading(void) return attitude.values.yaw; } +int16_t osdPanServoHomeDirectionOffset(void) +{ + int8_t servoIndex = osdConfig()->pan_servo_index; + int16_t servoPosition = servo[servoIndex]; + int16_t servoMiddle = servoParams(servoIndex)->middle; + return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_us2centideg); +} + // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle) { @@ -1129,14 +1137,6 @@ static int16_t osdGet3DSpeed(void) return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); } -static int16_t osdPanServoHomeDirectionOffset(void) -{ - int8_t servoIndex = osdConfig()->pan_servo_index; - int16_t servoPosition = servo[servoIndex]; - int16_t servoMiddle = servoParams(servoIndex)->middle; - return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_us2centideg); -} - #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { @@ -1340,7 +1340,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { // && isImuHeadingValid() + if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 07a861e9c9e..021113a0090 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -349,7 +349,7 @@ typedef struct osdConfig_s { uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. bool osd_home_position_arm_screen; uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - float pan_servo_us2centideg; // cenitdegrees per us pwm + int8_t pan_servo_us2centideg; // centidegrees per us pwm uint8_t crsf_lq_format; } osdConfig_t; From 40cbcc7e45dd910a460901c34e5e9ca448d06e7a Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 13 Jan 2021 15:03:29 +0100 Subject: [PATCH 040/143] Update docs --- docs/Settings.md | 2 ++ src/main/fc/settings.yaml | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 547e03cacde..0bd5cf8c28a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -389,6 +389,8 @@ | osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | +| osd_pan_servo_index | | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. | +| osd_pan_servo_us2centideg | 0 | Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. | | osd_plus_code_digits | 10 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | | osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. | | osd_right_sidebar_scroll | | | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ef3dc38c128..b5e9ada3466 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3022,7 +3022,7 @@ groups: max: 10 - name: osd_pan_servo_us2centideg - description: Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. + description: Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. field: pan_servo_us2centideg default_value: 0 min: -36 From 3117a2ee59811efb638b59322a0ad8dffede26fb Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 13 Jan 2021 15:09:05 +0100 Subject: [PATCH 041/143] change command name --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 6 +++--- src/main/io/osd.c | 6 +++--- src/main/io/osd.h | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 0bd5cf8c28a..e2302536101 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -390,7 +390,7 @@ | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_pan_servo_index | | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. | -| osd_pan_servo_us2centideg | 0 | Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. | +| osd_pan_servo_pwm2centideg | 0 | Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. | | osd_plus_code_digits | 10 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | | osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. | | osd_right_sidebar_scroll | | | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b5e9ada3466..78ad14710e9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3021,9 +3021,9 @@ groups: min: 0 max: 10 - - name: osd_pan_servo_us2centideg - description: Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. - field: pan_servo_us2centideg + - name: osd_pan_servo_pwm2centideg + description: Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. + field: pan_servo_pwm2centideg default_value: 0 min: -36 max: 36 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 5f97bea74e2..660db14c038 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -954,7 +954,7 @@ int16_t osdPanServoHomeDirectionOffset(void) int8_t servoIndex = osdConfig()->pan_servo_index; int16_t servoPosition = servo[servoIndex]; int16_t servoMiddle = servoParams(servoIndex)->middle; - return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_us2centideg); + return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg); } // Returns a heading angle in degrees normalized to [0, 360). @@ -1347,7 +1347,7 @@ static bool osdDrawSingleElement(uint8_t item) else { int16_t panHomeDirOffset = 0; - if (!(osdConfig()->pan_servo_us2centideg == 0)){ + if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ panHomeDirOffset = osdPanServoHomeDirectionOffset(); } int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset; @@ -2605,7 +2605,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .sidebar_scroll_arrows = 0, .osd_home_position_arm_screen = true, .pan_servo_index = 0, - .pan_servo_us2centideg = 0, + .pan_servo_pwm2centideg = 0, .units = OSD_UNIT_METRIC, .main_voltage_decimals = 1, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 021113a0090..e260e14f21b 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -349,7 +349,7 @@ typedef struct osdConfig_s { uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. bool osd_home_position_arm_screen; uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - int8_t pan_servo_us2centideg; // centidegrees per us pwm + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; } osdConfig_t; From 22772392154e398b4a8d07710e336453f0f03ae6 Mon Sep 17 00:00:00 2001 From: Daniel Arruda Ribeiro Date: Thu, 14 Jan 2021 21:18:11 -0300 Subject: [PATCH 042/143] Add a new MSP2_INAV_MISC2 message type, that allows a peer to fetch the On Time, Flight Time, Throttle percent and Auto Throttle flag. --- src/main/fc/fc_msp.c | 19 +++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 3 +++ 2 files changed, 22 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d5ad2f30225..854c000d4f7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -824,6 +824,25 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, currentBatteryProfile->capacity.unit); break; + case MSP2_INAV_MISC2: + // Timers + sbufWriteU32(dst, micros() / 1000000); // On time (seconds) + sbufWriteU32(dst, getFlightTime()); // Flight time (seconds) + + // Throttle + int16_t thr = 0; + const int minThrottle = getThrottleIdleValue(); + bool autoThrottle = navigationIsControllingThrottle(); + if(autoThrottle) + thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); + else + thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle); + + sbufWriteU8(dst, thr); // Throttle Percent + sbufWriteU8(dst, autoThrottle ? 1 : 0); // Auto Throttle Flag (0 or 1) + + break; + case MSP2_INAV_BATTERY_CONFIG: sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); sbufWriteU8(dst, batteryMetersConfig()->voltageSource); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 219e7707224..70e394ccb3f 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -77,3 +77,6 @@ #define MSP2_INAV_SAFEHOME 0x2038 #define MSP2_INAV_SET_SAFEHOME 0x2039 + +#define MSP2_INAV_MISC2 0x203A + From 101799952e608afd061cfce44ef6ec96005ee3fb Mon Sep 17 00:00:00 2001 From: Daniel Arruda Ribeiro Date: Fri, 15 Jan 2021 18:12:45 -0300 Subject: [PATCH 043/143] Moved throttle percent calculation from MSP and OSD to Mixer --- src/main/fc/fc_msp.c | 12 ++---------- src/main/flight/mixer.c | 7 +++++++ src/main/flight/mixer.h | 1 + src/main/io/osd.c | 5 +---- 4 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 854c000d4f7..08875648926 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -830,16 +830,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU32(dst, getFlightTime()); // Flight time (seconds) // Throttle - int16_t thr = 0; - const int minThrottle = getThrottleIdleValue(); - bool autoThrottle = navigationIsControllingThrottle(); - if(autoThrottle) - thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); - else - thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle); - - sbufWriteU8(dst, thr); // Throttle Percent - sbufWriteU8(dst, autoThrottle ? 1 : 0); // Auto Throttle Flag (0 or 1) + sbufWriteU8(dst, throttlePercent); // Throttle Percent + sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1) break; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c3699ac1b0b..9d6ed592b9c 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -62,6 +62,7 @@ static float mixerScale = 1.0f; static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static EXTENDED_FASTRAM uint8_t motorCount = 0; EXTENDED_FASTRAM int mixerThrottleCommand; +EXTENDED_FASTRAM int throttlePercent; static EXTENDED_FASTRAM int throttleIdleValue = 0; static EXTENDED_FASTRAM int motorValueWhenStopped = 0; static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; @@ -529,6 +530,12 @@ void FAST_CODE mixTable(const float dT) throttleRange = throttleMax - throttleMin; + // Throttle Percent used by OSD and MSP + if(navigationIsControllingThrottle()) + throttlePercent = (constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); + else + throttlePercent = (constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX) - throttleMin) * 100 / throttleRange; + #define THROTTLE_CLIPPING_FACTOR 0.33f motorMixRange = (float)rpyMixRange / (float)throttleRange; if (motorMixRange > 1.0f) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 405c876aa9b..14d2d6c6d43 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -111,6 +111,7 @@ typedef enum { extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int mixerThrottleCommand; +extern int throttlePercent; int getThrottleIdleValue(void); uint8_t getMotorCount(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 15bdb87f121..7c4a7aeac2f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -869,18 +869,15 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) **/ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { - const int minThrottle = getThrottleIdleValue(); buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle); if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; - thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); if (isFixedWingAutoThrottleManuallyIncreased()) TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } - tfp_sprintf(buff + 2, "%3d", thr); + tfp_sprintf(buff + 2, "%3d", throttlePercent); } /** From b60e7337a99e592e718935634b7091d272fbdb76 Mon Sep 17 00:00:00 2001 From: Adilson Oliveira Date: Sat, 23 Jan 2021 12:33:54 -0300 Subject: [PATCH 044/143] Update Building in Linux.md (#6526) --- docs/development/Building in Linux.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/Building in Linux.md b/docs/development/Building in Linux.md index 0c3a1aa83a7..73287a216b1 100644 --- a/docs/development/Building in Linux.md +++ b/docs/development/Building in Linux.md @@ -69,7 +69,7 @@ For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplies ## Using `cmake` -The canonanical method of using `cmake` is to create a `build` directory and run the `cmake` and `make` commands from within the `build` directory. So, assuming we've cloned the firmware repository into an `inav` directory, we can issue the following commands to set up the build environment. +The canonical method of using `cmake` is to create a `build` directory and run the `cmake` and `make` commands from within the `build` directory. So, assuming we've cloned the firmware repository into an `inav` directory, we can issue the following commands to set up the build environment. ``` cd inav From 331afab90712e98fbd0279e30624291a0b01e017 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 29 Jan 2021 20:59:44 +0000 Subject: [PATCH 045/143] Update Buzzer.md Added link to tool that can be used to listen to the buzzer sequences. There is also a custom sequence box at the bottom of the tool, which may aid creating new sequences. --- docs/Buzzer.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/Buzzer.md b/docs/Buzzer.md index 580c58ade14..62e2c8527ee 100644 --- a/docs/Buzzer.md +++ b/docs/Buzzer.md @@ -43,6 +43,8 @@ Sequences: 14 DISARM_REPEAT 0, 100, 10 Stick held in disarm position (after pause) 15 ARMED 0, 245, 10, 5 Board is armed (after pause ; repeats until board is disarmed or throttle is increased) +You can use [this tool](https://www.mrd-rc.com/tutorials-tools-and-testing/useful-tools/helpful-inav-buzzer-code-checker/) to hear current buzzer sequences or enter custom sequences. + ## Controlling buzzer usage The usage of the buzzer can be controlled by the CLI `beeper` command. From be58a0457e7e7ad3a9490f55b484eac1278f8b88 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 31 Jan 2021 15:10:48 +0100 Subject: [PATCH 046/143] set min mag calibration time to 20 sec --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f5fde879efe..71af8c5f62e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -461,7 +461,7 @@ groups: description: "Adjust how long time the Calibration of mag will last." default_value: "30" field: magCalibrationTimeLimit - min: 30 + min: 20 max: 120 - name: mag_to_use description: "Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target" From fa660aaacd490e56cda5907e1d4e6319ec064e31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 31 Jan 2021 16:45:03 +0000 Subject: [PATCH 047/143] build: Use -Os for F7 targets with flash <= 512K As requested by @dzikuvx ;-) --- cmake/stm32f7.cmake | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cmake/stm32f7.cmake b/cmake/stm32f7.cmake index 51d6786a265..238920bcf80 100644 --- a/cmake/stm32f7.cmake +++ b/cmake/stm32f7.cmake @@ -97,8 +97,6 @@ function(target_stm32f7xx) VCP_SOURCES ${STM32F7_USB_SRC} ${STM32F7_VCP_SRC} VCP_INCLUDE_DIRECTORIES ${STM32F7_USB_INCLUDE_DIRS} ${STM32F7_VCP_DIR} - OPTIMIZATION -O2 - OPENOCD_TARGET stm32f7x BOOTLOADER @@ -112,6 +110,11 @@ macro(define_target_stm32f7 subfamily size) set(func_ARGV ARGV) string(TOUPPER ${size} upper_size) get_stm32_flash_size(flash_size ${size}) + if(flash_size GREATER 512) + set(opt -O2) + else() + set(opt -Os) + endif() set(definitions STM32F7 STM32F7${subfamily}xx @@ -123,6 +126,8 @@ macro(define_target_stm32f7 subfamily size) STARTUP startup_stm32f7${subfamily}xx.s COMPILE_DEFINITIONS ${definitions} LINKER_SCRIPT stm32_flash_f7${subfamily}x${size} + OPTIMIZATION ${opt} + ${${func_ARGV}} ) endfunction() From 7447beb25dd693c83796c061c1820e430e3e48a7 Mon Sep 17 00:00:00 2001 From: Daniel Arruda Ribeiro Date: Mon, 1 Feb 2021 10:54:06 -0300 Subject: [PATCH 048/143] Fix Throttle percent logic and code cleanup --- src/main/fc/fc_msp.c | 2 +- src/main/flight/mixer.c | 12 ++++++------ src/main/flight/mixer.h | 2 +- src/main/io/osd.c | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 08875648926..a32961dabae 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -830,7 +830,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU32(dst, getFlightTime()); // Flight time (seconds) // Throttle - sbufWriteU8(dst, throttlePercent); // Throttle Percent + sbufWriteU8(dst, getThrottlePercent()); // Throttle Percent sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1) break; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9d6ed592b9c..a07e4f9aa45 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -530,12 +530,6 @@ void FAST_CODE mixTable(const float dT) throttleRange = throttleMax - throttleMin; - // Throttle Percent used by OSD and MSP - if(navigationIsControllingThrottle()) - throttlePercent = (constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); - else - throttlePercent = (constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX) - throttleMin) * 100 / throttleRange; - #define THROTTLE_CLIPPING_FACTOR 0.33f motorMixRange = (float)rpyMixRange / (float)throttleRange; if (motorMixRange > 1.0f) { @@ -579,6 +573,12 @@ void FAST_CODE mixTable(const float dT) motorRateLimitingApplyFn(dT); } +int16_t getThrottlePercent(void) +{ + int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - getThrottleIdleValue()) * 100 / (motorConfig()->maxthrottle - getThrottleIdleValue()); + return thr; +} + motorStatus_e getMotorStatus(void) { if (failsafeRequiresMotorStop()) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 14d2d6c6d43..dfe1abedb59 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -111,9 +111,9 @@ typedef enum { extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int mixerThrottleCommand; -extern int throttlePercent; int getThrottleIdleValue(void); +int16_t getThrottlePercent(void); uint8_t getMotorCount(void); float getMotorMixRange(void); bool mixerIsOutputSaturated(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7c4a7aeac2f..c637c45a1b4 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -877,7 +877,7 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t if (isFixedWingAutoThrottleManuallyIncreased()) TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } - tfp_sprintf(buff + 2, "%3d", throttlePercent); + tfp_sprintf(buff + 2, "%3d", getThrottlePercent()); } /** From 8a8efbcbeebd3156a0f4a095c57ce53ad4634744 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 1 Feb 2021 14:58:53 +0100 Subject: [PATCH 049/143] Basic support for Rush Blade F7 --- src/main/target/RUSH_BLADE_F7/CMakeLists.txt | 1 + src/main/target/RUSH_BLADE_F7/target.c | 37 +++++ src/main/target/RUSH_BLADE_F7/target.h | 147 +++++++++++++++++++ 3 files changed, 185 insertions(+) create mode 100644 src/main/target/RUSH_BLADE_F7/CMakeLists.txt create mode 100644 src/main/target/RUSH_BLADE_F7/target.c create mode 100644 src/main/target/RUSH_BLADE_F7/target.h diff --git a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt new file mode 100644 index 00000000000..5dd7f2d2e1d --- /dev/null +++ b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(RUSH_BLADE_F7) \ No newline at end of file diff --git a/src/main/target/RUSH_BLADE_F7/target.c b/src/main/target/RUSH_BLADE_F7/target.c new file mode 100644 index 00000000000..c56197a99e2 --- /dev/null +++ b/src/main/target/RUSH_BLADE_F7/target.c @@ -0,0 +1,37 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP2-1 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP2-1 D(2, 7, 7) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 UP1-2 D(1, 2, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 UP1-2 D(1, 7, 5) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2, 3, 7) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1, 4, 5) + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h new file mode 100644 index 00000000000..ca0175f10df --- /dev/null +++ b/src/main/target/RUSH_BLADE_F7/target.h @@ -0,0 +1,147 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "RBF7" +#define USBD_PRODUCT_STRING "RUSH_BLADE_F7" + +#define LED0 PB10 //Blue SWCLK +// #define LED1 PA13 //Green SWDIO + +#define BEEPER PB2 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define GYRO_INT_EXTI PA4 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_CS_PIN PC4 +#define MPU6000_SPI_BUS BUS_SPI1 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** SPI2 Flash *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP +#define USB_DETECT_PIN PC14 +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL) +#define CURRENT_METER_SCALE 179 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 10 +#define USE_OSD +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR From 0586f595b24519ce0afb7269c2c0a6f85427051e Mon Sep 17 00:00:00 2001 From: Daniel Arruda Ribeiro Date: Mon, 1 Feb 2021 11:05:09 -0300 Subject: [PATCH 050/143] Remove unused variable --- src/main/flight/mixer.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a07e4f9aa45..fd0b83a468b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -62,7 +62,6 @@ static float mixerScale = 1.0f; static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static EXTENDED_FASTRAM uint8_t motorCount = 0; EXTENDED_FASTRAM int mixerThrottleCommand; -EXTENDED_FASTRAM int throttlePercent; static EXTENDED_FASTRAM int throttleIdleValue = 0; static EXTENDED_FASTRAM int motorValueWhenStopped = 0; static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; From bae0e9b00c6b2152a817258dfb1adc4dea4bba36 Mon Sep 17 00:00:00 2001 From: Olivier C Date: Tue, 2 Feb 2021 09:15:41 +0100 Subject: [PATCH 051/143] Remove duplicate WP message in the OSD/Hud ... since it's now displayed in the system message ("TO WP 1/4") --- src/main/io/osd.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3612ef00d5a..6dbe3189aac 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1762,9 +1762,6 @@ static bool osdDrawSingleElement(uint8_t item) gpsLocation_t wp2; int j; - tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount); - displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff); - for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top j = posControl.activeWaypointIndex + i; if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) { From 4c5dc00668b27450723aa31c7b97acc1a1195d93 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Tue, 2 Feb 2021 14:24:04 +0000 Subject: [PATCH 052/143] add missed V2 ADC pin changes (#6555) --- src/main/target/FLYWOOF411/target.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index 57d2347660a..675513def2f 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -140,8 +140,13 @@ #define USE_ADC #define ADC_INSTANCE ADC1 #define ADC_CHANNEL_1_PIN PA1 +#ifdef FLYWOOF411_V2 +#define ADC_CHANNEL_2_PIN PB1 +#define ADC_CHANNEL_3_PIN PB0 +#else #define ADC_CHANNEL_2_PIN PA0 #define ADC_CHANNEL_3_PIN PB1 +#endif #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 #define VBAT_ADC_CHANNEL ADC_CHN_2 From b322940dd78f542864a0d091b5d9420d35c5dd18 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 2 Feb 2021 18:17:28 +0100 Subject: [PATCH 053/143] AFATFS: fix freadSync (#6550) --- src/main/io/asyncfatfs/asyncfatfs.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 0d84a756ca9..f4f8af6d33f 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -3252,6 +3252,7 @@ uint32_t afatfs_freadSync(afatfsFilePtr_t file, uint8_t *buffer, uint32_t length uint32_t leftToRead = length - bytesRead; uint32_t readNow = afatfs_fread(file, buffer, leftToRead); bytesRead += readNow; + buffer += readNow; if (bytesRead < length) { if (afatfs_feof(file)) { From 349f054bdaae83957d0e67a7d54c506ce5ab888b Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 3 Feb 2021 13:19:42 +0000 Subject: [PATCH 054/143] Implement Prearm --- src/main/fc/fc_core.c | 9 +++++++++ src/main/fc/fc_msp_box.c | 2 ++ src/main/fc/rc_modes.h | 1 + src/main/fc/runtime_config.c | 5 +++-- src/main/fc/runtime_config.h | 6 +++--- src/main/io/osd.c | 2 ++ src/main/io/osd.h | 1 + src/main/io/osd_dji_hd.c | 2 ++ 8 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 71f758dc408..48e941a0184 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -295,6 +295,14 @@ static void updateArmingStatus(void) DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); } + if (isModeActivationConditionPresent(BOXPREARM)) { + if (IS_RC_MODE_ACTIVE(BOXPREARM)) { + DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); + } else { + ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); + } + } + /* CHECK: Arming switch */ // If arming is disabled and the ARM switch is on // Note that this should be last check so all other blockers could be cleared correctly @@ -506,6 +514,7 @@ void tryArm(void) #else beeper(BEEPER_ARMING); #endif + statsOnArm(); return; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 3afe82c9cbc..4189c4f8639 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -86,6 +86,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 }, { BOXLOITERDIRCHN, "LOITER CHANGE", 49 }, { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 }, + { BOXPREARM, "PREARM", 51 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -163,6 +164,7 @@ void initActiveBoxIds(void) activeBoxIdCount = 0; activeBoxIds[activeBoxIdCount++] = BOXARM; + activeBoxIds[activeBoxIdCount++] = BOXPREARM; if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 2058c822d31..a29a39bf1df 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -68,6 +68,7 @@ typedef enum { BOXFPVANGLEMIX = 39, BOXLOITERDIRCHN = 40, BOXMSPRCOVERRIDE = 41, + BOXPREARM = 42, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 5ba8c6287e9..b0d014d25ef 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -36,7 +36,7 @@ const char *armingDisableFlagNames[]= { "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS", "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX", "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM", - "SETTINGFAIL", "PWMOUT" + "SETTINGFAIL", "PWMOUT", "NOPREARM" }; #endif @@ -57,7 +57,8 @@ const armingFlag_e armDisableReasonsChecklist[] = { ARMING_DISABLED_OSD_MENU, ARMING_DISABLED_ROLLPITCH_NOT_CENTERED, ARMING_DISABLED_SERVO_AUTOTRIM, - ARMING_DISABLED_OOM + ARMING_DISABLED_OOM, + ARMING_DISABLED_NO_PREARM }; armingFlag_e isArmingDisabledReason(void) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 7b28b464688..aeb7a99af41 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -23,7 +23,6 @@ typedef enum { WAS_EVER_ARMED = (1 << 3), ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), - ARMING_DISABLED_NOT_LEVEL = (1 << 8), ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9), ARMING_DISABLED_SYSTEM_OVERLOADED = (1 << 10), @@ -44,6 +43,7 @@ typedef enum { ARMING_DISABLED_OOM = (1 << 25), ARMING_DISABLED_INVALID_SETTING = (1 << 26), ARMING_DISABLED_PWM_OUTPUT_ERROR = (1 << 27), + ARMING_DISABLED_NO_PREARM = (1 << 28), ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | @@ -52,7 +52,7 @@ typedef enum { ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING | - ARMING_DISABLED_PWM_OUTPUT_ERROR), + ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM), } armingFlag_e; // Arming blockers that can be overriden by emergency arming. @@ -78,7 +78,7 @@ extern const char *armingDisableFlagNames[]; #define ARMING_FLAG(mask) (armingFlags & (mask)) // Returns the 1st flag from ARMING_DISABLED_ALL_FLAGS which is -// preventing arming, or zero is arming is not disabled. +// preventing arming, or zero if arming is not disabled. armingFlag_e isArmingDisabledReason(void); typedef enum { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3612ef00d5a..0cfc25a6f2d 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -712,6 +712,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE); case ARMING_DISABLED_PWM_OUTPUT_ERROR: return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); + case ARMING_DISABLED_NO_PREARM: + return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d740b5de521..386d7bbcaab 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -76,6 +76,7 @@ #define OSD_MSG_INVALID_SETTING "INVALID SETTING" #define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" #define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" +#define OSD_MSG_NO_PREARM "NO PREARM" #define OSD_MSG_RTH_FS "(RTH)" #define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" #define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index f506c8b9cf9..7da1e1542a7 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -500,6 +500,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR("CLI"); case ARMING_DISABLED_PWM_OUTPUT_ERROR: return OSD_MESSAGE_STR("PWM ERR"); + case ARMING_DISABLED_NO_PREARM: + return OSD_MESSAGE_STR("NO PREARM"); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; From d9158e3ff520a558c810e37b2c8562f61f912776 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 3 Feb 2021 17:36:28 +0000 Subject: [PATCH 055/143] Optimize isModeActivationConditionPresent --- src/main/fc/rc_modes.c | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 2336f6e965f..1bd12915f27 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -134,13 +134,7 @@ void rcModeUpdate(boxBitmask_t *newState) bool isModeActivationConditionPresent(boxId_e modeId) { - for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - if (modeActivationConditions(index)->modeId == modeId && IS_RANGE_USABLE(&modeActivationConditions(index)->range)) { - return true; - } - } - - return false; + return specifiedConditionCountPerMode[modeId] > 0; } bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) From c22218c701562657321bb07793e450541563321b Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 3 Feb 2021 20:36:39 +0200 Subject: [PATCH 056/143] Update vtx_control.c added smartAudioEarlyAkkWorkaroundEnable initialization --- src/main/io/vtx_control.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 6e213d75570..f80b992c291 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -39,10 +39,11 @@ #if defined(USE_VTX_CONTROL) -PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3); PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, .halfDuplex = true, + .smartAudioEarlyAkkWorkaroundEnable = true, ); static uint8_t locked = 0; From fdc8db6a9165e33f5fb987bec148a194adb4cefd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 5 Jan 2021 12:23:48 +0100 Subject: [PATCH 057/143] Extract navigation PID controller to separate module --- src/main/CMakeLists.txt | 2 + src/main/common/fp_pid.c | 156 +++++++++++++++++++++++ src/main/common/fp_pid.h | 75 +++++++++++ src/main/fc/cli.c | 1 + src/main/flight/pid.c | 1 + src/main/navigation/navigation.c | 126 ------------------ src/main/navigation/navigation.h | 28 +--- src/main/navigation/navigation_private.h | 22 ---- src/main/programming/pid.h | 2 +- 9 files changed, 237 insertions(+), 176 deletions(-) create mode 100644 src/main/common/fp_pid.c create mode 100644 src/main/common/fp_pid.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 364356342f3..a80fdd9feb4 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -24,6 +24,8 @@ main_sources(COMMON_SRC common/encoding.h common/filter.c common/filter.h + common/fp_pid.c + common/fp_pid.h common/gps_conversion.c common/gps_conversion.h common/log.c diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c new file mode 100644 index 00000000000..753789c9dc1 --- /dev/null +++ b/src/main/common/fp_pid.c @@ -0,0 +1,156 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" + +FILE_COMPILE_FOR_SPEED + +#include +#include "common/fp_pid.h" + +/*----------------------------------------------------------- + * Float point PID-controller implementation + *-----------------------------------------------------------*/ +// Implementation of PID with back-calculation I-term anti-windup +// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228) +// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf +float navPidApply3( + pidController_t *pid, + const float setpoint, + const float measurement, + const float dt, + const float outMin, + const float outMax, + const pidControllerFlags_e pidFlags, + const float gainScaler, + const float dTermScaler +) { + float newProportional, newDerivative, newFeedForward; + float error = setpoint - measurement; + + /* P-term */ + newProportional = error * pid->param.kP * gainScaler; + + /* D-term */ + if (pid->reset) { + pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement; + pid->reset = false; + } + + if (pidFlags & PID_DTERM_FROM_ERROR) { + /* Error-tracking D-term */ + newDerivative = (error - pid->last_input) / dt; + pid->last_input = error; + } else { + /* Measurement tracking D-term */ + newDerivative = -(measurement - pid->last_input) / dt; + pid->last_input = measurement; + } + + if (pid->dTermLpfHz > 0) { + newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt); + } else { + newDerivative = pid->param.kD * newDerivative; + } + + newDerivative = newDerivative * gainScaler * dTermScaler; + + if (pidFlags & PID_ZERO_INTEGRATOR) { + pid->integrator = 0.0f; + } + + /* + * Compute FeedForward parameter + */ + newFeedForward = setpoint * pid->param.kFF * gainScaler; + + /* Pre-calculate output and limit it if actuator is saturating */ + const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward; + const float outValConstrained = constrainf(outVal, outMin, outMax); + + pid->proportional = newProportional; + pid->integral = pid->integrator; + pid->derivative = newDerivative; + pid->feedForward = newFeedForward; + pid->output_constrained = outValConstrained; + + /* Update I-term */ + if (!(pidFlags & PID_ZERO_INTEGRATOR)) { + const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt); + + if (pidFlags & PID_SHRINK_INTEGRATOR) { + // Only allow integrator to shrink + if (fabsf(newIntegrator) < fabsf(pid->integrator)) { + pid->integrator = newIntegrator; + } + } + else { + pid->integrator = newIntegrator; + } + } + + if (pidFlags & PID_LIMIT_INTEGRATOR) { + pid->integrator = constrainf(pid->integrator, outMin, outMax); + } + + return outValConstrained; +} + +float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags) +{ + return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f); +} + +void navPidReset(pidController_t *pid) +{ + pid->reset = true; + pid->proportional = 0.0f; + pid->integral = 0.0f; + pid->derivative = 0.0f; + pid->integrator = 0.0f; + pid->last_input = 0.0f; + pid->feedForward = 0.0f; + pt1FilterReset(&pid->dterm_filter_state, 0.0f); + pid->output_constrained = 0.0f; +} + +void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz) +{ + pid->param.kP = _kP; + pid->param.kI = _kI; + pid->param.kD = _kD; + pid->param.kFF = _kFF; + + if (_kI > 1e-6f && _kP > 1e-6f) { + float Ti = _kP / _kI; + float Td = _kD / _kP; + pid->param.kT = 2.0f / (Ti + Td); + } + else { + pid->param.kI = 0.0; + pid->param.kT = 0.0; + } + pid->dTermLpfHz = _dTermLpfHz; + navPidReset(pid); +} \ No newline at end of file diff --git a/src/main/common/fp_pid.h b/src/main/common/fp_pid.h new file mode 100644 index 00000000000..0698c94ddbc --- /dev/null +++ b/src/main/common/fp_pid.h @@ -0,0 +1,75 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include + +#include "common/filter.h" +#include "common/maths.h" + +typedef struct { + float kP; + float kI; + float kD; + float kT; // Tracking gain (anti-windup) + float kFF; // FeedForward Component +} pidControllerParam_t; + +typedef enum { + PID_DTERM_FROM_ERROR = 1 << 0, + PID_ZERO_INTEGRATOR = 1 << 1, + PID_SHRINK_INTEGRATOR = 1 << 2, + PID_LIMIT_INTEGRATOR = 1 << 3, +} pidControllerFlags_e; + +typedef struct { + bool reset; + pidControllerParam_t param; + pt1Filter_t dterm_filter_state; // last derivative for low-pass filter + float dTermLpfHz; // dTerm low pass filter cutoff frequency + float integrator; // integrator value + float last_input; // last input for derivative + + float integral; // used integral value in output + float proportional; // used proportional value in output + float derivative; // used derivative value in output + float feedForward; // used FeedForward value in output + float output_constrained; // controller output constrained +} pidController_t; + +float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags); +float navPidApply3( + pidController_t *pid, + const float setpoint, + const float measurement, + const float dt, + const float outMin, + const float outMax, + const pidControllerFlags_e pidFlags, + const float gainScaler, + const float dTermScaler +); +void navPidReset(pidController_t *pid); +void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz); \ No newline at end of file diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 3555f463c54..3b04aa4dc6f 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -41,6 +41,7 @@ uint8_t cliMode = 0; #include "common/memory.h" #include "common/time.h" #include "common/typeconversion.h" +#include "common/fp_pid.h" #include "programming/global_variables.h" #include "programming/pid.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5e5f9d3b5a6..fc1db033703 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -30,6 +30,7 @@ FILE_COMPILE_FOR_SPEED #include "common/filter.h" #include "common/maths.h" #include "common/utils.h" +#include "common/fp_pid.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b00dbbc0500..e61811104c6 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1913,132 +1913,6 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) return &posControl.rthState.homeTmpWaypoint; } -/*----------------------------------------------------------- - * Float point PID-controller implementation - *-----------------------------------------------------------*/ -// Implementation of PID with back-calculation I-term anti-windup -// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228) -// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf -float navPidApply3( - pidController_t *pid, - const float setpoint, - const float measurement, - const float dt, - const float outMin, - const float outMax, - const pidControllerFlags_e pidFlags, - const float gainScaler, - const float dTermScaler -) { - float newProportional, newDerivative, newFeedForward; - float error = setpoint - measurement; - - /* P-term */ - newProportional = error * pid->param.kP * gainScaler; - - /* D-term */ - if (pid->reset) { - pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement; - pid->reset = false; - } - - if (pidFlags & PID_DTERM_FROM_ERROR) { - /* Error-tracking D-term */ - newDerivative = (error - pid->last_input) / dt; - pid->last_input = error; - } else { - /* Measurement tracking D-term */ - newDerivative = -(measurement - pid->last_input) / dt; - pid->last_input = measurement; - } - - if (pid->dTermLpfHz > 0) { - newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt); - } else { - newDerivative = pid->param.kD * newDerivative; - } - - newDerivative = newDerivative * gainScaler * dTermScaler; - - if (pidFlags & PID_ZERO_INTEGRATOR) { - pid->integrator = 0.0f; - } - - /* - * Compute FeedForward parameter - */ - newFeedForward = setpoint * pid->param.kFF * gainScaler; - - /* Pre-calculate output and limit it if actuator is saturating */ - const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward; - const float outValConstrained = constrainf(outVal, outMin, outMax); - - pid->proportional = newProportional; - pid->integral = pid->integrator; - pid->derivative = newDerivative; - pid->feedForward = newFeedForward; - pid->output_constrained = outValConstrained; - - /* Update I-term */ - if (!(pidFlags & PID_ZERO_INTEGRATOR)) { - const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt); - - if (pidFlags & PID_SHRINK_INTEGRATOR) { - // Only allow integrator to shrink - if (fabsf(newIntegrator) < fabsf(pid->integrator)) { - pid->integrator = newIntegrator; - } - } - else { - pid->integrator = newIntegrator; - } - } - - if (pidFlags & PID_LIMIT_INTEGRATOR) { - pid->integrator = constrainf(pid->integrator, outMin, outMax); - } - - return outValConstrained; -} - -float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags) -{ - return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f); -} - -void navPidReset(pidController_t *pid) -{ - pid->reset = true; - pid->proportional = 0.0f; - pid->integral = 0.0f; - pid->derivative = 0.0f; - pid->integrator = 0.0f; - pid->last_input = 0.0f; - pid->feedForward = 0.0f; - pt1FilterReset(&pid->dterm_filter_state, 0.0f); - pid->output_constrained = 0.0f; -} - -void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz) -{ - pid->param.kP = _kP; - pid->param.kI = _kI; - pid->param.kD = _kD; - pid->param.kFF = _kFF; - - if (_kI > 1e-6f && _kP > 1e-6f) { - float Ti = _kP / _kI; - float Td = _kD / _kP; - pid->param.kT = 2.0f / (Ti + Td); - } - else { - pid->param.kI = 0.0; - pid->param.kT = 0.0; - } - pid->dTermLpfHz = _dTermLpfHz; - navPidReset(pid); -} - /*----------------------------------------------------------- * Detects if thrust vector is facing downwards *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5dcd8c71ca2..50ba6b2632f 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -23,6 +23,7 @@ #include "common/filter.h" #include "common/maths.h" #include "common/vector.h" +#include "common/fp_pid.h" #include "config/feature.h" @@ -324,33 +325,6 @@ typedef struct navDestinationPath_s { int32_t bearing; // deg * 100 } navDestinationPath_t; -typedef struct { - float kP; - float kI; - float kD; - float kT; // Tracking gain (anti-windup) - float kFF; // FeedForward Component -} pidControllerParam_t; - -typedef struct { - float kP; -} pControllerParam_t; - -typedef struct { - bool reset; - pidControllerParam_t param; - pt1Filter_t dterm_filter_state; // last derivative for low-pass filter - float dTermLpfHz; // dTerm low pass filter cutoff frequency - float integrator; // integrator value - float last_input; // last input for derivative - - float integral; // used integral value in output - float proportional; // used proportional value in output - float derivative; // used derivative value in output - float feedForward; // used FeedForward value in output - float output_constrained; // controller output constrained -} pidController_t; - typedef struct navigationPIDControllers_s { /* Multicopter PIDs */ pidController_t pos[XYZ_AXIS_COUNT]; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 95a67e39173..6e7c4d3c0c8 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -91,13 +91,6 @@ typedef struct navigationFlags_s { bool forcedRTHActivated; } navigationFlags_t; -typedef enum { - PID_DTERM_FROM_ERROR = 1 << 0, - PID_ZERO_INTEGRATOR = 1 << 1, - PID_SHRINK_INTEGRATOR = 1 << 2, - PID_LIMIT_INTEGRATOR = 1 << 3, -} pidControllerFlags_e; - typedef struct { fpVector3_t pos; fpVector3_t vel; @@ -392,21 +385,6 @@ extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients; /* Internally used functions */ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); -float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags); -float navPidApply3( - pidController_t *pid, - const float setpoint, - const float measurement, - const float dt, - const float outMin, - const float outMax, - const pidControllerFlags_e pidFlags, - const float gainScaler, - const float dTermScaler -); -void navPidReset(pidController_t *pid); -void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz); - bool isThrustFacingDownwards(void); uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos); int32_t calculateBearingToDestination(const fpVector3_t * destinationPos); diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h index f120f9f0739..8645de8f130 100644 --- a/src/main/programming/pid.h +++ b/src/main/programming/pid.h @@ -26,11 +26,11 @@ #include "config/parameter_group.h" #include "common/time.h" +#include "common/fp_pid.h" #include "programming/logic_condition.h" #include "common/axis.h" #include "flight/pid.h" -#include "navigation/navigation.h" #define MAX_PROGRAMMING_PID_COUNT 4 From 0f5d6f4463ef18f028adae7decc35c2b2e343f3d Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 3 Feb 2021 20:24:22 +0000 Subject: [PATCH 058/143] Handle edge case --- src/main/fc/fc_core.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 48e941a0184..b1fe71ca436 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -301,6 +301,8 @@ static void updateArmingStatus(void) } else { ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); } + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); } /* CHECK: Arming switch */ From 9a579ea4b4fcbc55678e955783d908c685fb6732 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Feb 2021 10:00:03 +0100 Subject: [PATCH 059/143] Ability to trim pitch angle for level flight --- docs/Settings.md | 1 + src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 24 +++++++++++++++++++++++- src/main/flight/pid.h | 2 ++ 4 files changed, 32 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 547e03cacde..3c64f833d30 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -120,6 +120,7 @@ | fw_i_yaw | 10 | Fixed-wing rate stabilisation I-gain for YAW | | fw_iterm_limit_stick_position | 0.5 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side | | fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | +| fw_level_pitch_trim | 0 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level | | fw_loiter_direction | RIGHT | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | | fw_min_throttle_down_pitch | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | | fw_p_level | 20 | Fixed-wing attitude stabilisation P-gain | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c3e286d37ec..0e2b51d4fd9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1874,6 +1874,12 @@ groups: condition: USE_GYRO_KALMAN min: 1 max: 16000 + - name: fw_level_pitch_trim + description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level" + default_value: "0" + field: fixedWingLevelTrim + min: -10 + max: 10 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fc1db033703..be792baa6a2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,8 +154,9 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; +static EXTENDED_FASTRAM float fixedWingLevelTrim; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -280,6 +281,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .kalman_w = 4, .kalman_sharpness = 100, .kalmanEnabled = 0, + .fixedWingLevelTrim = 0, ); bool pidInitFilters(void) @@ -534,6 +536,24 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); + + //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming + if (axis == FD_PITCH && STATE(AIRPLANE)) { + /* + * fixedWingLevelTrim has opposite sign to rcCommand. + * Positive rcCommand means nose should point downwards + * Negative rcCommand mean nose should point upwards + * This is counter intuitive and a natural way suggests that + should mean UP + * This is why fixedWingLevelTrim has opposite sign to rcCommand + * Positive fixedWingLevelTrim means nose should point upwards + * Negative fixedWingLevelTrim means nose should point downwards + */ + DEBUG_SET(DEBUG_ALWAYS, 0, fixedWingLevelTrim); + DEBUG_SET(DEBUG_ALWAYS, 1, angleTarget); + angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); + DEBUG_SET(DEBUG_ALWAYS, 2, angleTarget); + } + const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); @@ -1108,6 +1128,8 @@ void pidInit(void) gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness); } #endif + + fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim; } const pidBank_t * pidBank(void) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7f83accab6c..83b7b6380ca 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -150,6 +150,8 @@ typedef struct pidProfile_s { uint16_t kalman_w; uint16_t kalman_sharpness; uint8_t kalmanEnabled; + + float fixedWingLevelTrim; } pidProfile_t; typedef struct pidAutotuneConfig_s { From 4b12ca3326bbfd3feb7ed3f014cdf9c558a95371 Mon Sep 17 00:00:00 2001 From: Marcin Galczynski Date: Fri, 5 Feb 2021 01:22:59 +0100 Subject: [PATCH 060/143] Add support for VL53L1X rangefinder --- src/main/CMakeLists.txt | 2 + src/main/drivers/bus.h | 1 + .../drivers/rangefinder/rangefinder_vl53l1x.c | 1692 +++++++++++++++++ .../drivers/rangefinder/rangefinder_vl53l1x.h | 29 + src/main/fc/settings.yaml | 2 +- src/main/sensors/rangefinder.c | 10 + src/main/sensors/rangefinder.h | 1 + src/main/target/common.h | 1 + src/main/target/common_hardware.c | 9 + 9 files changed, 1746 insertions(+), 1 deletion(-) create mode 100644 src/main/drivers/rangefinder/rangefinder_vl53l1x.c create mode 100644 src/main/drivers/rangefinder/rangefinder_vl53l1x.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 364356342f3..be87444ed0f 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -224,6 +224,8 @@ main_sources(COMMON_SRC drivers/rangefinder/rangefinder_srf10.h drivers/rangefinder/rangefinder_vl53l0x.c drivers/rangefinder/rangefinder_vl53l0x.h + drivers/rangefinder/rangefinder_vl53l1x.c + drivers/rangefinder/rangefinder_vl53l1x.h drivers/rangefinder/rangefinder_virtual.c drivers/rangefinder/rangefinder_virtual.h diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 2d7a51fc979..cad41d64401 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -137,6 +137,7 @@ typedef enum { DEVHW_SRF10, DEVHW_HCSR04_I2C, // DIY-style adapter DEVHW_VL53L0X, + DEVHW_VL53L1X, /* Other hardware */ DEVHW_MS4525, // Pitot meter diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c new file mode 100644 index 00000000000..01d6e119553 --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -0,0 +1,1692 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + * + * ================================================================= + * + * Most of the functionality of this library is based on the VL53L1X + * API provided by ST (STSW-IMG009). + * + * The following applies to source code reproduced or derived from + * the API: + * + * ----------------------------------------------------------------- + * + * Copyright (c) 2017, STMicroelectronics - All Rights Reserved + * + * This file : part of VL53L1 Core and : dual licensed, + * either 'STMicroelectronics + * Proprietary license' + * or 'BSD 3-clause "New" or "Revised" License' , at your option. + * + * ******************************************************************************* + * + * 'STMicroelectronics Proprietary license' + * + * ******************************************************************************* + * + * License terms: STMicroelectronics Proprietary in accordance with licensing + * terms at www.st.com/sla0081 + * + * STMicroelectronics confidential + * Reproduction and Communication of this document : strictly prohibited unless + * specifically authorized in writing by STMicroelectronics. + * + * + * ******************************************************************************* + * + * Alternatively, VL53L1 Core may be distributed under the terms of + * 'BSD 3-clause "New" or "Revised" License', in which case the following + * provisions apply instead of the ones mentioned above : + * + * ******************************************************************************* + * + * License terms: BSD 3-clause "New" or "Revised" License. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_VL53L1X) + +#include "build/build_config.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "drivers/rangefinder/rangefinder.h" +#include "drivers/rangefinder/rangefinder_vl53l1x.h" + +#include "build/debug.h" + +#define VL53L1X_MAX_RANGE_CM (300) +#define VL53L1X_DETECTION_CONE_DECIDEGREES (270) + +#define VL53L1X_IMPLEMENTATION_VER_MAJOR 3 +#define VL53L1X_IMPLEMENTATION_VER_MINOR 4 +#define VL53L1X_IMPLEMENTATION_VER_SUB 0 +#define VL53L1X_IMPLEMENTATION_VER_REVISION 0000 + +typedef int8_t VL53L1X_ERROR; + +#define SOFT_RESET 0x0000 +#define VL53L1_I2C_SLAVE__DEVICE_ADDRESS 0x0001 +#define VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND 0x0008 +#define ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS 0x0016 +#define ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS 0x0018 +#define ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS 0x001A +#define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E +#define MM_CONFIG__INNER_OFFSET_MM 0x0020 +#define MM_CONFIG__OUTER_OFFSET_MM 0x0022 +#define GPIO_HV_MUX__CTRL 0x0030 +#define GPIO__TIO_HV_STATUS 0x0031 +#define SYSTEM__INTERRUPT_CONFIG_GPIO 0x0046 +#define PHASECAL_CONFIG__TIMEOUT_MACROP 0x004B +#define RANGE_CONFIG__TIMEOUT_MACROP_A_HI 0x005E +#define RANGE_CONFIG__VCSEL_PERIOD_A 0x0060 +#define RANGE_CONFIG__VCSEL_PERIOD_B 0x0063 +#define RANGE_CONFIG__TIMEOUT_MACROP_B_HI 0x0061 +#define RANGE_CONFIG__TIMEOUT_MACROP_B_LO 0x0062 +#define RANGE_CONFIG__SIGMA_THRESH 0x0064 +#define RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS 0x0066 +#define RANGE_CONFIG__VALID_PHASE_HIGH 0x0069 +#define VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD 0x006C +#define SYSTEM__THRESH_HIGH 0x0072 +#define SYSTEM__THRESH_LOW 0x0074 +#define SD_CONFIG__WOI_SD0 0x0078 +#define SD_CONFIG__INITIAL_PHASE_SD0 0x007A +#define ROI_CONFIG__USER_ROI_CENTRE_SPAD 0x007F +#define ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE 0x0080 +#define SYSTEM__SEQUENCE_CONFIG 0x0081 +#define VL53L1_SYSTEM__GROUPED_PARAMETER_HOLD 0x0082 +#define SYSTEM__INTERRUPT_CLEAR 0x0086 +#define SYSTEM__MODE_START 0x0087 +#define VL53L1_RESULT__RANGE_STATUS 0x0089 +#define VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0 0x008C +#define RESULT__AMBIENT_COUNT_RATE_MCPS_SD 0x0090 +#define VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0 0x0096 +#define VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0 0x0098 +#define VL53L1_RESULT__OSC_CALIBRATE_VAL 0x00DE +#define VL53L1_FIRMWARE__SYSTEM_STATUS 0x00E5 +#define VL53L1_IDENTIFICATION__MODEL_ID 0x010F +#define VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD 0x013E + +#define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E +#define MM_CONFIG__INNER_OFFSET_MM 0x0020 +#define MM_CONFIG__OUTER_OFFSET_MM 0x0022 + + + +#define VL53L1_ERROR_NONE ((VL53L1X_ERROR) 0) +#define VL53L1_ERROR_CALIBRATION_WARNING ((VL53L1X_ERROR) - 1) + /*!< Warning invalid calibration data may be in used + \a VL53L1_InitData() + \a VL53L1_GetOffsetCalibrationData + \a VL53L1_SetOffsetCalibrationData */ +#define VL53L1_ERROR_MIN_CLIPPED ((VL53L1X_ERROR) - 2) + /*!< Warning parameter passed was clipped to min before to be applied */ + +#define VL53L1_ERROR_UNDEFINED ((VL53L1X_ERROR) - 3) + /*!< Unqualified error */ +#define VL53L1_ERROR_INVALID_PARAMS ((VL53L1X_ERROR) - 4) + /*!< Parameter passed is invalid or out of range */ +#define VL53L1_ERROR_NOT_SUPPORTED ((VL53L1X_ERROR) - 5) + /*!< Function is not supported in current mode or configuration */ +#define VL53L1_ERROR_RANGE_ERROR ((VL53L1X_ERROR) - 6) + /*!< Device report a ranging error interrupt status */ +#define VL53L1_ERROR_TIME_OUT ((VL53L1X_ERROR) - 7) + /*!< Aborted due to time out */ +#define VL53L1_ERROR_MODE_NOT_SUPPORTED ((VL53L1X_ERROR) - 8) + /*!< Asked mode is not supported by the device */ +#define VL53L1_ERROR_BUFFER_TOO_SMALL ((VL53L1X_ERROR) - 9) + /*!< ... */ +#define VL53L1_ERROR_COMMS_BUFFER_TOO_SMALL ((VL53L1X_ERROR) - 10) + /*!< Supplied buffer is larger than I2C supports */ +#define VL53L1_ERROR_GPIO_NOT_EXISTING ((VL53L1X_ERROR) - 11) + /*!< User tried to setup a non-existing GPIO pin */ +#define VL53L1_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ((VL53L1X_ERROR) - 12) + /*!< unsupported GPIO functionality */ +#define VL53L1_ERROR_CONTROL_INTERFACE ((VL53L1X_ERROR) - 13) + /*!< error reported from IO functions */ +#define VL53L1_ERROR_INVALID_COMMAND ((VL53L1X_ERROR) - 14) + /*!< The command is not allowed in the current device state + * (power down) */ +#define VL53L1_ERROR_DIVISION_BY_ZERO ((VL53L1X_ERROR) - 15) + /*!< In the function a division by zero occurs */ +#define VL53L1_ERROR_REF_SPAD_INIT ((VL53L1X_ERROR) - 16) + /*!< Error during reference SPAD initialization */ +#define VL53L1_ERROR_GPH_SYNC_CHECK_FAIL ((VL53L1X_ERROR) - 17) + /*!< GPH sync interrupt check fail - API out of sync with device*/ +#define VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL ((VL53L1X_ERROR) - 18) + /*!< Stream count check fail - API out of sync with device */ +#define VL53L1_ERROR_GPH_ID_CHECK_FAIL ((VL53L1X_ERROR) - 19) + /*!< GPH ID check fail - API out of sync with device */ +#define VL53L1_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL ((VL53L1X_ERROR) - 20) + /*!< Zone dynamic config stream count check failed - API out of sync */ +#define VL53L1_ERROR_ZONE_GPH_ID_CHECK_FAIL ((VL53L1X_ERROR) - 21) + /*!< Zone dynamic config GPH ID check failed - API out of sync */ + +#define VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 22) + /*!< Thrown when run_xtalk_extraction fn has 0 succesful samples + * when using the full array to sample the xtalk. In this case there is + * not enough information to generate new Xtalk parm info. The function + * will exit and leave the current xtalk parameters unaltered */ +#define VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL ((VL53L1X_ERROR) - 23) + /*!< Thrown when run_xtalk_extraction fn has found that the + * avg sigma estimate of the full array xtalk sample is > than the + * maximal limit allowed. In this case the xtalk sample is too noisy for + * measurement. The function will exit and leave the current xtalk parameters + * unaltered. */ + + +#define VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 24) + /*!< Thrown if there one of stages has no valid offset calibration + * samples. A fatal error calibration not valid */ +#define VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL ((VL53L1X_ERROR) - 25) + /*!< Thrown if there one of stages has zero effective SPADS + * Traps the case when MM1 SPADs is zero. + * A fatal error calibration not valid */ +#define VL53L1_ERROR_ZONE_CAL_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 26) + /*!< Thrown if then some of the zones have no valid samples + * A fatal error calibration not valid */ + +#define VL53L1_ERROR_TUNING_PARM_KEY_MISMATCH ((VL53L1X_ERROR) - 27) + /*!< Thrown if the tuning file key table version does not match with + * expected value. The driver expects the key table version to match + * the compiled default version number in the define + * #VL53L1_TUNINGPARM_KEY_TABLE_VERSION_DEFAULT + * */ + +#define VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS ((VL53L1X_ERROR) - 28) + /*!< Thrown if there are less than 5 good SPADs are available. */ +#define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH ((VL53L1X_ERROR) - 29) + /*!< Thrown if the final reference rate is greater than + the upper reference rate limit - default is 40 Mcps. + Implies a minimum Q3 (x10) SPAD (5) selected */ +#define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW ((VL53L1X_ERROR) - 30) + /*!< Thrown if the final reference rate is less than + the lower reference rate limit - default is 10 Mcps. + Implies maximum Q1 (x1) SPADs selected */ + + +#define VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES ((VL53L1X_ERROR) - 31) + /*!< Thrown if there is less than the requested number of + * valid samples. */ +#define VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH ((VL53L1X_ERROR) - 32) + /*!< Thrown if the offset calibration range sigma estimate is greater + * than 8.0 mm. This is the recommended min value to yield a stable + * offset measurement */ +#define VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH ((VL53L1X_ERROR) - 33) + /*!< Thrown when VL53L1_run_offset_calibration() peak rate is greater + than that 50.0Mcps. This is the recommended max rate to avoid + pile-up influencing the offset measurement */ +#define VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW ((VL53L1X_ERROR) - 34) + /*!< Thrown when VL53L1_run_offset_calibration() when one of stages + range has less that 5.0 effective SPADS. This is the recommended + min value to yield a stable offset */ + + +#define VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES ((VL53L1X_ERROR) - 35) + /*!< Thrown if one of more of the zones have less than + the requested number of valid samples */ +#define VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH ((VL53L1X_ERROR) - 36) + /*!< Thrown if one or more zones have sigma estimate value greater + * than 8.0 mm. This is the recommended min value to yield a stable + * offset measurement */ +#define VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH ((VL53L1X_ERROR) - 37) + /*!< Thrown if one of more zones have peak rate higher than + that 50.0Mcps. This is the recommended max rate to avoid + pile-up influencing the offset measurement */ + + +#define VL53L1_WARNING_XTALK_MISSING_SAMPLES ((VL53L1X_ERROR) - 38) + /*!< Thrown to notify that some of the xtalk samples did not yield + * valid ranging pulse data while attempting to measure + * the xtalk signal in vl53l1_run_xtalk_extract(). This can signify any of + * the zones are missing samples, for further debug information the + * xtalk_results struct should be referred to. This warning is for + * notification only, the xtalk pulse and shape have still been generated + */ +#define VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT ((VL53L1X_ERROR) - 39) + /*!< Thrown to notify that some of teh xtalk samples used for gradient + * generation did not yield valid ranging pulse data while attempting to + * measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify + * that any one of the zones 0-3 yielded no successful samples. The + * xtalk_results struct should be referred to for further debug info. + * This warning is for notification only, the xtalk pulse and shape + * have still been generated. + */ +#define VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT ((VL53L1X_ERROR) - 40) +/*!< Thrown to notify that some of the xtalk samples used for gradient + * generation did not pass the sigma limit check while attempting to + * measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify + * that any one of the zones 0-3 yielded an avg sigma_mm value > the limit. + * The xtalk_results struct should be referred to for further debug info. + * This warning is for notification only, the xtalk pulse and shape + * have still been generated. + */ + +#define VL53L1_ERROR_NOT_IMPLEMENTED ((VL53L1X_ERROR) - 41) + /*!< Tells requested functionality has not been implemented yet or + * not compatible with the device */ +#define VL53L1_ERROR_PLATFORM_SPECIFIC_START ((VL53L1X_ERROR) - 60) + /*!< Tells the starting code for platform */ +/** @} VL53L1_define_Error_group */ + +/**************************************** + * PRIVATE define do not edit + ****************************************/ + +/** + * @brief defines SW Version + */ +typedef struct { + uint8_t major; /*!< major number */ + uint8_t minor; /*!< minor number */ + uint8_t build; /*!< build number */ + uint32_t revision; /*!< revision number */ +} VL53L1X_Version_t; + +/** + * @brief defines packed reading results type + */ +typedef struct { + uint8_t Status; /*!< ResultStatus */ + uint16_t Distance; /*!< ResultDistance */ + uint16_t Ambient; /*!< ResultAmbient */ + uint16_t SigPerSPAD;/*!< ResultSignalPerSPAD */ + uint16_t NumSPADs; /*!< ResultNumSPADs */ +} VL53L1X_Result_t; + +/** + * @brief This function returns the SW driver version + */ +VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion); + +/** + * @brief This function sets the sensor I2C address used in case multiple devices application, default address 0x52 + */ +VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address); + +/** + * @brief This function loads the 135 bytes default values to initialize the sensor. + * @param dev Device address + * @return 0:success, != 0:failed + */ +VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev); + +/** + * @brief This function clears the interrupt, to be called after a ranging data reading + * to arm the interrupt for the next data ready event. + */ +VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev); + +/** + * @brief This function programs the interrupt polarity\n + * 1=active high (default), 0=active low + */ +VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t IntPol); + +/** + * @brief This function returns the current interrupt polarity\n + * 1=active high (default), 0=active low + */ +VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pIntPol); + +/** + * @brief This function starts the ranging distance operation\n + * The ranging operation is continuous. The clear interrupt has to be done after each get data to allow the interrupt to raise when the next data is ready\n + * 1=active high (default), 0=active low, use SetInterruptPolarity() to change the interrupt polarity if required. + */ +VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev); + +/** + * @brief This function stops the ranging. + */ +VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev); + +/** + * @brief This function checks if the new ranging data is available by polling the dedicated register. + * @param : isDataReady==0 -> not ready; isDataReady==1 -> ready + */ +VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady); + +/** + * @brief This function programs the timing budget in ms. + * Predefined values = 15, 20, 33, 50, 100(default), 200, 500. + */ +VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs); + +/** + * @brief This function returns the current timing budget in ms. + */ +VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudgetInMs); + +/** + * @brief This function programs the distance mode (1=short, 2=long(default)). + * Short mode max distance is limited to 1.3 m but better ambient immunity.\n + * Long mode can range up to 4 m in the dark with 200 ms timing budget. + */ +VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DistanceMode); + +/** + * @brief This function returns the current distance mode (1=short, 2=long). + */ +VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *pDistanceMode); + +/** + * @brief This function programs the Intermeasurement period in ms\n + * Intermeasurement period must be >/= timing budget. This condition is not checked by the API, + * the customer has the duty to check the condition. Default = 100 ms + */ +VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, + uint32_t InterMeasurementInMs); + +/** + * @brief This function returns the Intermeasurement period in ms. + */ +VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t * pIM); + +/** + * @brief This function returns the boot state of the device (1:booted, 0:not booted) + */ +VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state); + +/** + * @brief This function returns the sensor id, sensor Id must be 0xEEAC + */ +VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *id); + +/** + * @brief This function returns the distance measured by the sensor in mm + */ +VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); + +/** + * @brief This function returns the returned signal per SPAD in kcps/SPAD. + * With kcps stands for Kilo Count Per Second + */ +VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalPerSp); + +/** + * @brief This function returns the ambient per SPAD in kcps/SPAD + */ +VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *amb); + +/** + * @brief This function returns the returned signal in kcps. + */ +VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signalRate); + +/** + * @brief This function returns the current number of enabled SPADs + */ +VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb); + +/** + * @brief This function returns the ambient rate in kcps + */ +VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate); + +/** + * @brief This function returns the ranging status error \n + * (0:no error, 1:sigma failed, 2:signal failed, ..., 7:wrap-around) + */ +VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus); + +/** + * @brief This function returns measurements and the range status in a single read access + */ +VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult); + +/** + * @brief This function programs the offset correction in mm + * @param OffsetValue:the offset correction value to program in mm + */ +VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue); + +/** + * @brief This function returns the programmed offset correction value in mm + */ +VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *Offset); + +/** + * @brief This function programs the xtalk correction value in cps (Count Per Second).\n + * This is the number of photons reflected back from the cover glass in cps. + */ +VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue); + +/** + * @brief This function returns the current programmed xtalk correction value in cps + */ +VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); + +/** + * @brief This function programs the threshold detection mode\n + * Example:\n + * VL53L1X_SetDistanceThreshold(dev,100,300,0,1): Below 100 \n + * VL53L1X_SetDistanceThreshold(dev,100,300,1,1): Above 300 \n + * VL53L1X_SetDistanceThreshold(dev,100,300,2,1): Out of window \n + * VL53L1X_SetDistanceThreshold(dev,100,300,3,1): In window \n + * @param dev : device address + * @param ThreshLow(in mm) : the threshold under which one the device raises an interrupt if Window = 0 + * @param ThreshHigh(in mm) : the threshold above which one the device raises an interrupt if Window = 1 + * @param Window detection mode : 0=below, 1=above, 2=out, 3=in + * @param IntOnNoTarget = 0 (No longer used - just use 0) + */ +VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, + uint16_t ThreshHigh, uint8_t Window, + uint8_t IntOnNoTarget); + +/** + * @brief This function returns the window detection mode (0=below; 1=above; 2=out; 3=in) + */ +VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window); + +/** + * @brief This function returns the low threshold in mm + */ +VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low); + +/** + * @brief This function returns the high threshold in mm + */ +VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high); + +/** + * @brief This function programs the ROI (Region of Interest)\n + * The ROI position is centered, only the ROI size can be reprogrammed.\n + * The smallest acceptable ROI size = 4\n + * @param X:ROI Width; Y=ROI Height + */ +VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y); + +/** + *@brief This function returns width X and height Y + */ +VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y); + +/** + *@brief This function programs the new user ROI center, please to be aware that there is no check in this function. + *if the ROI center vs ROI size is out of border the ranging function return error #13 + */ +VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter); + +/** + *@brief This function returns the current user ROI center + */ +VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter); + +/** + * @brief This function programs a new signal threshold in kcps (default=1024 kcps\n + */ +VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t signal); + +/** + * @brief This function returns the current signal threshold in kcps + */ +VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal); + +/** + * @brief This function programs a new sigma threshold in mm (default=15 mm) + */ +VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t sigma); + +/** + * @brief This function returns the current sigma threshold in mm + */ +VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *signal); + +/** + * @brief This function performs the temperature calibration. + * It is recommended to call this function any time the temperature might have changed by more than 8 deg C + * without sensor ranging activity for an extended period. + */ +VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); + +/** + * @brief This function performs the offset calibration.\n + * The function returns the offset value found and programs the offset compensation into the device. + * @param TargetDistInMm target distance in mm, ST recommended 100 mm + * Target reflectance = grey17% + * @return 0:success, !=0: failed + * @return offset pointer contains the offset found in mm + */ +int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset); + +/** + * @brief This function performs the xtalk calibration.\n + * The function returns the xtalk value found and programs the xtalk compensation to the device + * @param TargetDistInMm target distance in mm\n + * The target distance : the distance where the sensor start to "under range"\n + * due to the influence of the photons reflected back from the cover glass becoming strong\n + * It's also called inflection point\n + * Target reflectance = grey 17% + * @return 0: success, !=0: failed + * @return xtalk pointer contains the xtalk value found in cps (number of photons in count per second) + */ +int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk); + + + +const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = { +0x00, /* 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */ +0x00, /* 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */ +0x00, /* 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */ +0x01, /* 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */ +0x02, /* 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */ +0x00, /* 0x32 : not user-modifiable */ +0x02, /* 0x33 : not user-modifiable */ +0x08, /* 0x34 : not user-modifiable */ +0x00, /* 0x35 : not user-modifiable */ +0x08, /* 0x36 : not user-modifiable */ +0x10, /* 0x37 : not user-modifiable */ +0x01, /* 0x38 : not user-modifiable */ +0x01, /* 0x39 : not user-modifiable */ +0x00, /* 0x3a : not user-modifiable */ +0x00, /* 0x3b : not user-modifiable */ +0x00, /* 0x3c : not user-modifiable */ +0x00, /* 0x3d : not user-modifiable */ +0xff, /* 0x3e : not user-modifiable */ +0x00, /* 0x3f : not user-modifiable */ +0x0F, /* 0x40 : not user-modifiable */ +0x00, /* 0x41 : not user-modifiable */ +0x00, /* 0x42 : not user-modifiable */ +0x00, /* 0x43 : not user-modifiable */ +0x00, /* 0x44 : not user-modifiable */ +0x00, /* 0x45 : not user-modifiable */ +0x20, /* 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */ +0x0b, /* 0x47 : not user-modifiable */ +0x00, /* 0x48 : not user-modifiable */ +0x00, /* 0x49 : not user-modifiable */ +0x02, /* 0x4a : not user-modifiable */ +0x0a, /* 0x4b : not user-modifiable */ +0x21, /* 0x4c : not user-modifiable */ +0x00, /* 0x4d : not user-modifiable */ +0x00, /* 0x4e : not user-modifiable */ +0x05, /* 0x4f : not user-modifiable */ +0x00, /* 0x50 : not user-modifiable */ +0x00, /* 0x51 : not user-modifiable */ +0x00, /* 0x52 : not user-modifiable */ +0x00, /* 0x53 : not user-modifiable */ +0xc8, /* 0x54 : not user-modifiable */ +0x00, /* 0x55 : not user-modifiable */ +0x00, /* 0x56 : not user-modifiable */ +0x38, /* 0x57 : not user-modifiable */ +0xff, /* 0x58 : not user-modifiable */ +0x01, /* 0x59 : not user-modifiable */ +0x00, /* 0x5a : not user-modifiable */ +0x08, /* 0x5b : not user-modifiable */ +0x00, /* 0x5c : not user-modifiable */ +0x00, /* 0x5d : not user-modifiable */ +0x01, /* 0x5e : not user-modifiable */ +0xcc, /* 0x5f : not user-modifiable */ +0x0f, /* 0x60 : not user-modifiable */ +0x01, /* 0x61 : not user-modifiable */ +0xf1, /* 0x62 : not user-modifiable */ +0x0d, /* 0x63 : not user-modifiable */ +0x01, /* 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm */ +0x68, /* 0x65 : Sigma threshold LSB */ +0x00, /* 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */ +0x80, /* 0x67 : Min count Rate LSB */ +0x08, /* 0x68 : not user-modifiable */ +0xb8, /* 0x69 : not user-modifiable */ +0x00, /* 0x6a : not user-modifiable */ +0x00, /* 0x6b : not user-modifiable */ +0x00, /* 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */ +0x00, /* 0x6d : Intermeasurement period */ +0x0f, /* 0x6e : Intermeasurement period */ +0x89, /* 0x6f : Intermeasurement period LSB */ +0x00, /* 0x70 : not user-modifiable */ +0x00, /* 0x71 : not user-modifiable */ +0x00, /* 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */ +0x00, /* 0x73 : distance threshold high LSB */ +0x00, /* 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */ +0x00, /* 0x75 : distance threshold low LSB */ +0x00, /* 0x76 : not user-modifiable */ +0x01, /* 0x77 : not user-modifiable */ +0x0f, /* 0x78 : not user-modifiable */ +0x0d, /* 0x79 : not user-modifiable */ +0x0e, /* 0x7a : not user-modifiable */ +0x0e, /* 0x7b : not user-modifiable */ +0x00, /* 0x7c : not user-modifiable */ +0x00, /* 0x7d : not user-modifiable */ +0x02, /* 0x7e : not user-modifiable */ +0xc7, /* 0x7f : ROI center, use SetROI() */ +0xff, /* 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */ +0x9B, /* 0x81 : not user-modifiable */ +0x00, /* 0x82 : not user-modifiable */ +0x00, /* 0x83 : not user-modifiable */ +0x00, /* 0x84 : not user-modifiable */ +0x01, /* 0x85 : not user-modifiable */ +0x00, /* 0x86 : clear interrupt, use ClearInterrupt() */ +0x00 /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */ +}; + +static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0, + 255, 255, 9, 13, 255, 255, 255, 255, 10, 6, + 255, 255, 11, 12 +}; + +static uint8_t _I2CBuffer[256]; + +static int32_t lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE; +static bool lastMeasurementIsNew = false; +static bool isInitialized = false; +static bool isResponding = true; + +#define _I2CWrite(dev, data, size) \ + (busWriteBuf(dev, 0xFF, data, size) ? 0 : -1) + +#define _I2CRead(dev, data, size) \ + (busReadBuf(dev, 0xFF, data, size) ? 0 : -1) + +VL53L1X_ERROR VL53L1_WriteMulti(busDevice_t * Dev, uint16_t index, uint8_t *pdata, uint32_t count) { + int status_int; + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + if (count > sizeof(_I2CBuffer) - 1) { + return VL53L1_ERROR_INVALID_PARAMS; + } + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + memcpy(&_I2CBuffer[2], pdata, count); + status_int = _I2CWrite(Dev, _I2CBuffer, count + 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +// the ranging_sensor_comms.dll will take care of the page selection +VL53L1X_ERROR VL53L1_ReadMulti(busDevice_t * Dev, uint16_t index, uint8_t *pdata, uint32_t count) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, pdata, count); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } +done: + return Status; +} + +VL53L1X_ERROR VL53L1_WrByte(busDevice_t * Dev, uint16_t index, uint8_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = data; + + status_int = _I2CWrite(Dev, _I2CBuffer, 3); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_WrWord(busDevice_t * Dev, uint16_t index, uint16_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = data >> 8; + _I2CBuffer[3] = data & 0x00FF; + + status_int = _I2CWrite(Dev, _I2CBuffer, 4); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_WrDWord(busDevice_t * Dev, uint16_t index, uint32_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = (data >> 24) & 0xFF; + _I2CBuffer[3] = (data >> 16) & 0xFF; + _I2CBuffer[4] = (data >> 8) & 0xFF; + _I2CBuffer[5] = (data >> 0 ) & 0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 6); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_RdByte(busDevice_t * Dev, uint16_t index, uint8_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if( status_int ){ + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, data, 1); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } +done: + return Status; +} + +VL53L1X_ERROR VL53L1_UpdateByte(busDevice_t * Dev, uint16_t index, uint8_t AndData, uint8_t OrData) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + uint8_t data; + + Status = VL53L1_RdByte(Dev, index, &data); + if (Status) { + goto done; + } + data = (data & AndData) | OrData; + Status = VL53L1_WrByte(Dev, index, data); +done: + return Status; +} + +VL53L1X_ERROR VL53L1_RdWord(busDevice_t * Dev, uint16_t index, uint16_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + + if( status_int ){ + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + + *data = ((uint16_t)_I2CBuffer[0]<<8) + (uint16_t)_I2CBuffer[1]; +done: + return Status; +} + +VL53L1X_ERROR VL53L1_RdDWord(busDevice_t * Dev, uint16_t index, uint32_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, _I2CBuffer, 4); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + + *data = ((uint32_t)_I2CBuffer[0]<<24) + ((uint32_t)_I2CBuffer[1]<<16) + ((uint32_t)_I2CBuffer[2]<<8) + (uint32_t)_I2CBuffer[3]; + +done: + return Status; +} + +VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) +{ + VL53L1X_ERROR Status = 0; + + pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR; + pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR; + pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB; + pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION; + return Status; +} + +VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1); + return status; +} + +VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + uint8_t Addr = 0x00, tmp; + + for (Addr = 0x2D; Addr <= 0x87; Addr++){ + status = VL53L1_WrByte(dev, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]); + } + status = VL53L1X_StartRanging(dev); + tmp = 0; + while(tmp==0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + status = VL53L1X_ClearInterrupt(dev); + status = VL53L1X_StopRanging(dev); + status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ + status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ + return status; +} + +VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CLEAR, 0x01); + return status; +} + +VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t NewPolarity) +{ + uint8_t Temp; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); + Temp = Temp & 0xEF; + status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4); + return status; +} + +VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterruptPolarity) +{ + uint8_t Temp; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); + Temp = Temp & 0x10; + *pInterruptPolarity = !(Temp>>4); + return status; +} + +VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x40); /* Enable VL53L1X */ + return status; +} + +VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */ + return status; +} + +VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) +{ + uint8_t Temp; + uint8_t IntPol; + VL53L1X_ERROR status = 0; + + status = VL53L1X_GetInterruptPolarity(dev, &IntPol); + status = VL53L1_RdByte(dev, GPIO__TIO_HV_STATUS, &Temp); + /* Read in the register to check if a new value is available */ + if (status == 0){ + if ((Temp & 1) == IntPol) + *isDataReady = 1; + else + *isDataReady = 0; + } + return status; +} + +VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs) +{ + uint16_t DM; + VL53L1X_ERROR status=0; + + status = VL53L1X_GetDistanceMode(dev, &DM); + if (DM == 0) + return 1; + else if (DM == 1) { /* Short DistanceMode */ + switch (TimingBudgetInMs) { + case 15: /* only available in short distance mode */ + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x01D); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0027); + break; + case 20: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0051); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 33: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x00D6); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 50: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x1AE); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x01E8); + break; + case 100: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x02E1); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0388); + break; + case 200: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x03E1); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0496); + break; + case 500: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0591); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x05C1); + break; + default: + status = 1; + break; + } + } else { + switch (TimingBudgetInMs) { + case 20: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x001E); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0022); + break; + case 33: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0060); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 50: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x00AD); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x00C6); + break; + case 100: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x01CC); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x01EA); + break; + case 200: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x02D9); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x02F8); + break; + case 500: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x048F); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x04A4); + break; + default: + status = 1; + break; + } + } + return status; +} + +VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudget) +{ + uint16_t Temp; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp); + switch (Temp) { + case 0x001D : + *pTimingBudget = 15; + break; + case 0x0051 : + case 0x001E : + *pTimingBudget = 20; + break; + case 0x00D6 : + case 0x0060 : + *pTimingBudget = 33; + break; + case 0x1AE : + case 0x00AD : + *pTimingBudget = 50; + break; + case 0x02E1 : + case 0x01CC : + *pTimingBudget = 100; + break; + case 0x03E1 : + case 0x02D9 : + *pTimingBudget = 200; + break; + case 0x0591 : + case 0x048F : + *pTimingBudget = 500; + break; + default: + status = 1; + *pTimingBudget = 0; + } + return status; +} + +VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) +{ + uint16_t TB; + VL53L1X_ERROR status = 0; + + status = VL53L1X_GetTimingBudgetInMs(dev, &TB); + if (status != 0) + return 1; + switch (DM) { + case 1: + status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38); + status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0705); + status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606); + break; + case 2: + status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8); + status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0F0D); + status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E); + break; + default: + status = 1; + break; + } + + if (status == 0) + status = VL53L1X_SetTimingBudgetInMs(dev, TB); + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) +{ + uint8_t TempDM, status=0; + + status = VL53L1_RdByte(dev,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM); + if (TempDM == 0x14) + *DM=1; + if(TempDM == 0x0A) + *DM=2; + return status; +} + +VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasMs) +{ + uint16_t ClockPLL; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); + ClockPLL = ClockPLL&0x3FF; + VL53L1_WrDWord(dev, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, + (uint32_t)(ClockPLL * InterMeasMs * 1.075)); + return status; + +} + +VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t *pIM) +{ + uint16_t ClockPLL; + VL53L1X_ERROR status = 0; + uint32_t tmp; + + status = VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp); + *pIM = (uint16_t)tmp; + status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); + ClockPLL = ClockPLL&0x3FF; + *pIM= (uint16_t)(*pIM/(ClockPLL*1.065)); + return status; +} + +VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp = 0; + + status = VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp); + *state = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *sensorId) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp = 0; + + status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp); + *sensorId = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = (VL53L1_RdWord(dev, + VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp)); + *distance = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate) +{ + VL53L1X_ERROR status = 0; + uint16_t SpNb=1, signal; + + status = VL53L1_RdWord(dev, + VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal); + status = VL53L1_RdWord(dev, + VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); + *signalRate = (uint16_t) (200.0*signal/SpNb); + return status; +} + +VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *ambPerSp) +{ + VL53L1X_ERROR status = 0; + uint16_t AmbientRate, SpNb = 1; + + status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate); + status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); + *ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb); + return status; +} + +VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signal) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev, + VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp); + *signal = tmp*8; + return status; +} + +VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev, + VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp); + *spNb = tmp >> 8; + return status; +} + +VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp); + *ambRate = tmp*8; + return status; +} + +VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus) +{ + VL53L1X_ERROR status = 0; + uint8_t RgSt; + + *rangeStatus = 255; + status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt); + RgSt = RgSt & 0x1F; + if (RgSt < 24) + *rangeStatus = status_rtn[RgSt]; + return status; +} + +VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult) +{ + VL53L1X_ERROR status = 0; + uint8_t Temp[17]; + uint8_t RgSt = 255; + + status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17); + RgSt = Temp[0] & 0x1F; + if (RgSt < 24) + RgSt = status_rtn[RgSt]; + pResult->Status = RgSt; + pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8; + pResult->NumSPADs = Temp[3]; + pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8; + pResult->Distance = Temp[13] << 8 | Temp[14]; + + return status; +} + +VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue) +{ + VL53L1X_ERROR status = 0; + int16_t Temp; + + Temp = (OffsetValue*4); + VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, + (uint16_t)Temp); + VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); + VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); + return status; +} + +VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *offset) +{ + VL53L1X_ERROR status = 0; + uint16_t Temp; + + status = VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp); + Temp = Temp<<3; + Temp = Temp>>5; + *offset = (int16_t)(Temp); + return status; +} + +VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue) +{ +/* XTalkValue in count per second to avoid float type */ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrWord(dev, + ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS, + 0x0000); + status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS, + 0x0000); + status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, + (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */ + return status; +} + +VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *xtalk ) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk); + *xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */ + return status; +} + +VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, + uint16_t ThreshHigh, uint8_t Window, + uint8_t IntOnNoTarget) +{ + VL53L1X_ERROR status = 0; + uint8_t Temp = 0; + + status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp); + Temp = Temp & 0x47; + if (IntOnNoTarget == 0) { + status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, + (Temp | (Window & 0x07))); + } else { + status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, + ((Temp | (Window & 0x07)) | 0x40)); + } + status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh); + status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow); + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp; + status = VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp); + *window = (uint16_t)(tmp & 0x7); + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp); + *low = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp); + *high = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter) +{ + VL53L1X_ERROR status = 0; + status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter); + return status; +} + +VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp; + status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp); + *ROICenter = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y) +{ + uint8_t OpticalCenter; + VL53L1X_ERROR status = 0; + + status =VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter); + if (X > 16) + X = 16; + if (Y > 16) + Y = 16; + if (X > 10 || Y > 10){ + OpticalCenter = 199; + } + status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter); + status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, + (Y - 1) << 4 | (X - 1)); + return status; +} + +VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp; + + status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp); + *ROI_X = ((uint16_t)tmp & 0x0F) + 1; + *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1; + return status; +} + +VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t Signal) +{ + VL53L1X_ERROR status = 0; + + VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3); + return status; +} + +VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev, + RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp); + *signal = tmp <<3; + return status; +} + +VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t Sigma) +{ + VL53L1X_ERROR status = 0; + + if(Sigma>(0xFFFF>>2)){ + return 1; + } + /* 16 bits register 14.2 format */ + status = VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2); + return status; +} + +VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *sigma) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp); + *sigma = tmp >> 2; + return status; + +} + +VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp=0; + + status = VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */ + status = VL53L1_WrByte(dev,0x0B,0x92); + status = VL53L1X_StartRanging(dev); + while(tmp==0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + tmp = 0; + status = VL53L1X_ClearInterrupt(dev); + status = VL53L1X_StopRanging(dev); + status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ + status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ + return status; +} + + +int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset) +{ + uint8_t i, tmp; + int16_t AverageDistance = 0; + uint16_t distance; + VL53L1X_ERROR status = 0; + + status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0); + status = VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); + status = VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); + status = VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */ + for (i = 0; i < 50; i++) { + tmp = 0; + while (tmp == 0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + status = VL53L1X_GetDistance(dev, &distance); + status = VL53L1X_ClearInterrupt(dev); + AverageDistance = AverageDistance + distance; + } + status = VL53L1X_StopRanging(dev); + AverageDistance = AverageDistance / 50; + *offset = TargetDistInMm - AverageDistance; + status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4); + return status; +} + +int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk) +{ + uint8_t i, tmp; + float AverageSignalRate = 0; + float AverageDistance = 0; + float AverageSpadNb = 0; + uint16_t distance = 0, spadNum; + uint16_t sr; + VL53L1X_ERROR status = 0; + uint32_t calXtalk; + + status = VL53L1_WrWord(dev, 0x0016,0); + status = VL53L1X_StartRanging(dev); + for (i = 0; i < 50; i++) { + tmp = 0; + while (tmp == 0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + status= VL53L1X_GetSignalRate(dev, &sr); + status= VL53L1X_GetDistance(dev, &distance); + status = VL53L1X_ClearInterrupt(dev); + AverageDistance = AverageDistance + distance; + status = VL53L1X_GetSpadNb(dev, &spadNum); + AverageSpadNb = AverageSpadNb + spadNum; + AverageSignalRate = + AverageSignalRate + sr; + } + status = VL53L1X_StopRanging(dev); + AverageDistance = AverageDistance / 50; + AverageSpadNb = AverageSpadNb / 50; + AverageSignalRate = AverageSignalRate / 50; + /* Calculate Xtalk value */ + calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb); + *xtalk = (uint16_t)((calXtalk*1000)>>9); + status = VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk); + return status; +} + +static void vl53l1x_Init(rangefinderDev_t * rangefinder) +{ + isInitialized = false; + VL53L1X_SensorInit(rangefinder->busDev); + VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ + VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */ + VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, 33); /* in ms, IM must be > = TB */ + VL53L1X_StartRanging(rangefinder->busDev); + isInitialized = true; +} + +void vl53l1x_Update(rangefinderDev_t * rangefinder) +{ + uint16_t Distance; + uint8_t dataReady; + + if (!isInitialized) { + return; + } + + VL53L1X_CheckForDataReady(rangefinder->busDev, &dataReady); + if (dataReady != 0) { + VL53L1X_GetDistance(rangefinder->busDev, &Distance); + lastMeasurementCm = Distance / 10; + lastMeasurementIsNew = true; + } + VL53L1X_ClearInterrupt(rangefinder->busDev); +} + +int32_t vl53l1x_GetDistance(rangefinderDev_t *dev) +{ + UNUSED(dev); + + if (isResponding && isInitialized) { + if (lastMeasurementIsNew) { + lastMeasurementIsNew = false; + return lastMeasurementCm < VL53L1X_MAX_RANGE_CM ? lastMeasurementCm : RANGEFINDER_OUT_OF_RANGE; + } + else { + return RANGEFINDER_NO_NEW_DATA; + } + } + else { + return RANGEFINDER_HARDWARE_FAILURE; + } +} + +static bool deviceDetect(busDevice_t * busDev) +{ + for (int retry = 0; retry < 5; retry++) { + uint8_t model_id; + + delay(150); + + VL53L1X_ERROR err = VL53L1_RdByte(busDev, 0x010F, &model_id); + if (err == 0 && model_id == 0xEA) { + return true; + } + }; + + return false; +} + +bool vl53l1xDetect(rangefinderDev_t * rangefinder) +{ + rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_VL53L1X, 0, OWNER_RANGEFINDER); + if (rangefinder->busDev == NULL) { + return false; + } + + if (!deviceDetect(rangefinder->busDev)) { + busDeviceDeInit(rangefinder->busDev); + return false; + } + + rangefinder->delayMs = RANGEFINDER_VL53L1X_TASK_PERIOD_MS; + rangefinder->maxRangeCm = VL53L1X_MAX_RANGE_CM; + rangefinder->detectionConeDeciDegrees = VL53L1X_DETECTION_CONE_DECIDEGREES; + rangefinder->detectionConeExtendedDeciDegrees = VL53L1X_DETECTION_CONE_DECIDEGREES; + + rangefinder->init = &vl53l1x_Init; + rangefinder->update = &vl53l1x_Update; + rangefinder->read = &vl53l1x_GetDistance; + + return true; +} + +#endif diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.h b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h new file mode 100644 index 00000000000..ad125ce8a0e --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h @@ -0,0 +1,29 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#define RANGEFINDER_VL53L1X_TASK_PERIOD_MS 50 + +bool vl53l1xDetect(rangefinderDev_t *dev); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f5fde879efe..6dcf22ea14f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -7,7 +7,7 @@ tables: values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"] + values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X"] enum: rangefinderType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"] diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 29955a42d5d..5c7f10c3539 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -40,6 +40,7 @@ #include "drivers/rangefinder/rangefinder_srf10.h" #include "drivers/rangefinder/rangefinder_hcsr04_i2c.h" #include "drivers/rangefinder/rangefinder_vl53l0x.h" +#include "drivers/rangefinder/rangefinder_vl53l1x.h" #include "drivers/rangefinder/rangefinder_virtual.h" #include "fc/config.h" @@ -131,6 +132,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; + case RANGEFINDER_VL53L1X: +#if defined(USE_RANGEFINDER_VL53L1X) + if (vl53l1xDetect(dev)) { + rangefinderHardware = RANGEFINDER_VL53L1X; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VL53L1X_TASK_PERIOD_MS)); + } +#endif + break; + case RANGEFINDER_MSP: #if defined(USE_RANGEFINDER_MSP) if (virtualRangefinderDetect(dev, &rangefinderMSPVtable)) { diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 2cd7d01e1cc..a814a11423f 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -30,6 +30,7 @@ typedef enum { RANGEFINDER_MSP = 5, RANGEFINDER_UNUSED = 6, // Was UIB RANGEFINDER_BENEWAKE = 7, + RANGEFINDER_VL53L1X = 8, } rangefinderType_e; typedef struct rangefinderConfig_s { diff --git a/src/main/target/common.h b/src/main/target/common.h index 9d0ddc33462..74752f0172c 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -97,6 +97,7 @@ #define USE_RANGEFINDER_MSP #define USE_RANGEFINDER_BENEWAKE #define USE_RANGEFINDER_VL53L0X +#define USE_RANGEFINDER_VL53L1X #define USE_RANGEFINDER_HCSR04_I2C // Allow default optic flow boards diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index ad8736d0db6..7825ac12362 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -295,6 +295,15 @@ #endif #endif +#if defined(USE_RANGEFINDER_VL53L1X) + #if !defined(VL53L1X_I2C_BUS) && defined(RANGEFINDER_I2C_BUS) + #define VL53L1X_I2C_BUS RANGEFINDER_I2C_BUS + #endif + + #if defined(VL53L1X_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_vl53l1x, DEVHW_VL53L1X, VL53L1X_I2C_BUS, 0x29, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); + #endif +#endif /** AIRSPEED SENSORS **/ From def19bc8ce4a32ad37d0687970f100569e5a9bb8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 5 Feb 2021 09:21:33 +0100 Subject: [PATCH 061/143] Remove unused debug --- 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 be792baa6a2..23fde837b74 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -548,10 +548,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h * Positive fixedWingLevelTrim means nose should point upwards * Negative fixedWingLevelTrim means nose should point downwards */ - DEBUG_SET(DEBUG_ALWAYS, 0, fixedWingLevelTrim); - DEBUG_SET(DEBUG_ALWAYS, 1, angleTarget); angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); - DEBUG_SET(DEBUG_ALWAYS, 2, angleTarget); } const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); From 08c9ed2d6ddeefd81c72ea76d2e8443196eb814f Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 6 Feb 2021 10:34:13 +0000 Subject: [PATCH 062/143] Removed mc_ from airmode CLI variables I have removed mc_ from the airmode types in the CLI. Airmode applies to both fixed wing and multicoptors, so the mc_ prefix makes no sense. This should be mentioned in the release notes. I will now create a PR for the configurator side, to reflect these changes. --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 57d89f36649..3c95f19859c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -20,6 +20,8 @@ | acczero_x | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | | acczero_y | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | | acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | +| airmode_throttle_threshold | 1300 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used | +| airmode_type | STICK_CENTER | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. | | airspeed_adc_channel | 0 | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | | align_acc | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | | align_board_pitch | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | @@ -217,8 +219,6 @@ | max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | | max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | | max_throttle | 1850 | This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. | -| mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used | -| mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. | | mc_cd_lpf_hz | 30 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky | | mc_cd_pitch | 60 | Multicopter Control Derivative gain for PITCH | | mc_cd_roll | 60 | Multicopter Control Derivative gain for ROLL | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ee5aaa0fe29..8ba40bdac92 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1342,13 +1342,13 @@ groups: field: mid_throttle_deadband min: 0 max: 200 - - name: mc_airmode_type - description: "Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode." + - name: airmode_type + description: "Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes." default_value: "STICK_CENTER" field: airmodeHandlingType table: airmodeHandlingType - - name: mc_airmode_threshold - description: "Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used" + - name: airmode_throttle_threshold + description: "Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used" default_value: "1300" field: airmodeThrottleThreshold min: 1000 From 9130152fab4e264a58a77dfb1fc91bec993dfabe Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 7 Feb 2021 10:55:31 +0000 Subject: [PATCH 063/143] Increased fw loiter radius default Changed the default loiter radius, as have seen a lot of issues with loiter recently. The larger radius will give more of a buffer for larger models and people who have forgotten to/haven't tuned the model correctly. --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e61811104c6..01e83e70b79 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -159,7 +159,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) .pitch_to_throttle_smooth = 6, .pitch_to_throttle_thresh = 50, - .loiter_radius = 5000, // 50m + .loiter_radius = 7500, // 75m //Fixed wing landing .land_dive_angle = 2, // 2 degrees dive by default From 02b3cc907dbc63fdac4690d9ffdb41e58c02750d Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sun, 7 Feb 2021 17:04:41 +0000 Subject: [PATCH 064/143] update settings.yaml / settings.md for #6581 (#6582) --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3c95f19859c..e0a8c522421 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -278,7 +278,7 @@ | nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) | | nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | | nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] | -| nav_fw_loiter_radius | 5000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | +| nav_fw_loiter_radius | 7500 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8ba40bdac92..a49d69ff33d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2335,7 +2335,7 @@ groups: max: 900 - name: nav_fw_loiter_radius description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]" - default_value: "5000" + default_value: "7500" field: fw.loiter_radius min: 0 max: 10000 From ba9fb649e01d6d31e7dd730189eecc2a1d13d108 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcin=20Ga=C5=82czy=C5=84ski?= Date: Tue, 9 Feb 2021 00:24:16 +0100 Subject: [PATCH 065/143] optimize timings --- src/main/drivers/rangefinder/rangefinder_vl53l1x.c | 5 +++-- src/main/drivers/rangefinder/rangefinder_vl53l1x.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c index 01d6e119553..adc251275b4 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -109,6 +109,7 @@ #define VL53L1X_MAX_RANGE_CM (300) #define VL53L1X_DETECTION_CONE_DECIDEGREES (270) +#define VL53L1X_TIMING_BUDGET (33) #define VL53L1X_IMPLEMENTATION_VER_MAJOR 3 #define VL53L1X_IMPLEMENTATION_VER_MINOR 4 @@ -1607,8 +1608,8 @@ static void vl53l1x_Init(rangefinderDev_t * rangefinder) isInitialized = false; VL53L1X_SensorInit(rangefinder->busDev); VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ - VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */ - VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, 33); /* in ms, IM must be > = TB */ + VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, VL53L1X_TIMING_BUDGET); /* in ms possible values [20, 50, 100, 200, 500] */ + VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */ VL53L1X_StartRanging(rangefinder->busDev); isInitialized = true; } diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.h b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h index ad125ce8a0e..3dc5b63bf0c 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l1x.h +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h @@ -24,6 +24,6 @@ #pragma once -#define RANGEFINDER_VL53L1X_TASK_PERIOD_MS 50 +#define RANGEFINDER_VL53L1X_TASK_PERIOD_MS (40) bool vl53l1xDetect(rangefinderDev_t *dev); From c2e9dc9cd861e5d8ae605c6fbf5c4f2e591ee196 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 10 Feb 2021 10:49:38 +0100 Subject: [PATCH 066/143] Skip release for Rush Blade F722 --- src/main/target/RUSH_BLADE_F7/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt index 5dd7f2d2e1d..5d612d6c68b 100644 --- a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt +++ b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt @@ -1 +1 @@ -target_stm32f722xe(RUSH_BLADE_F7) \ No newline at end of file +target_stm32f722xe(RUSH_BLADE_F7 SKIP_RELEASES) \ No newline at end of file From 730e1a7354cdda5f2eefad40427e83181df4ff7a Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 10 Feb 2021 20:01:04 +0100 Subject: [PATCH 067/143] Enable baro median filtering by default again (#6584) It had been disabled by mistake in f0943875 --- src/main/sensors/barometer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 3506e9361a3..bb1325aeeb9 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -56,7 +56,7 @@ baro_t baro; // barometer access functions -PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3); #ifdef USE_BARO #define BARO_HARDWARE_DEFAULT BARO_AUTODETECT @@ -65,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER #endif PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, .baro_hardware = BARO_HARDWARE_DEFAULT, - .use_median_filtering = 0, + .use_median_filtering = 1, .baro_calibration_tolerance = 150 ); From 282c3b132e14c87a56d829a46dbc2e5d68f91e6c Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sun, 14 Feb 2021 17:07:30 +0100 Subject: [PATCH 068/143] Fix logic condition VSpeed constraint not allowing the value to be negative --- src/main/programming/logic_condition.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index d4465c4cec6..84e3b4607c8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -418,7 +418,7 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s - return constrain(getEstimatedActualVelocity(Z), 0, INT16_MAX); + return constrain(getEstimatedActualVelocity(Z), INT16_MIN, INT16_MAX); break; case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // % From 9a6fd4a7eee2f6799dca22684f7b29b058ff3882 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 15 Feb 2021 14:27:01 +0100 Subject: [PATCH 069/143] velocity in cm/s --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 94f612afa58..50ffb70fb73 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -569,7 +569,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) if (ctx->newFlags & EST_GPS_Z_VALID) { // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z; - const float gpsRocScaler = bellCurve(gpsRocResidual, 2.5f); + const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f); ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt; } From c43109178055614002fc0642826dde726785930d Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Mon, 15 Feb 2021 12:06:14 -0500 Subject: [PATCH 070/143] CRSF SNR Range update This updates SNR range values to keep them inline with typical LoRa SNR values --- src/main/fc/settings.yaml | 4 ++-- src/main/io/osd.c | 10 +++++----- src/main/io/osd.h | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a49d69ff33d..ec2b1a01ea9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2799,8 +2799,8 @@ groups: description: "Value below which Crossfire SNR Alarm pops-up. (dB)" default_value: "4" field: snr_alarm - min: -12 - max: 8 + min: -20 + max: 10 - name: osd_link_quality_alarm condition: USE_SERIALRX_CRSF description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6dbe3189aac..4055cf733a6 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -785,7 +785,7 @@ static const char * navigationStateMessage(void) break; case MW_NAV_STATE_RTH_START: return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); - case MW_NAV_STATE_RTH_CLIMB: + case MW_NAV_STATE_RTH_CLIMB: return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); case MW_NAV_STATE_RTH_ENROUTE: return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); @@ -1685,7 +1685,7 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_CRSF_SNR_DB: { - const char* showsnr = "-12"; + const char* showsnr = "-20"; const char* hidesnr = " "; int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; if (osdSNR_Alarm <= osdConfig()->snr_alarm) { @@ -3378,7 +3378,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints - tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); + tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { // WP hold time countdown in seconds @@ -3388,8 +3388,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); messages[messageCount++] = messageBuf; } - } else { - const char *navStateMessage = navigationStateMessage(); + } else { + const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { messages[messageCount++] = navStateMessage; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d740b5de521..cff09933587 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -292,7 +292,7 @@ typedef struct osdConfig_s { float gforce_axis_alarm_min; float gforce_axis_alarm_max; #ifdef USE_SERIALRX_CRSF - int16_t snr_alarm; //CRSF SNR alarm in dB + int8_t snr_alarm; //CRSF SNR alarm in dB int8_t link_quality_alarm; #endif #ifdef USE_BARO @@ -337,7 +337,7 @@ typedef struct osdConfig_s { bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_short; + uint8_t plus_code_short; uint8_t osd_ahi_style; uint8_t force_grid; // Force a pixel based OSD to use grid mode. uint8_t ahi_bordered; // Only used by the AHI widget From 84083689bb4e144149be7a2c3ef9501c05ca1ba4 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 16 Feb 2021 11:54:39 +0100 Subject: [PATCH 071/143] Allow to configure speed source for DJI OSD --- src/main/fc/settings.yaml | 9 ++++++++ src/main/io/osd.c | 7 ------ src/main/io/osd_common.c | 11 ++++++++++ src/main/io/osd_common.h | 5 +++++ src/main/io/osd_dji_hd.c | 30 +++++++++++++++++--------- src/main/io/osd_dji_hd.h | 7 ++++++ src/main/programming/logic_condition.c | 3 ++- 7 files changed, 54 insertions(+), 18 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ec2b1a01ea9..f13d9253f59 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -152,6 +152,9 @@ tables: - name: djiOsdTempSource values: ["ESC", "IMU", "BARO"] enum: djiOsdTempSource_e + - name: djiOsdSpeedSource + values: ["GROUND", "3D", "AIR"] + enum: djiOsdSpeedSource_e - name: nav_overrides_motor_stop enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] @@ -3249,3 +3252,9 @@ groups: field: esc_temperature_source table: djiOsdTempSource type: uint8_t + - name: dji_speed_source + description: "Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR" + default_value: "GROUND" + field: speedSource + table: djiOsdSpeedSource + type: uint8_t diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4055cf733a6..a4d8f3cf0e2 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1118,13 +1118,6 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } -static int16_t osdGet3DSpeed(void) -{ - int16_t vert_speed = getEstimatedActualVelocity(Z); - int16_t hor_speed = gpsSol.groundSpeed; - return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); -} - #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index c7975652650..6d24dc41db1 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -36,6 +36,8 @@ #include "io/osd_common.h" #include "io/osd_grid.h" +#include "navigation/navigation.h" + #if defined(USE_OSD) #define CANVAS_DEFAULT_GRID_ELEMENT_WIDTH OSD_CHAR_WIDTH @@ -151,4 +153,13 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas) osdGridDrawSidebars(display); } +#ifdef USE_GPS +int16_t osdGet3DSpeed(void) +{ + int16_t vert_speed = getEstimatedActualVelocity(Z); + int16_t hor_speed = gpsSol.groundSpeed; + return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); +} +#endif + #endif diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index 054f458a63d..2bed67d5693 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -82,3 +82,8 @@ void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, c // grid slots. void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading); void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas); + +#ifdef USE_GPS +int16_t osdGet3DSpeed(void); +#endif + diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index f506c8b9cf9..57438dac365 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -58,6 +58,7 @@ #include "io/gps.h" #include "io/osd.h" #include "io/osd_dji_hd.h" +#include "io/osd_common.h" #include "rx/rx.h" @@ -68,6 +69,7 @@ #include "sensors/acceleration.h" #include "sensors/esc_sensor.h" #include "sensors/temperature.h" +#include "sensors/pitotmeter.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -118,11 +120,12 @@ * but reuse the packet decoder to minimize code duplication */ -PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 2); PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, .use_name_for_messages = true, .esc_temperature_source = DJI_OSD_TEMP_ESC, .proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA, + .speedSource = DJI_OSD_SPEED_GROUND, ); // External dependency on looptime @@ -639,13 +642,6 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) return -1; } -static int16_t osdDJIGet3DSpeed(void) -{ - int16_t vert_speed = getEstimatedActualVelocity(Z); - int16_t hor_speed = gpsSol.groundSpeed; - return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); -} - /** * Converts velocity into a string based on the current unit system. * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) @@ -766,7 +762,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) osdDJIFormatThrottlePosition(djibuf,true); break; case 'S': - osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed()); + osdDJIFormatVelocityStr(djibuf, osdGet3DSpeed()); break; case 'E': osdDJIEfficiencyMahPerKM(djibuf); @@ -1004,7 +1000,21 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms sbufWriteU32(dst, gpsSol.llh.lat); sbufWriteU32(dst, gpsSol.llh.lon); sbufWriteU16(dst, gpsSol.llh.alt / 100); - sbufWriteU16(dst, gpsSol.groundSpeed); + + int reportedSpeed = 0; + if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_GROUND) { + reportedSpeed = gpsSol.groundSpeed; + } else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_3D) { + reportedSpeed = osdGet3DSpeed(); + } else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_AIR) { + #ifdef USE_PITOT + reportedSpeed = pitot.airSpeed; + #else + reportedSpeed = 0; + #endif + } + + sbufWriteU16(dst, reportedSpeed); sbufWriteU16(dst, gpsSol.groundCourse); break; diff --git a/src/main/io/osd_dji_hd.h b/src/main/io/osd_dji_hd.h index d105b275b65..84053231673 100644 --- a/src/main/io/osd_dji_hd.h +++ b/src/main/io/osd_dji_hd.h @@ -68,6 +68,12 @@ enum djiOsdTempSource_e { DJI_OSD_TEMP_BARO = 2 }; +enum djiOsdSpeedSource_e { + DJI_OSD_SPEED_GROUND = 0, + DJI_OSD_SPEED_3D = 1, + DJI_OSD_SPEED_AIR = 2 +}; + enum djiOsdProtoWorkarounds_e { DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA = 1 << 0, }; @@ -76,6 +82,7 @@ typedef struct djiOsdConfig_s { uint8_t use_name_for_messages; uint8_t esc_temperature_source; uint8_t proto_workarounds; + uint8_t speedSource; } djiOsdConfig_t; PG_DECLARE(djiOsdConfig_t, djiOsdConfig); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index d4465c4cec6..e806dacd7e8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -44,6 +44,7 @@ #include "flight/imu.h" #include "flight/pid.h" #include "drivers/io_port_expander.h" +#include "io/osd_common.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -402,7 +403,7 @@ static int logicConditionGetFlightOperandValue(int operand) { //FIXME align with osdGet3DSpeed case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s - return (int) sqrtf(sq(gpsSol.groundSpeed) + sq((int)getEstimatedActualVelocity(Z))); + return osdGet3DSpeed(); break; case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s From 1994ce7acbc3c5f3eb8c5f67e974225bbeac4e72 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 16 Feb 2021 11:58:38 +0100 Subject: [PATCH 072/143] Update Settings.md --- docs/Settings.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Settings.md b/docs/Settings.md index e0a8c522421..7fcdddf11e2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -64,6 +64,7 @@ | disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | | display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | | dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature | +| dji_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR | | dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | | dji_workarounds | | Enables workarounds for different versions of MSP protocol used | | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | From 34666c5c0a7cd3b351181cbdb2f65af2cbfa221f Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 16 Feb 2021 12:07:28 +0100 Subject: [PATCH 073/143] Fix compile on no-OSD boards --- src/main/io/osd_common.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index 6d24dc41db1..d785eac9798 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -153,6 +153,8 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas) osdGridDrawSidebars(display); } +#endif + #ifdef USE_GPS int16_t osdGet3DSpeed(void) { @@ -160,6 +162,4 @@ int16_t osdGet3DSpeed(void) int16_t hor_speed = gpsSol.groundSpeed; return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); } -#endif - -#endif +#endif \ No newline at end of file From e61e5632d08dfb63c3ff4be3399a4b365254ef39 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 19 Feb 2021 10:30:37 +0100 Subject: [PATCH 074/143] Simplify the Iterm freeze routine --- src/main/flight/pid.c | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 493c2192f7c..1693aa7e8fc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -81,7 +81,6 @@ typedef struct { // Rate integrator float errorGyroIf; float errorGyroIfLimit; - float errorGyroIfFrozen; // Used for ANGLE filtering (PT1, we don't need super-sharpness here) pt1Filter_t angleFilterState; @@ -353,7 +352,6 @@ void pidResetErrorAccumulators(void) for (int axis = 0; axis < 3; axis++) { pidState[axis].errorGyroIf = 0.0f; pidState[axis].errorGyroIfLimit = 0.0f; - pidState[axis].errorGyroIfFrozen = 0.0f; } } @@ -603,15 +601,6 @@ static void applyItermLimiting(pidState_t *pidState) { } } -static void applyItermFreezing(pidState_t *pidState) { - if (pidState->itermFreezeActive) { - pidState->errorGyroIf = pidState->errorGyroIfFrozen; - } else - { - pidState->errorGyroIfFrozen = pidState->errorGyroIf; - } -} - static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { UNUSED(pidState); UNUSED(axis); @@ -624,11 +613,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float newPTerm = pTermProcess(pidState, rateError, dT); const float newFFTerm = pidState->rateTarget * pidState->kFF; - // Calculate integral - pidState->errorGyroIf += rateError * pidState->kI * dT; + /* + * Integral should be updated only if axis Iterm is not frozen + */ + if (!pidState->itermFreezeActive) { + pidState->errorGyroIf += rateError * pidState->kI * dT; + } applyItermLimiting(pidState); - applyItermFreezing(pidState); if (pidProfile()->fixedWingItermThrowLimit != 0) { pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); From 5bfbf90549dd617b8ae613a4c0e4b6cc68905cd7 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 20 Feb 2021 18:52:17 +0100 Subject: [PATCH 075/143] Add MATEKF405CAN board; Add support for RM3100 compass on SPI bus --- src/main/CMakeLists.txt | 2 + src/main/drivers/bus.h | 1 + src/main/drivers/compass/compass_rm3100.c | 179 ++++++++++++++++++++ src/main/drivers/compass/compass_rm3100.h | 27 +++ src/main/fc/settings.yaml | 2 +- src/main/sensors/compass.c | 17 ++ src/main/sensors/compass.h | 3 +- src/main/target/MATEKF405CAN/CMakeLists.txt | 1 + src/main/target/MATEKF405CAN/config.c | 31 ++++ src/main/target/MATEKF405CAN/target.c | 44 +++++ src/main/target/MATEKF405CAN/target.h | 174 +++++++++++++++++++ src/main/target/common_hardware.c | 6 + 12 files changed, 485 insertions(+), 2 deletions(-) create mode 100644 src/main/drivers/compass/compass_rm3100.c create mode 100644 src/main/drivers/compass/compass_rm3100.h create mode 100644 src/main/target/MATEKF405CAN/CMakeLists.txt create mode 100644 src/main/target/MATEKF405CAN/config.c create mode 100644 src/main/target/MATEKF405CAN/target.c create mode 100644 src/main/target/MATEKF405CAN/target.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index a80fdd9feb4..b55aa2f6fa9 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -152,6 +152,8 @@ main_sources(COMMON_SRC drivers/compass/compass_mpu9250.h drivers/compass/compass_qmc5883l.c drivers/compass/compass_qmc5883l.h + drivers/compass/compass_rm3100.c + drivers/compass/compass_rm3100.h drivers/compass/compass_msp.c drivers/compass/compass_msp.h diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 2d7a51fc979..24cde286476 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -116,6 +116,7 @@ typedef enum { DEVHW_QMC5883, DEVHW_MAG3110, DEVHW_LIS3MDL, + DEVHW_RM3100, /* Temp sensor chips */ DEVHW_LM75_0, diff --git a/src/main/drivers/compass/compass_rm3100.c b/src/main/drivers/compass/compass_rm3100.c new file mode 100644 index 00000000000..e4d7ba3afa3 --- /dev/null +++ b/src/main/drivers/compass/compass_rm3100.c @@ -0,0 +1,179 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include + +#include "platform.h" + +#ifdef USE_MAG_RM3100 + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" + +#include "drivers/sensor.h" +#include "drivers/compass/compass.h" + +#include "drivers/compass/compass_rm3100.h" + +#define RM3100_REG_POLL 0x00 +#define RM3100_REG_CMM 0x01 +#define RM3100_REG_CCX1 0x04 +#define RM3100_REG_CCX0 0x05 +#define RM3100_REG_CCY1 0x06 +#define RM3100_REG_CCY0 0x07 +#define RM3100_REG_CCZ1 0x08 +#define RM3100_REG_CCZ0 0x09 +#define RM3100_REG_TMRC 0x0B +#define RM3100_REG_MX 0x24 +#define RM3100_REG_MY 0x27 +#define RM3100_REG_MZ 0x2A +#define RM3100_REG_BIST 0x33 +#define RM3100_REG_STATUS 0x34 +#define RM3100_REG_HSHAKE 0x35 +#define RM3100_REG_REVID 0x36 + +#define RM3100_REVID 0x22 + +#define CCX_DEFAULT_MSB 0x00 +#define CCX_DEFAULT_LSB 0xC8 +#define CCY_DEFAULT_MSB CCX_DEFAULT_MSB +#define CCY_DEFAULT_LSB CCX_DEFAULT_LSB +#define CCZ_DEFAULT_MSB CCX_DEFAULT_MSB +#define CCZ_DEFAULT_LSB CCX_DEFAULT_LSB +#define CMM_DEFAULT 0x71 // Continuous mode +#define TMRC_DEFAULT 0x94 + + +static bool deviceInit(magDev_t * mag) +{ + busWrite(mag->busDev, RM3100_REG_TMRC, TMRC_DEFAULT); + + busWrite(mag->busDev, RM3100_REG_CMM, CMM_DEFAULT); + + busWrite(mag->busDev, RM3100_REG_CCX1, CCX_DEFAULT_MSB); + busWrite(mag->busDev, RM3100_REG_CCX0, CCX_DEFAULT_LSB); + + busWrite(mag->busDev, RM3100_REG_CCY1, CCY_DEFAULT_MSB); + busWrite(mag->busDev, RM3100_REG_CCY0, CCY_DEFAULT_LSB); + + busWrite(mag->busDev, RM3100_REG_CCZ1, CCZ_DEFAULT_MSB); + busWrite(mag->busDev, RM3100_REG_CCZ0, CCZ_DEFAULT_LSB); + + return true; +} + +static bool deviceRead(magDev_t * mag) +{ + uint8_t status; + +#pragma pack(push, 1) + struct { + uint8_t x[3]; + uint8_t y[3]; + uint8_t z[3]; + } rm_report; +#pragma pack(pop) + + mag->magADCRaw[X] = 0; + mag->magADCRaw[Y] = 0; + mag->magADCRaw[Z] = 0; + + /* Check if new measurement is ready */ + bool ack = busRead(mag->busDev, RM3100_REG_STATUS, &status); + + if (!ack || (status & 0x80) == 0) { + return false; + } + + ack = busReadBuf(mag->busDev, RM3100_REG_MX, (uint8_t *)&rm_report, sizeof(rm_report)); + if (!ack) { + return false; + } + + int32_t xraw; + int32_t yraw; + int32_t zraw; + + /* Rearrange mag data */ + xraw = ((rm_report.x[0] << 24) | (rm_report.x[1] << 16) | (rm_report.x[2]) << 8); + yraw = ((rm_report.y[0] << 24) | (rm_report.y[1] << 16) | (rm_report.y[2]) << 8); + zraw = ((rm_report.z[0] << 24) | (rm_report.z[1] << 16) | (rm_report.z[2]) << 8); + + /* Truncate to 16-bit integers and pass along */ + mag->magADCRaw[X] = (int16_t)(xraw >> 16); + mag->magADCRaw[Y] = (int16_t)(yraw >> 16); + mag->magADCRaw[Z] = (int16_t)(zraw >> 16); + + return true; +} + +#define DETECTION_MAX_RETRY_COUNT 5 +static bool deviceDetect(magDev_t * mag) +{ + for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { + uint8_t revid = 0; + bool ack = busRead(mag->busDev, RM3100_REG_REVID, &revid); + + if (ack && revid == RM3100_REVID) { + return true; + } + } + + return false; +} + +bool rm3100MagDetect(magDev_t * mag) +{ + busSetSpeed(mag->busDev, BUS_SPEED_STANDARD); + + mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_RM3100, mag->magSensorToUse, OWNER_COMPASS); + if (mag->busDev == NULL) { + return false; + } + + if (!deviceDetect(mag)) { + busDeviceDeInit(mag->busDev); + return false; + } + + mag->init = deviceInit; + mag->read = deviceRead; + + return true; +} + +#endif diff --git a/src/main/drivers/compass/compass_rm3100.h b/src/main/drivers/compass/compass_rm3100.h new file mode 100644 index 00000000000..32f265c6301 --- /dev/null +++ b/src/main/drivers/compass/compass_rm3100.h @@ -0,0 +1,27 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +bool rm3100MagDetect(magDev_t *mag); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ec2b1a01ea9..ea3dc109704 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -10,7 +10,7 @@ tables: values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"] enum: rangefinderType_e - name: mag_hardware - values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"] + values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"] enum: magSensor_e - name: opflow_hardware values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"] diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 90a108109d6..2a7d3a61c4e 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,6 +40,7 @@ #include "drivers/compass/compass_qmc5883l.h" #include "drivers/compass/compass_mpu9250.h" #include "drivers/compass/compass_lis3mdl.h" +#include "drivers/compass/compass_rm3100.h" #include "drivers/compass/compass_msp.h" #include "drivers/io.h" #include "drivers/light_led.h" @@ -264,6 +265,22 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) } FALLTHROUGH; + case MAG_RM3100: +#ifdef USE_MAG_RM3100 + if (rm3100MagDetect(dev)) { +#ifdef MAG_RM3100_ALIGN + dev->magAlign.onBoard = MAG_RM3100_ALIGN; +#endif + magHardware = MAG_RM3100; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } + FALLTHROUGH; + case MAG_FAKE: #ifdef USE_FAKE_MAG if (fakeMagDetect(dev)) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index f7156381073..1695311bd24 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -42,7 +42,8 @@ typedef enum { MAG_IST8308 = 10, MAG_LIS3MDL = 11, MAG_MSP = 12, - MAG_FAKE = 13, + MAG_RM3100 = 13, + MAG_FAKE = 14, MAG_MAX = MAG_FAKE } magSensor_e; diff --git a/src/main/target/MATEKF405CAN/CMakeLists.txt b/src/main/target/MATEKF405CAN/CMakeLists.txt new file mode 100644 index 00000000000..29a924b4e63 --- /dev/null +++ b/src/main/target/MATEKF405CAN/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(MATEKF405CAN) diff --git a/src/main/target/MATEKF405CAN/config.c b/src/main/target/MATEKF405CAN/config.c new file mode 100644 index 00000000000..f3f22b74a9c --- /dev/null +++ b/src/main/target/MATEKF405CAN/config.c @@ -0,0 +1,31 @@ +/* + * 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 "platform.h" +#include "config/config_master.h" +#include "config/feature.h" +#include "io/serial.h" + + +void targetConfiguration(void) +{ + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS; + // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200; + +} diff --git a/src/main/target/MATEKF405CAN/target.c b/src/main/target/MATEKF405CAN/target.c new file mode 100644 index 00000000000..c8e7a088a32 --- /dev/null +++ b/src/main/target/MATEKF405CAN/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S1 D(2,2,7) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S2 D(2,3,7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S3 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 D(2,7,7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 D(1,7,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 D(1,2,5) + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S7 D(1,0,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S8 D(1,3,2) + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) + + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h new file mode 100644 index 00000000000..057bf922e4f --- /dev/null +++ b/src/main/target/MATEKF405CAN/target.h @@ -0,0 +1,174 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "MF4C" +#define USBD_PRODUCT_STRING "Matek_F405CAN" + +#define LED0 PA14 //Blue +#define LED1 PA13 //Green + +#define BEEPER PA8 +#define BEEPER_INVERTED +#define BEEPER_PWM +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW180_DEG_FLIP +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +#define PITOT_I2C_BUS BUS_I2C2 +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PCA9685_I2C_BUS BUS_I2C2 + + +// *************** SPI2 RM3100 ************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAG_RM3100 +#define MAG_RM3100_ALIGN CW0_DEG_FLIP +#define RM3100_CS_PIN PB12 +#define RM3100_SPI_BUS BUS_SPI2 + +// *************** SPI3 SD Card ******************** +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC14 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 //TX2 pad +#define SOFTSERIAL_1_RX_PIN PA2 //TX2 pad + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 +#define ADC_CHANNEL_4_PIN PC3 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** LED2812 ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA15 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST5_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream5 +#define WS2811_DMA_CHANNEL DMA_Channel_3 + +// *************** OTHERS ************************* +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_GPS | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) +#define VBAT_SCALE 2100 //1K:20K + +#define USE_SPEKTRUM_BIND +#define BIND_PIN PA3 // RX2 + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index ad8736d0db6..06c38e3d2bf 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -229,6 +229,12 @@ #endif BUSDEV_REGISTER_I2C(busdev_ist8308, DEVHW_IST8308, IST8308_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); #endif + +#if defined(USE_MAG_RM3100) + #if defined(RM3100_SPI_BUS) + BUSDEV_REGISTER_SPI(busdev_rm3100, DEVHW_RM3100, RM3100_SPI_BUS, RM3100_CS_PIN, NONE, DEVFLAGS_NONE, 0); + #endif +#endif #endif From 8f5ac3e51f8cb239d77efc9abde9c4a3d2f83773 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 21 Feb 2021 10:38:39 +0100 Subject: [PATCH 076/143] Refactor PID value getter --- src/main/programming/pid.c | 2 +- src/main/programming/pid.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index 09916fbb1b3..8b284087ebd 100644 --- a/src/main/programming/pid.c +++ b/src/main/programming/pid.c @@ -110,7 +110,7 @@ void programmingPidInit(void) } } -int programmingPidGetOutput(uint8_t i) { +int32_t programmingPidGetOutput(uint8_t i) { return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output; } diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h index 8645de8f130..0d55b952c67 100644 --- a/src/main/programming/pid.h +++ b/src/main/programming/pid.h @@ -51,4 +51,4 @@ typedef struct programmingPidState_s { void programmingPidUpdateTask(timeUs_t currentTimeUs); void programmingPidInit(void); void programmingPidReset(void); -int programmingPidGetOutput(uint8_t i); \ No newline at end of file +int32_t programmingPidGetOutput(uint8_t i); \ No newline at end of file From 67572fed9d776eff58f781d4bedd29f8f3aab8d5 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 21 Feb 2021 11:20:07 +0100 Subject: [PATCH 077/143] Add ability to read Programming PID status via MSP --- src/main/fc/fc_msp.c | 5 +++++ src/main/msp/msp_protocol_v2_inav.h | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a32961dabae..0e47385044d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -567,6 +567,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, programmingPids(i)->gains.FF); } break; + case MSP2_INAV_PROGRAMMING_PID_STATUS: + for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) { + sbufWriteU32(dst, programmingPidGetOutput(i)); + } + break; #endif case MSP2_COMMON_MOTOR_MIXER: for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 70e394ccb3f..d3e7d14585a 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -63,6 +63,7 @@ #define MSP2_INAV_GVAR_STATUS 0x2027 #define MSP2_INAV_PROGRAMMING_PID 0x2028 #define MSP2_INAV_SET_PROGRAMMING_PID 0x2029 +#define MSP2_INAV_PROGRAMMING_PID_STATUS 0x202A #define MSP2_PID 0x2030 #define MSP2_SET_PID 0x2031 @@ -79,4 +80,3 @@ #define MSP2_INAV_SET_SAFEHOME 0x2039 #define MSP2_INAV_MISC2 0x203A - From 1a5133fa11d6f1e1d00482d96a3c57d9d161a67d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 24 Feb 2021 11:16:24 +0100 Subject: [PATCH 078/143] Fix mag gain computation for negative mag zero --- src/main/sensors/compass.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 90a108109d6..adfc8e79a3d 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -350,7 +350,7 @@ void compassUpdate(timeUs_t currentTimeUs) static sensorCalibrationState_t calState; static timeUs_t calStartedAt = 0; static int16_t magPrev[XYZ_AXIS_COUNT]; - static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096}; + static int magAxisDeviation[XYZ_AXIS_COUNT]; // Check magZero if ( @@ -381,6 +381,7 @@ void compassUpdate(timeUs_t currentTimeUs) compassConfigMutable()->magZero.raw[axis] = 0; compassConfigMutable()->magGain[axis] = 1024; magPrev[axis] = 0; + magAxisDeviation[axis] = 0; // Gain is based on the biggest absolute deviation from the mag zero point. Gain computation starts at 0 } beeper(BEEPER_ACTION_SUCCESS); @@ -400,9 +401,9 @@ void compassUpdate(timeUs_t currentTimeUs) diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]); avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f; - const int32_t sample = ABS(mag.magADC[axis]); - if (sample > magGain[axis]) { - magGain[axis] = sample; + // Find the biggest sample deviation together with sample' sign + if (ABS(mag.magADC[axis]) > ABS(magAxisDeviation[axis])) { + magAxisDeviation[axis] = mag.magADC[axis]; } } @@ -429,7 +430,7 @@ void compassUpdate(timeUs_t currentTimeUs) * It is dirty, but worth checking if this will solve the problem of changing mag vector when UAV is tilted */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - compassConfigMutable()->magGain[axis] = magGain[axis] - compassConfig()->magZero.raw[axis]; + compassConfigMutable()->magGain[axis] = ABS(magAxisDeviation[axis] - compassConfig()->magZero.raw[axis]); } calStartedAt = 0; From 0b13c2cd8d0b2d83cf3539a19d78a64ad83d9ee0 Mon Sep 17 00:00:00 2001 From: Tim O'Brien Date: Wed, 24 Feb 2021 13:59:47 -0800 Subject: [PATCH 079/143] Adds heading target logic condition - allows for a slider/global-var to set the desired heading - addresses issue #6642 --- src/main/programming/logic_condition.c | 6 ++++++ src/main/programming/logic_condition.h | 3 ++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 84e3b4607c8..3c256531e25 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -317,6 +317,12 @@ static int logicConditionCompute( return true; break; + case LOGIC_CONDITION_SET_HEADING_TARGET: + temporaryValue = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(operandA))); + updateHeadingHoldTarget(temporaryValue); + return temporaryValue; + break; + default: return false; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index fc38839d71e..f6d68307038 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -68,7 +68,8 @@ typedef enum { LOGIC_CONDITION_MAP_INPUT = 36, LOGIC_CONDITION_MAP_OUTPUT = 37, LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38, - LOGIC_CONDITION_LAST = 39, + LOGIC_CONDITION_SET_HEADING_TARGET = 39, + LOGIC_CONDITION_LAST = 40, } logicOperation_e; typedef enum logicOperandType_s { From 25e1524dea9504d164178519cd1252250ad58424 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 25 Feb 2021 11:40:32 +0100 Subject: [PATCH 080/143] Update docs for Lua telemetry --- README.md | 2 +- docs/Telemetry.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index cbe75a1b189..f34db4d1361 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ Tool for Blackbox logs analysis is available [here](https://github.com/iNavFligh ### Telemetry screen for OpenTX -Users of FrSky Taranis X9 and Q X7 can use INAV Lua Telemetry screen created by @teckel12 . Software and installation instruction are available here: [https://github.com/iNavFlight/LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry) +Users of OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) ## Installation diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 8c8b406ccf8..2b5972370e0 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -123,7 +123,7 @@ The following sensors are transmitted * **0450** : 'Flight Path Vector' or 'Course over ground' in degrees*10 ### Compatible SmartPort/INAV telemetry flight status -To quickly and easily monitor these SmartPort sensors and flight modes, install [iNav LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry) to your Taranis Q X7, X9D, X9D+ or X9E transmitter. +To quickly and easily monitor these SmartPort sensors and flight modes, install [OpenTX Telemetry Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) to your Taranis Q X7, X9D, X9D+ or X9E transmitter. ## FrSky telemetry From 0cb632263d57b9694495648fb764f688d45acd20 Mon Sep 17 00:00:00 2001 From: Tim O'Brien Date: Thu, 25 Feb 2021 11:14:42 -0800 Subject: [PATCH 081/143] Adds LC heading documentation + RC override docs - also adds docs for the new RC overrides #6341 --- docs/Programming Framework.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 6e658058d1a..8d2297a97dd 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -71,6 +71,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 35 | TAN | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | | 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled | | 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | +| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` | +| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. | ### Operands From 0e659ae82ffd09bf138475f2fd5d835a3a7ac11b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 2 Mar 2021 11:16:17 +0100 Subject: [PATCH 082/143] Change PIDFF settings in CMS to UINT16 --- src/main/cms/cms_menu_imu.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 68311250c75..eaf5770162c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -55,7 +55,7 @@ #define RPY_PIDFF_MAX 200 #define OTHER_PIDDF_MAX 255 -#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP })) +#define PIDFF_ENTRY(label, ptr, max) OSD_UINT16_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP })) #define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX) #define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX) From 3cad49d006aaa64dada5e48cbe5be6419ba97e98 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 2 Mar 2021 11:55:06 +0100 Subject: [PATCH 083/143] Limit the size of OSD DEBUG element --- src/main/io/osd.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ef76c0cbe7f..a02d79720bf 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2194,12 +2194,21 @@ static bool osdDrawSingleElement(uint8_t item) } break; } - case OSD_DEBUG: { - // Longest representable string is -2147483648, hence 11 characters + /* + * Longest representable string is -2147483648 does not fit in the screen. + * Only 7 digits for negative and 8 digits for positive values allowed + */ for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { - tfp_sprintf(buff, "[%u]=%11ld [%u]=%11ld", bufferIndex, debug[bufferIndex], bufferIndex+1, debug[bufferIndex+1]); + tfp_sprintf( + buff, + "[%u]=%8ld [%u]=%8ld", + bufferIndex, + constrain(debug[bufferIndex], -9999999, 99999999), + bufferIndex+1, + constrain(debug[bufferIndex+1], -9999999, 99999999) + ); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); } break; From 87508187d3c5c98925bee661a8daab6da2e086bb Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 2 Mar 2021 12:27:00 +0100 Subject: [PATCH 084/143] Fix compilation warnings --- src/main/common/maths.c | 2 +- src/main/common/maths.h | 2 +- src/main/io/osd_dji_hd.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index d3d5a2410db..6d973980cf7 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -142,7 +142,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } -int constrain(int amt, int low, int high) +int32_t constrain(int32_t amt, int32_t low, int32_t high) { if (amt < low) return low; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index a5a375e9f8b..1066807ac7b 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -131,7 +131,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu int gcd(int num, int denom); int32_t applyDeadband(int32_t value, int32_t deadband); -int constrain(int amt, int low, int high); +int32_t constrain(int32_t amt, int32_t low, int32_t high); float constrainf(float amt, float low, float high); void devClear(stdev_t *dev); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index ef6ad6fcd1f..023fb470374 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -668,7 +668,7 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) thr = rcCommand[THROTTLE]; } - tfp_sprintf(buff, "%3d%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR"); + tfp_sprintf(buff, "%3ld%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR"); } /** From 303ba84353f53359eaecbedc63d7e32bb2fd298c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 3 Mar 2021 21:25:10 +0100 Subject: [PATCH 085/143] Add an ability not to slow down when switching to the next waypoint --- src/main/fc/settings.yaml | 5 +++++ src/main/navigation/navigation.c | 3 ++- src/main/navigation/navigation.h | 3 ++- src/main/navigation/navigation_multicopter.c | 2 +- 4 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d462d72cc8d..08b1a488c03 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2288,6 +2288,11 @@ groups: condition: USE_NAV min: 0 max: 255 + - name: nav_mc_wp_slowdown + description: "When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes." + default_value: "ON" + field: mc.slowDownForTurning + type: bool - name: nav_fw_cruise_thr description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )" default_value: "1400" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 01e83e70b79..55f1c7fb294 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -91,7 +91,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -144,6 +144,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_bank_angle = 40, // Max braking angle .posDecelerationTime = 120, // posDecelerationTime * 100 .posResponseExpo = 10, // posResponseExpo * 100 + .slowDownForTurning = true, }, // Fixed wing diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 50ba6b2632f..1bd5e858fb8 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -219,6 +219,7 @@ typedef struct navConfig_s { uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase uint8_t posDecelerationTime; // Brake time parameter uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC) + bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint } mc; struct { @@ -250,7 +251,7 @@ typedef struct navConfig_s { uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled bool allow_manual_thr_increase; - bool useFwNavYawControl; + bool useFwNavYawControl; uint8_t yawControlDeadband; } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 52235c59309..adfdb28ec0d 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -406,7 +406,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR static float getVelocityHeadingAttenuationFactor(void) { // In WP mode scale velocity if heading is different from bearing - if (navGetCurrentStateFlags() & NAV_AUTO_WP) { + if (navConfig()->mc.slowDownForTurning && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { const int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000); const float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError)); From 457777d24013b98f63208f383e7d0736a6513815 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 3 Mar 2021 21:26:19 +0100 Subject: [PATCH 086/143] Docs update --- docs/Settings.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Settings.md b/docs/Settings.md index 0662d982ec0..3262d5cd54e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -330,6 +330,7 @@ | nav_mc_vel_z_d | 10 | D gain of velocity PID controller | | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | +| nav_mc_wp_slowdown | ON | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | | nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | From 118d41d60a90d1063a99edb55272e7de79599e07 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 23 Feb 2021 22:38:24 +0100 Subject: [PATCH 087/143] d-term for fixed wing --- src/main/fc/settings.yaml | 22 ++++++++++++++++-- src/main/flight/pid.c | 48 +++++++++++++++++++++++++++------------ src/main/flight/pid.h | 2 +- src/main/io/osd.c | 2 +- 4 files changed, 55 insertions(+), 19 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d462d72cc8d..63040416a4d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -136,7 +136,7 @@ tables: values: ["HIGH", "MEDIUM", "LOW"] enum: dynamicFilterRange_e - name: pidTypeTable - values: ["NONE", "PID", "PIFF", "AUTO"] + values: ["NONE", "PID", "PIDFF", "AUTO"] enum: pidType_e - name: osd_ahi_style values: ["DEFAULT", "LINE"] @@ -1464,6 +1464,12 @@ groups: field: bank_fw.pid[PID_PITCH].I min: 0 max: 200 + - name: fw_d_pitch + description: "Fixed wing rate stabilisation D-gain for PITCH" + default_value: "0" + field: bank_fw.pid[PID_PITCH].D + min: 0 + max: 200 - name: fw_ff_pitch description: "Fixed-wing rate stabilisation FF-gain for PITCH" default_value: "50" @@ -1482,6 +1488,12 @@ groups: field: bank_fw.pid[PID_ROLL].I min: 0 max: 200 + - name: fw_d_roll + description: "Fixed wing rate stabilisation D-gain for roll" + default_value: "0" + field: bank_fw.pid[PID_ROLL].D + min: 0 + max: 200 - name: fw_ff_roll description: "Fixed-wing rate stabilisation FF-gain for ROLL" default_value: "50" @@ -1500,6 +1512,12 @@ groups: field: bank_fw.pid[PID_YAW].I min: 0 max: 200 + - name: fw_d_yaw + description: "Fixed wing rate stabilisation D-gain for YAW" + default_value: "0" + field: bank_fw.pid[PID_YAW].D + min: 0 + max: 200 - name: fw_ff_yaw description: "Fixed-wing rate stabilisation FF-gain for YAW" default_value: "60" @@ -1847,7 +1865,7 @@ groups: min: 1 max: 30 - name: pid_type - description: "Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID`" + description: "Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIDFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIDFF` and multirotors `PID`" field: pidControllerType table: pidTypeTable - name: mc_cd_lpf_hz diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 17ba8a758bf..089391759dc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -343,7 +343,7 @@ bool pidInitFilters(void) void pidResetTPAFilter(void) { - if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) { + if (usedPidControllerType == PID_TYPE_PIDFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) { pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, TASK_PERIOD_HZ(TASK_AUX_RATE_HZ) * 1e-6f); pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue()); } @@ -441,7 +441,7 @@ void updatePIDCoefficients() STATIC_FASTRAM uint16_t prevThrottle = 0; // Check if throttle changed. Different logic for fixed wing vs multirotor - if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { + if (usedPidControllerType == PID_TYPE_PIDFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]); if (filteredThrottle != prevThrottle) { prevThrottle = filteredThrottle; @@ -474,16 +474,16 @@ void updatePIDCoefficients() return; } - const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); + const float tpaFactor = usedPidControllerType == PID_TYPE_PIDFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { - if (usedPidControllerType == PID_TYPE_PIFF) { + if (usedPidControllerType == PID_TYPE_PIDFF) { // Airplanes - scale all PIDs according to TPA pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor; - pidState[axis].kD = 0.0f; + pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor; pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; pidState[axis].kCD = 0.0f; pidState[axis].kT = 0.0f; @@ -497,7 +497,7 @@ void updatePIDCoefficients() pidState[axis].kFF = 0.0f; // Tracking anti-windup requires P/I/D to be all defined which is only true for MC - if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0)) { + if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0) && (usedPidControllerType == PID_TYPE_PID)) { pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP)); } else { pidState[axis].kT = 0; @@ -638,6 +638,21 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf += rateError * pidState->kI * dT; } + // Calculate new D-term + float newDTerm; + if (pidState->kD == 0) { + // optimisation for when D8 is zero, often used by YAW axis + newDTerm = 0; + } else { + float delta = pidState->previousRateGyro - pidState->gyroRate; + + delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); + delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); + + // Calculate derivative + newDTerm = delta * (pidState->kD / dT); // * applyDBoost(pidState, dT); + } + applyItermLimiting(pidState); if (pidProfile()->fixedWingItermThrowLimit != 0) { @@ -650,14 +665,17 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh } #endif - axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit); + axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); #ifdef USE_BLACKBOX axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; - axisPID_D[axis] = newFFTerm; + axisPID_D[axis] = newDTerm; axisPID_Setpoint[axis] = pidState->rateTarget; #endif + + pidState->previousRateGyro = pidState->gyroRate; + } static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate) @@ -949,7 +967,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng void checkItermLimitingActive(pidState_t *pidState) { bool shouldActivate; - if (usedPidControllerType == PID_TYPE_PIFF) { + if (usedPidControllerType == PID_TYPE_PIDFF) { shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { @@ -961,7 +979,7 @@ void checkItermLimitingActive(pidState_t *pidState) void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { - if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { + if (usedPidControllerType == PID_TYPE_PIDFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ @@ -1115,7 +1133,7 @@ void pidInit(void) mixerConfig()->platformType == PLATFORM_BOAT || mixerConfig()->platformType == PLATFORM_ROVER ) { - usedPidControllerType = PID_TYPE_PIFF; + usedPidControllerType = PID_TYPE_PIDFF; } else { usedPidControllerType = PID_TYPE_PID; } @@ -1141,7 +1159,7 @@ void pidInit(void) } } - if (usedPidControllerType == PID_TYPE_PIFF) { + if (usedPidControllerType == PID_TYPE_PIDFF) { pidControllerApplyFn = pidApplyFixedWingRateController; } else if (usedPidControllerType == PID_TYPE_PID) { pidControllerApplyFn = pidApplyMulticopterRateController; @@ -1160,15 +1178,15 @@ void pidInit(void) } const pidBank_t * pidBank(void) { - return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; + return usedPidControllerType == PID_TYPE_PIDFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } pidBank_t * pidBankMutable(void) { - return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; + return usedPidControllerType == PID_TYPE_PIDFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex) { - if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) { + if (pidIndexGetType(pidIndex) == PID_TYPE_PIDFF) { return &pidBank->pid[pidIndex].FF; } else { return &pidBank->pid[pidIndex].D; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d627ed4b6c7..47185ce0d86 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -76,7 +76,7 @@ typedef enum { typedef enum { PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration PID_TYPE_PID, // Uses P, I and D terms - PID_TYPE_PIFF, // Uses P, I and FF, ignoring D + PID_TYPE_PIDFF, // Uses P, I, D and FF PID_TYPE_AUTO, // Autodetect } pidType_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a02d79720bf..bf7de42288e 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1195,7 +1195,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char * displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); + tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIDFF ? pid->FF : pid->D); if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); From 53fe29ac575bd037795b2bcf2679f829da6945c6 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 4 Mar 2021 10:10:16 +0100 Subject: [PATCH 088/143] update docs --- docs/Settings.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 0662d982ec0..243a0b8e504 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -114,6 +114,9 @@ | fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection | | fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot | | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | +| fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH | +| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for roll | +| fw_d_yaw | 0 | Fixed wing rate stabilisation D-gain for YAW | | fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH | | fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL | | fw_ff_yaw | 60 | Fixed-wing rate stabilisation FF-gain for YAW | @@ -408,7 +411,7 @@ | osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | | osd_units | METRIC | IMPERIAL, METRIC, UK | | osd_video_system | AUTO | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` | -| pid_type | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | +| pid_type | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIDFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIDFF` and multirotors `PID` | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pinio_box1 | | | From 570affa349db2c12a1366a700dce6df0f47815ae Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 4 Mar 2021 10:12:22 +0100 Subject: [PATCH 089/143] capitalization --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 243a0b8e504..868357e06f6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -115,7 +115,7 @@ | fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot | | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | | fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH | -| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for roll | +| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL | | fw_d_yaw | 0 | Fixed wing rate stabilisation D-gain for YAW | | fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH | | fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 63040416a4d..264fdab9c78 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1489,7 +1489,7 @@ groups: min: 0 max: 200 - name: fw_d_roll - description: "Fixed wing rate stabilisation D-gain for roll" + description: "Fixed wing rate stabilisation D-gain for ROLL" default_value: "0" field: bank_fw.pid[PID_ROLL].D min: 0 From 14fb20d99ee6faf5f18878d5ffcd641b9c7ab9aa Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 4 Mar 2021 10:26:04 +0100 Subject: [PATCH 090/143] Remove unnecessary comment --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 089391759dc..570c8f6877b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -650,7 +650,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); // Calculate derivative - newDTerm = delta * (pidState->kD / dT); // * applyDBoost(pidState, dT); + newDTerm = delta * (pidState->kD / dT); } applyItermLimiting(pidState); From cf785df86441321566d509e7ee004d7ed50e8b30 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 4 Mar 2021 11:08:05 +0100 Subject: [PATCH 091/143] D-term process function; PIFF instead of PIDFF --- src/main/flight/pid.c | 123 ++++++++++++++++++++---------------------- src/main/flight/pid.h | 2 +- src/main/io/osd.c | 2 +- 3 files changed, 59 insertions(+), 68 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 570c8f6877b..ebd3ce86ffd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -343,7 +343,7 @@ bool pidInitFilters(void) void pidResetTPAFilter(void) { - if (usedPidControllerType == PID_TYPE_PIDFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) { + if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) { pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, TASK_PERIOD_HZ(TASK_AUX_RATE_HZ) * 1e-6f); pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue()); } @@ -441,7 +441,7 @@ void updatePIDCoefficients() STATIC_FASTRAM uint16_t prevThrottle = 0; // Check if throttle changed. Different logic for fixed wing vs multirotor - if (usedPidControllerType == PID_TYPE_PIDFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { + if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]); if (filteredThrottle != prevThrottle) { prevThrottle = filteredThrottle; @@ -474,12 +474,12 @@ void updatePIDCoefficients() return; } - const float tpaFactor = usedPidControllerType == PID_TYPE_PIDFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); + const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { - if (usedPidControllerType == PID_TYPE_PIDFF) { + if (usedPidControllerType == PID_TYPE_PIFF) { // Airplanes - scale all PIDs according to TPA pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor; @@ -610,6 +610,50 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) { return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT); } +#ifdef USE_D_BOOST +static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { + + float dBoost = 1.0f; + + if (dBoostFactor > 1) { + const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; + const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); + const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); + + const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration); + dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor); + dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); + dBoost = constrainf(dBoost, 1.0f, dBoostFactor); + } + + return dBoost; +} +#else +static float applyDBoost(pidState_t *pidState, float dT) { + UNUSED(pidState); + UNUSED(dT); + return 1.0f; +} +#endif + +static float dTermProcess(pidState_t *pidState, float dT) { + // Calculate new D-term + float newDTerm = 0; + if (pidState->kD == 0) { + // optimisation for when D is zero, often used by YAW axis + newDTerm = 0; + } else { + float delta = pidState->previousRateGyro - pidState->gyroRate; + + delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); + delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); + + // Calculate derivative + newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT); + } + return(newDTerm); +} + static void applyItermLimiting(pidState_t *pidState) { if (pidState->itermLimitActive) { pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); @@ -629,6 +673,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh { const float rateError = pidState->rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); + const float newDTerm = dTermProcess(pidState, dT); const float newFFTerm = pidState->rateTarget * pidState->kFF; /* @@ -638,21 +683,6 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf += rateError * pidState->kI * dT; } - // Calculate new D-term - float newDTerm; - if (pidState->kD == 0) { - // optimisation for when D8 is zero, often used by YAW axis - newDTerm = 0; - } else { - float delta = pidState->previousRateGyro - pidState->gyroRate; - - delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); - delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); - - // Calculate derivative - newDTerm = delta * (pidState->kD / dT); - } - applyItermLimiting(pidState); if (pidProfile()->fixedWingItermThrowLimit != 0) { @@ -693,36 +723,12 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, return itermErrorRate; } -#ifdef USE_D_BOOST -static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { - - float dBoost = 1.0f; - - if (dBoostFactor > 1) { - const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; - const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); - const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); - - const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration); - dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor); - dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); - dBoost = constrainf(dBoost, 1.0f, dBoostFactor); - } - - return dBoost; -} -#else -static float applyDBoost(pidState_t *pidState, float dT) { - UNUSED(pidState); - UNUSED(dT); - return 1.0f; -} -#endif static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { const float rateError = pidState->rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); + const float newDTerm = dTermProcess(pidState, dT); const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta); @@ -739,21 +745,6 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid } DEBUG_SET(DEBUG_CD, axis, newCDTerm); - // Calculate new D-term - float newDTerm; - if (pidState->kD == 0) { - // optimisation for when D8 is zero, often used by YAW axis - newDTerm = 0; - } else { - float delta = pidState->previousRateGyro - pidState->gyroRate; - - delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); - delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); - - // Calculate derivative - newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT); - } - // TODO: Get feedback from mixer on available correction range for each axis const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit); @@ -967,7 +958,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng void checkItermLimitingActive(pidState_t *pidState) { bool shouldActivate; - if (usedPidControllerType == PID_TYPE_PIDFF) { + if (usedPidControllerType == PID_TYPE_PIFF) { shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { @@ -979,7 +970,7 @@ void checkItermLimitingActive(pidState_t *pidState) void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { - if (usedPidControllerType == PID_TYPE_PIDFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { + if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ @@ -1133,7 +1124,7 @@ void pidInit(void) mixerConfig()->platformType == PLATFORM_BOAT || mixerConfig()->platformType == PLATFORM_ROVER ) { - usedPidControllerType = PID_TYPE_PIDFF; + usedPidControllerType = PID_TYPE_PIFF; } else { usedPidControllerType = PID_TYPE_PID; } @@ -1159,7 +1150,7 @@ void pidInit(void) } } - if (usedPidControllerType == PID_TYPE_PIDFF) { + if (usedPidControllerType == PID_TYPE_PIFF) { pidControllerApplyFn = pidApplyFixedWingRateController; } else if (usedPidControllerType == PID_TYPE_PID) { pidControllerApplyFn = pidApplyMulticopterRateController; @@ -1178,15 +1169,15 @@ void pidInit(void) } const pidBank_t * pidBank(void) { - return usedPidControllerType == PID_TYPE_PIDFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; + return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } pidBank_t * pidBankMutable(void) { - return usedPidControllerType == PID_TYPE_PIDFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; + return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex) { - if (pidIndexGetType(pidIndex) == PID_TYPE_PIDFF) { + if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) { return &pidBank->pid[pidIndex].FF; } else { return &pidBank->pid[pidIndex].D; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 47185ce0d86..aa993f8a7af 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -76,7 +76,7 @@ typedef enum { typedef enum { PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration PID_TYPE_PID, // Uses P, I and D terms - PID_TYPE_PIDFF, // Uses P, I, D and FF + PID_TYPE_PIFF, // Uses P, I, D and FF PID_TYPE_AUTO, // Autodetect } pidType_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index bf7de42288e..a02d79720bf 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1195,7 +1195,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char * displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIDFF ? pid->FF : pid->D); + tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); From 75b023f3399947593e0b4ac35d3d2c760f0fdb50 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 4 Mar 2021 11:11:17 +0100 Subject: [PATCH 092/143] docs --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 868357e06f6..6cf68d3826c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -411,7 +411,7 @@ | osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | | osd_units | METRIC | IMPERIAL, METRIC, UK | | osd_video_system | AUTO | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` | -| pid_type | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIDFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIDFF` and multirotors `PID` | +| pid_type | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pinio_box1 | | | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 264fdab9c78..86cf71f8ce7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -136,7 +136,7 @@ tables: values: ["HIGH", "MEDIUM", "LOW"] enum: dynamicFilterRange_e - name: pidTypeTable - values: ["NONE", "PID", "PIDFF", "AUTO"] + values: ["NONE", "PID", "PIFF", "AUTO"] enum: pidType_e - name: osd_ahi_style values: ["DEFAULT", "LINE"] @@ -1865,7 +1865,7 @@ groups: min: 1 max: 30 - name: pid_type - description: "Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIDFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIDFF` and multirotors `PID`" + description: "Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID`" field: pidControllerType table: pidTypeTable - name: mc_cd_lpf_hz From bbbcf82f9edd4f910f8a036fd3515caf7ca60084 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Mar 2021 12:05:57 +0100 Subject: [PATCH 093/143] Small docs update --- docs/Settings.md | 16 ++++++++-------- src/main/fc/settings.yaml | 18 ++++++++++++++++-- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 0662d982ec0..dbcd3dc48de 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -82,7 +82,7 @@ | eleres_signature | | | | eleres_telemetry_en | | | | eleres_telemetry_power | | | -| esc_sensor_listen_only | | | +| esc_sensor_listen_only | OFF | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | | failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | | failsafe_fw_pitch_angle | 100 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb | | failsafe_fw_roll_angle | -200 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | @@ -145,7 +145,7 @@ | gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | | gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF | | gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF | -| gyro_hardware_lpf | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. | +| gyro_hardware_lpf | 256HZ | Hardware lowpass filter for gyro. Do value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. | | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | | gyro_notch_cutoff | | | @@ -247,7 +247,7 @@ | motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | | motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | | motor_direction_inverted | OFF | Use if you need to inverse yaw motor direction. | -| motor_poles | | | +| motor_poles | 14 | The number of motor poles. Required to compute motor RPM | | motor_pwm_protocol | ONESHOT125 | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED | | motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. | | msp_override_channels | 0 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. | @@ -302,7 +302,7 @@ | nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] | | nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | | nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | -| nav_max_terrain_follow_alt | | | +| nav_max_terrain_follow_alt | 100 | Max allowed above the ground altitude for terrain following mode | | nav_mc_auto_disarm_delay | 2000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) | | nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude | | nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode | @@ -411,10 +411,10 @@ | pid_type | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| pinio_box1 | | | -| pinio_box2 | | | -| pinio_box3 | | | -| pinio_box4 | | | +| pinio_box1 | target specific | Mode assignment for PINIO#1 | +| pinio_box2 | target specific | Mode assignment for PINIO#1 | +| pinio_box3 | target specific | Mode assignment for PINIO#1 | +| pinio_box4 | target specific | Mode assignment for PINIO#1 | | pitch_rate | 20 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | | pitot_hardware | NONE | Selection of pitot hardware. | | pitot_lpf_milli_hz | | | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d462d72cc8d..8dd7b3388f6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -182,8 +182,8 @@ groups: type: uint8_t table: alignment - name: gyro_hardware_lpf - description: "Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first." - default_value: "42HZ" + description: "Hardware lowpass filter for gyro. Do value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first." + default_value: "256HZ" field: gyro_lpf table: gyro_lpf - name: gyro_lpf_hz @@ -718,6 +718,8 @@ groups: max: 30 - name: motor_poles field: motorPoleCount + description: "The number of motor poles. Required to compute motor RPM" + default_value: "14" min: 4 max: 255 @@ -2183,6 +2185,8 @@ groups: max: 65000 - name: nav_max_terrain_follow_alt field: general.max_terrain_follow_altitude + default_value: "100" + description: "Max allowed above the ground altitude for terrain following mode" max: 1000 - name: nav_rth_altitude description: "Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters)" @@ -3194,21 +3198,29 @@ groups: members: - name: pinio_box1 field: permanentId[0] + description: "Mode assignment for PINIO#1" + default_value: "target specific" min: 0 max: 255 type: uint8_t - name: pinio_box2 field: permanentId[1] + default_value: "target specific" + description: "Mode assignment for PINIO#1" min: 0 max: 255 type: uint8_t - name: pinio_box3 field: permanentId[2] + default_value: "target specific" + description: "Mode assignment for PINIO#1" min: 0 max: 255 type: uint8_t - name: pinio_box4 field: permanentId[3] + default_value: "target specific" + description: "Mode assignment for PINIO#1" min: 0 max: 255 type: uint8_t @@ -3236,6 +3248,8 @@ groups: condition: USE_ESC_SENSOR members: - name: esc_sensor_listen_only + default_value: "OFF" + description: "Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case" field: listenOnly type: bool From efbb4126fff546ba4648e80c0ae2b4e00e1cb1a8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Mar 2021 12:10:08 +0100 Subject: [PATCH 094/143] Typo fix --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8dd7b3388f6..546cd46e8e2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -182,7 +182,7 @@ groups: type: uint8_t table: alignment - name: gyro_hardware_lpf - description: "Hardware lowpass filter for gyro. Do value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first." + description: "Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first." default_value: "256HZ" field: gyro_lpf table: gyro_lpf From 86229f823e4e0afbc8a66a0b2b2487791cd882fb Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Mar 2021 12:21:30 +0100 Subject: [PATCH 095/143] Regenerate docs --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index dbcd3dc48de..d704df6aca2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -145,7 +145,7 @@ | gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | | gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF | | gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF | -| gyro_hardware_lpf | 256HZ | Hardware lowpass filter for gyro. Do value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. | +| gyro_hardware_lpf | 256HZ | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. | | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | | gyro_notch_cutoff | | | From 6235f0e39c1c9a1b1d06296ba3dc90cd4e45ff09 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 4 Mar 2021 09:43:07 +0100 Subject: [PATCH 096/143] Separate D and FF adjustments --- src/main/fc/rc_adjustments.c | 58 ++++++++++++++++----- src/main/fc/rc_adjustments.h | 94 ++++++++++++++++++---------------- src/main/flight/pid.c | 9 ---- src/main/flight/pid.h | 1 - src/main/flight/pid_autotune.c | 6 +-- src/main/io/osd.c | 62 ++++++++++++++++++---- 6 files changed, 150 insertions(+), 80 deletions(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 434815b69c8..67cc5f5c10d 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -101,7 +101,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D_FF, + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { @@ -113,7 +117,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_YAW_D_FF, + .adjustmentFunction = ADJUSTMENT_YAW_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_YAW_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { @@ -137,7 +145,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_D_FF, + .adjustmentFunction = ADJUSTMENT_PITCH_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_PITCH_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { @@ -149,7 +161,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_ROLL_D_FF, + .adjustmentFunction = ADJUSTMENT_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_ROLL_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { @@ -447,18 +463,32 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta); schedulePidGainsUpdate(); break; - case ADJUSTMENT_PITCH_ROLL_D_FF: - case ADJUSTMENT_PITCH_D_FF: - applyAdjustmentPID(ADJUSTMENT_PITCH_D_FF, getD_FFRefByBank(pidBankMutable(), PID_PITCH), delta); - if (adjustmentFunction == ADJUSTMENT_PITCH_D_FF) { + case ADJUSTMENT_PITCH_ROLL_D: + case ADJUSTMENT_PITCH_D: + applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_ROLL].D, delta); + if (adjustmentFunction == ADJUSTMENT_PITCH_D) { schedulePidGainsUpdate(); break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_D FALLTHROUGH; - case ADJUSTMENT_ROLL_D_FF: - applyAdjustmentPID(ADJUSTMENT_ROLL_D_FF, getD_FFRefByBank(pidBankMutable(), PID_ROLL), delta); + case ADJUSTMENT_ROLL_D: + applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta); + schedulePidGainsUpdate(); + break; + case ADJUSTMENT_PITCH_ROLL_FF: + case ADJUSTMENT_PITCH_FF: + applyAdjustmentPID(ADJUSTMENT_PITCH_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta); + if (adjustmentFunction == ADJUSTMENT_PITCH_FF) { + schedulePidGainsUpdate(); + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_FF + FALLTHROUGH; + + case ADJUSTMENT_ROLL_FF: + applyAdjustmentPID(ADJUSTMENT_ROLL_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_YAW_P: @@ -469,8 +499,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta); schedulePidGainsUpdate(); break; - case ADJUSTMENT_YAW_D_FF: - applyAdjustmentPID(ADJUSTMENT_YAW_D_FF, getD_FFRefByBank(pidBankMutable(), PID_YAW), delta); + case ADJUSTMENT_YAW_D: + applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_ROLL].D, delta); + schedulePidGainsUpdate(); + break; + case ADJUSTMENT_YAW_FF: + applyAdjustmentPID(ADJUSTMENT_YAW_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_NAV_FW_CRUISE_THR: diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index ce5470eb460..87369788251 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -33,51 +33,55 @@ typedef enum { ADJUSTMENT_YAW_RATE = 5, ADJUSTMENT_PITCH_ROLL_P = 6, ADJUSTMENT_PITCH_ROLL_I = 7, - ADJUSTMENT_PITCH_ROLL_D_FF = 8, - ADJUSTMENT_YAW_P = 9, - ADJUSTMENT_YAW_I = 10, - ADJUSTMENT_YAW_D_FF = 11, - ADJUSTMENT_RATE_PROFILE = 12, // Unused, placeholder for compatibility - ADJUSTMENT_PITCH_RATE = 13, - ADJUSTMENT_ROLL_RATE = 14, - ADJUSTMENT_PITCH_P = 15, - ADJUSTMENT_PITCH_I = 16, - ADJUSTMENT_PITCH_D_FF = 17, - ADJUSTMENT_ROLL_P = 18, - ADJUSTMENT_ROLL_I = 19, - ADJUSTMENT_ROLL_D_FF = 20, - ADJUSTMENT_RC_YAW_EXPO = 21, - ADJUSTMENT_MANUAL_RC_EXPO = 22, - ADJUSTMENT_MANUAL_RC_YAW_EXPO = 23, - ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 24, - ADJUSTMENT_MANUAL_ROLL_RATE = 25, - ADJUSTMENT_MANUAL_PITCH_RATE = 26, - ADJUSTMENT_MANUAL_YAW_RATE = 27, - ADJUSTMENT_NAV_FW_CRUISE_THR = 28, - ADJUSTMENT_NAV_FW_PITCH2THR = 29, - ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 30, - ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 31, - ADJUSTMENT_LEVEL_P = 32, - ADJUSTMENT_LEVEL_I = 33, - ADJUSTMENT_LEVEL_D = 34, - ADJUSTMENT_POS_XY_P = 35, - ADJUSTMENT_POS_XY_I = 36, - ADJUSTMENT_POS_XY_D = 37, - ADJUSTMENT_POS_Z_P = 38, - ADJUSTMENT_POS_Z_I = 39, - ADJUSTMENT_POS_Z_D = 40, - ADJUSTMENT_HEADING_P = 41, - ADJUSTMENT_VEL_XY_P = 42, - ADJUSTMENT_VEL_XY_I = 43, - ADJUSTMENT_VEL_XY_D = 44, - ADJUSTMENT_VEL_Z_P = 45, - ADJUSTMENT_VEL_Z_I = 46, - ADJUSTMENT_VEL_Z_D = 47, - ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48, - ADJUSTMENT_VTX_POWER_LEVEL = 49, - ADJUSTMENT_TPA = 50, - ADJUSTMENT_TPA_BREAKPOINT = 51, - ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 52, + ADJUSTMENT_PITCH_ROLL_D = 8, + ADJUSTMENT_PITCH_ROLL_FF = 9, + ADJUSTMENT_PITCH_P = 10, + ADJUSTMENT_PITCH_I = 11, + ADJUSTMENT_PITCH_D = 12, + ADJUSTMENT_PITCH_FF = 13, + ADJUSTMENT_ROLL_P = 14, + ADJUSTMENT_ROLL_I = 15, + ADJUSTMENT_ROLL_D = 16, + ADJUSTMENT_ROLL_FF = 17, + ADJUSTMENT_YAW_P = 18, + ADJUSTMENT_YAW_I = 19, + ADJUSTMENT_YAW_D = 20, + ADJUSTMENT_YAW_FF = 21, + ADJUSTMENT_RATE_PROFILE = 22, // Unused, placeholder for compatibility + ADJUSTMENT_PITCH_RATE = 23, + ADJUSTMENT_ROLL_RATE = 24, + ADJUSTMENT_RC_YAW_EXPO = 25, + ADJUSTMENT_MANUAL_RC_EXPO = 26, + ADJUSTMENT_MANUAL_RC_YAW_EXPO = 27, + ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 28, + ADJUSTMENT_MANUAL_ROLL_RATE = 29, + ADJUSTMENT_MANUAL_PITCH_RATE = 30, + ADJUSTMENT_MANUAL_YAW_RATE = 31, + ADJUSTMENT_NAV_FW_CRUISE_THR = 32, + ADJUSTMENT_NAV_FW_PITCH2THR = 33, + ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 34, + ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 35, + ADJUSTMENT_LEVEL_P = 36, + ADJUSTMENT_LEVEL_I = 37, + ADJUSTMENT_LEVEL_D = 38, + ADJUSTMENT_POS_XY_P = 39, + ADJUSTMENT_POS_XY_I = 40, + ADJUSTMENT_POS_XY_D = 41, + ADJUSTMENT_POS_Z_P = 42, + ADJUSTMENT_POS_Z_I = 43, + ADJUSTMENT_POS_Z_D = 44, + ADJUSTMENT_HEADING_P = 45, + ADJUSTMENT_VEL_XY_P = 46, + ADJUSTMENT_VEL_XY_I = 47, + ADJUSTMENT_VEL_XY_D = 48, + ADJUSTMENT_VEL_Z_P = 49, + ADJUSTMENT_VEL_Z_I = 50, + ADJUSTMENT_VEL_Z_D = 51, + ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 52, + ADJUSTMENT_VTX_POWER_LEVEL = 53, + ADJUSTMENT_TPA = 54, + ADJUSTMENT_TPA_BREAKPOINT = 55, + ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 56, ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 17ba8a758bf..9fba3aff569 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1165,12 +1165,3 @@ const pidBank_t * pidBank(void) { pidBank_t * pidBankMutable(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } - -uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex) -{ - if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) { - return &pidBank->pid[pidIndex].FF; - } else { - return &pidBank->pid[pidIndex].D; - } -} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d627ed4b6c7..928c4fdf4d0 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -202,4 +202,3 @@ void autotuneUpdateState(void); void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput); pidType_e pidIndexGetType(pidIndex_e pidIndex); -uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex); diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 9db1f15284c..245571c2f4c 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -230,15 +230,15 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa switch (axis) { case FD_ROLL: - blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D_FF, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF); break; case FD_PITCH: - blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D_FF, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF); break; case FD_YAW: - blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D_FF, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF); break; } } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a02d79720bf..e40749735ed 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1165,7 +1165,49 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_ displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); } -static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD) +static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD, adjustmentFunction_e adjFuncFF) +{ + textAttributes_t elemAttr; + char buff[4]; + + const pid8_t *pid = &pidBank()->pid[pidIndex]; + pidType_e pidType = pidIndexGetType(pidIndex); + + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + + if (pidType == PID_TYPE_NONE) { + // PID is not used in this configuration. Draw dashes. + // XXX: Keep this in sync with the %3d format and spacing used below + displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - - -"); + return; + } + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->P); + if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P)))) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->I); + if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I)))) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->D); + if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->FF); + if ((isAdjustmentFunctionSelected(adjFuncFF)) || (((adjFuncFF == ADJUSTMENT_ROLL_FF) || (adjFuncFF == ADJUSTMENT_PITCH_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_FF)))) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 16, elemPosY, buff, elemAttr); +} + +static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD) { textAttributes_t elemAttr; char buff[4]; @@ -1196,7 +1238,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char * elemAttr = TEXT_ATTRIBUTES_NONE; tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); - if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF)))) + if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); } @@ -1862,35 +1904,35 @@ static bool osdDrawSingleElement(uint8_t item) #endif case OSD_ROLL_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D_FF); + osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF); return true; case OSD_PITCH_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D_FF); + osdDisplayFlightPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D, ADJUSTMENT_PITCH_FF); return true; case OSD_YAW_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D_FF); + osdDisplayFlightPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_FF); return true; case OSD_LEVEL_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); + osdDisplayNavPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); return true; case OSD_POS_XY_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); + osdDisplayNavPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); return true; case OSD_POS_Z_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); + osdDisplayNavPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); return true; case OSD_VEL_XY_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); + osdDisplayNavPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); return true; case OSD_VEL_Z_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); + osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); return true; case OSD_HEADING_P: From 7b50c0afd0aa5e0283c9cd797a8a1052ce70ffad Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 4 Mar 2021 22:43:23 +0100 Subject: [PATCH 097/143] fix adjustment selection bug --- src/main/fc/rc_adjustments.c | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 67cc5f5c10d..eac0ae9c8ef 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -109,63 +109,63 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_YAW_P, + .adjustmentFunction = ADJUSTMENT_PITCH_P, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_YAW_I, + .adjustmentFunction = ADJUSTMENT_PITCH_I, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_YAW_D, + .adjustmentFunction = ADJUSTMENT_PITCH_D, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_YAW_FF, + .adjustmentFunction = ADJUSTMENT_PITCH_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, + .adjustmentFunction = ADJUSTMENT_ROLL_P, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_RATE, + .adjustmentFunction = ADJUSTMENT_ROLL_I, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_ROLL_RATE, + .adjustmentFunction = ADJUSTMENT_ROLL_D, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_P, + .adjustmentFunction = ADJUSTMENT_ROLL_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_I, + .adjustmentFunction = ADJUSTMENT_YAW_P, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_D, + .adjustmentFunction = ADJUSTMENT_YAW_I, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_FF, + .adjustmentFunction = ADJUSTMENT_YAW_D, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_ROLL_P, + .adjustmentFunction = ADJUSTMENT_YAW_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_ROLL_I, + .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_ROLL_D, + .adjustmentFunction = ADJUSTMENT_PITCH_RATE, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_ROLL_FF, + .adjustmentFunction = ADJUSTMENT_ROLL_RATE, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { From 9654688fb4c2d00906a22989b3bece628980013c Mon Sep 17 00:00:00 2001 From: luca Date: Thu, 4 Mar 2021 23:38:42 +0100 Subject: [PATCH 098/143] Flight mode implemented but doesn't work --- src/main/common/maths.h | 2 + src/main/drivers/pwm_output.h | 1 + src/main/fc/fc_core.c | 17 ++++++ src/main/fc/fc_msp_box.c | 5 ++ src/main/fc/rc_modes.h | 3 +- src/main/fc/runtime_config.h | 1 + src/main/flight/mixer.c | 109 ++++++++++++++++++++++++++++++++-- src/main/io/osd.c | 2 + 8 files changed, 134 insertions(+), 6 deletions(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index a5a375e9f8b..9620dd2be2f 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -88,6 +88,8 @@ #define _ABS_I(x, var) _ABS_II(x, var) #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__)) +#define power3(x) ((x)*(x)*(x)) + // Floating point Euler angles. typedef struct fp_angles { float roll; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 2192d6d2983..93eafea87fe 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -28,6 +28,7 @@ void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteMotorUpdate(void); bool isMotorProtocolDigital(void); +bool isMotorProtocolDshot(void); void pwmWriteServo(uint8_t index, uint16_t value); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b1fe71ca436..3ae4b4714c2 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -398,6 +398,9 @@ void disarm(disarmReason_t disarmReason) blackboxFinish(); } #endif + if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) { + DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH); + } statsOnDisarm(); logicConditionReset(); @@ -457,6 +460,20 @@ void releaseSharedTelemetryPorts(void) { void tryArm(void) { updateArmingStatus(); + +#ifdef USE_DSHOT + if ( + STATE(MULTIROTOR) && + IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && + isMotorProtocolDshot() && + !ARMING_FLAG(ARMING_DISABLED_THROTTLE) && + !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH) + ) { + ENABLE_ARMING_FLAG(ARMED); + enableFlightMode(FLIP_OVER_AFTER_CRASH); + return; + } +#endif #ifdef USE_PROGRAMMING_FRAMEWORK if ( !isArmingDisabled() || diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 4189c4f8639..270551dd513 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -87,6 +87,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXLOITERDIRCHN, "LOITER CHANGE", 49 }, { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 }, { BOXPREARM, "PREARM", 51 }, + { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -306,6 +307,10 @@ void initActiveBoxIds(void) #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE; #endif + +#ifdef USE_DSHOT + activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH; +#endif } #define IS_ENABLED(mask) (mask == 0 ? 0 : 1) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index a29a39bf1df..cd4f66a452b 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -69,6 +69,7 @@ typedef enum { BOXLOITERDIRCHN = 40, BOXMSPRCOVERRIDE = 41, BOXPREARM = 42, + BOXFLIPOVERAFTERCRASH = 43, CHECKBOX_ITEM_COUNT } boxId_e; @@ -128,4 +129,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); void updateActivatedModes(void); void updateUsedModeActivationConditionFlags(void); -void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm); +void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm); \ No newline at end of file diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index aeb7a99af41..7f0bdd6e386 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -97,6 +97,7 @@ typedef enum { NAV_CRUISE_MODE = (1 << 12), FLAPERON = (1 << 13), TURN_ASSISTANT = (1 << 14), + FLIP_OVER_AFTER_CRASH = (1 << 15), } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index fd0b83a468b..d7aff4dc6a9 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -105,7 +105,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmProtocol = DEFAULT_PWM_PROTOCOL, .motorPwmRate = DEFAULT_PWM_RATE, .maxthrottle = DEFAULT_MAX_THROTTLE, - .mincommand = 1000, + .mincommand = 1000, .motorAccelTimeMs = 0, .motorDecelTimeMs = 0, .throttleIdle = 15.0f, @@ -184,7 +184,7 @@ void mixerUpdateStateFlags(void) } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); - } + } if (mixerConfig()->hasFlaps) { ENABLE_STATE(FLAPERON_AVAILABLE); @@ -266,7 +266,7 @@ void mixerInit(void) void mixerResetDisarmedMotors(void) { int motorZeroCommand; - + if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; throttleRangeMin = throttleDeadbandHigh; @@ -319,6 +319,100 @@ static uint16_t handleOutputScaling( } return value; } + +#define CRASH_FLIP_DEADBAND 20.0f +#define CRASH_FLIP_STICK_MINF 0.15f + +static void applyFlipOverAfterCrashModeToMotors(void) +{ + //To convert in configs + const float crashlip_expo = 35.0f; + + if (ARMING_FLAG(ARMED)) { + const float flipPowerFactor = 1.0f - crashlip_expo / 100.0f; + const float stickDeflectionPitchAbs = ABS(((float)rcCommand[PITCH])/500.0f); + const float stickDeflectionRollAbs = ABS(((float)rcCommand[ROLL])/500.0f); + const float stickDeflectionYawAbs = ABS(((float)rcCommand[YAW])/500.0f); + //deflection stick position + + const float stickDeflectionPitchExpo = flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor); + const float stickDeflectionRollExpo = flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor); + const float stickDeflectionYawExpo = flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor); + + float signPitch = rcCommand[PITCH] < 0 ? 1 : -1; + float signRoll = rcCommand[ROLL] < 0 ? 1 : -1; + //float signYaw = (rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 1 : -1); + float signYaw = (rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1); + + float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs)); + float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo)); + + if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) { + // If yaw is the dominant, disable pitch and roll + stickDeflectionLength = stickDeflectionYawAbs; + stickDeflectionExpoLength = stickDeflectionYawExpo; + signRoll = 0; + signPitch = 0; + } else { + // If pitch/roll dominant, disable yaw + signYaw = 0; + } + + const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0; + const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f) + + if (cosPhi < cosThreshold) { + // Enforce either roll or pitch exclusively, if not on diagonal + if (stickDeflectionRollAbs > stickDeflectionPitchAbs) { + signPitch = 0; + } else { + signRoll = 0; + } + } + + // Apply a reasonable amount of stick deadband + const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor); + const float flipStickRange = 1.0f - crashFlipStickMinExpo; + const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange; + + for (int i = 0; i < motorCount; ++i) { + + float motorOutputNormalised = + signPitch * currentMixer[i].pitch + //mixer, per ogni motore quanto interviene nel pitch, roll e yaw + signRoll * currentMixer[i].roll + + signYaw * currentMixer[i].yaw; + + if (motorOutputNormalised < 0) { + motorOutputNormalised = 0; + } + + motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised); + + float motorOutput = motorConfig()->mincommand+ motorOutputNormalised * motorConfig()->maxthrottle; + + // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered + motorOutput = (motorOutput < motorConfig()->mincommand + CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (motorOutput - CRASH_FLIP_DEADBAND); + + uint16_t motorValue; + + motorValue = handleOutputScaling( + motorOutput, + throttleIdleValue, + DSHOT_DISARM_COMMAND, + motorConfig()->mincommand, + motorConfig()->maxthrottle, + DSHOT_MIN_THROTTLE, + DSHOT_3D_DEADBAND_LOW, + false + ); + + pwmWriteMotor(i,motorValue); + } + } else { + // Disarmed mode + stopMotors(); + } +} #endif void FAST_CODE writeMotors(void) @@ -443,6 +537,11 @@ static int getReversibleMotorsThrottleDeadband(void) void FAST_CODE mixTable(const float dT) { + if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) { + applyFlipOverAfterCrashModeToMotors(); + return; + } + int16_t input[3]; // RPY, range [-500:+500] // Allow direct stick input to motors in passthrough mode on airplanes if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) { @@ -482,7 +581,7 @@ void FAST_CODE mixTable(const float dT) if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) { throttleRangeMin = throttleIdleValue; throttleRangeMax = motorConfig()->maxthrottle; - mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); + mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); } else #endif if (feature(FEATURE_REVERSIBLE_MOTORS)) { @@ -519,7 +618,7 @@ void FAST_CODE mixTable(const float dT) mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin; #endif // Throttle compensation based on battery voltage - if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { + if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax); } } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ef76c0cbe7f..6c721c3f1da 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1609,6 +1609,8 @@ static bool osdDrawSingleElement(uint8_t item) p = "ANGL"; else if (FLIGHT_MODE(HORIZON_MODE)) p = "HOR "; + else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) + p = "TURT"; displayWrite(osdDisplayPort, elemPosX, elemPosY, p); return true; From 0e9219423945c31014aa6394146e163b5db65e06 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 5 Mar 2021 10:01:17 +0100 Subject: [PATCH 099/143] Update Docs to correct LC VBAT scale --- docs/Programming Framework.md | 4 ++-- src/main/programming/logic_condition.c | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 8d2297a97dd..173e613901e 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -95,8 +95,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 1 | HOME_DISTANCE | in `meters` | | 2 | TRIP_DISTANCE | in `meters` | | 3 | RSSI | | -| 4 | VBAT | in `Volts * 10`, eg. `12.1V` is `121` | -| 5 | CELL_VOLTAGE | in `Volts * 10`, eg. `12.1V` is `121` | +| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` | +| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` | | 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` | | 7 | MAH_DRAWN | in `mAh` | | 8 | GPS_SATS | | diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 9cda8418558..47ca866cb83 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -384,7 +384,7 @@ static int logicConditionGetFlightOperandValue(int operand) { return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); break; - case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 10 + case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100 return getBatteryVoltage(); break; From 4f789a66dba7842dbfe3904ad830afaaba4eab9c Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Fri, 5 Mar 2021 11:52:52 +0100 Subject: [PATCH 100/143] debug mode --- src/main/build/debug.h | 1 + src/main/fc/settings.yaml | 2 +- src/main/flight/pid.c | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b484fd4d35c..95fdaff1589 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,5 +80,6 @@ typedef enum { DEBUG_SPM_VARIO, // Smartport master variometer DEBUG_PCF8574, DEBUG_DYNAMIC_GYRO_LPF, + DEBUG_FW_D, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 86cf71f8ce7..0fa5e30f595 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "DEBUG_FW_D"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ebd3ce86ffd..dab4426317e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -676,6 +676,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float newDTerm = dTermProcess(pidState, dT); const float newFFTerm = pidState->rateTarget * pidState->kFF; + DEBUG_SET(DEBUG_FW_D, axis, newDTerm); /* * Integral should be updated only if axis Iterm is not frozen */ From 9c129e20527bbaba3f270bf239094b83cde9daeb Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Fri, 5 Mar 2021 12:21:26 +0100 Subject: [PATCH 101/143] debug setting name --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0fa5e30f595..a7fc3c704df 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "DEBUG_FW_D"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator From 4c6f00c5881c1e0abeb286b56019391b1d5a710b Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Fri, 5 Mar 2021 17:34:12 +0100 Subject: [PATCH 102/143] Remove the OPFLOW_PMW3901 opflow hardware option The PMW3901 driver has never been implemented. Quote from @DigitalEntity: "It's using very weird SPI timings, won't be compatible with our scheduling" --- src/main/fc/settings.yaml | 2 +- src/main/sensors/opflow.c | 2 +- src/main/sensors/opflow.h | 7 +++---- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 546cd46e8e2..9fad1f308b8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -13,7 +13,7 @@ tables: values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"] enum: magSensor_e - name: opflow_hardware - values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"] + values: ["NONE", "CXOF", "MSP", "FAKE"] enum: opticalFlowSensor_e - name: baro_hardware values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"] diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index 942bc4ff843..f0f47b556f0 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -75,7 +75,7 @@ static float opflowCalibrationFlowAcc; #define OPFLOW_UPDATE_TIMEOUT_US 200000 // At least 5Hz updates required #define OPFLOW_CALIBRATE_TIME_MS 30000 // 30 second calibration time -PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 2); PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, .opflow_hardware = OPFLOW_NONE, diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h index 52fc487f7ed..afe94ec79ac 100755 --- a/src/main/sensors/opflow.h +++ b/src/main/sensors/opflow.h @@ -30,10 +30,9 @@ typedef enum { OPFLOW_NONE = 0, - OPFLOW_PMW3901 = 1, - OPFLOW_CXOF = 2, - OPFLOW_MSP = 3, - OPFLOW_FAKE = 4, + OPFLOW_CXOF = 1, + OPFLOW_MSP = 2, + OPFLOW_FAKE = 3, } opticalFlowSensor_e; typedef enum { From bb00731f30510027c2c90c5205d37a3a2fb6dec1 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 6 Mar 2021 21:42:13 +0100 Subject: [PATCH 103/143] fix bug --- src/main/fc/rc_adjustments.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index eac0ae9c8ef..c05a390ea4d 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -465,7 +465,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t break; case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_D: - applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_ROLL].D, delta); + applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta); if (adjustmentFunction == ADJUSTMENT_PITCH_D) { schedulePidGainsUpdate(); break; @@ -479,7 +479,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t break; case ADJUSTMENT_PITCH_ROLL_FF: case ADJUSTMENT_PITCH_FF: - applyAdjustmentPID(ADJUSTMENT_PITCH_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta); + applyAdjustmentPID(ADJUSTMENT_PITCH_FF, &pidBankMutable()->pid[PID_PITCH].FF, delta); if (adjustmentFunction == ADJUSTMENT_PITCH_FF) { schedulePidGainsUpdate(); break; @@ -500,11 +500,11 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t schedulePidGainsUpdate(); break; case ADJUSTMENT_YAW_D: - applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_ROLL].D, delta); + applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_YAW_FF: - applyAdjustmentPID(ADJUSTMENT_YAW_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta); + applyAdjustmentPID(ADJUSTMENT_YAW_FF, &pidBankMutable()->pid[PID_YAW].FF, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_NAV_FW_CRUISE_THR: From 5e4e4bf69c6db685c494fc5c9d1de4fdacb9256c Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 6 Mar 2021 23:06:10 +0100 Subject: [PATCH 104/143] edit authors file --- AUTHORS | 1 + 1 file changed, 1 insertion(+) diff --git a/AUTHORS b/AUTHORS index 11085ca78c9..839a8a5d8e9 100644 --- a/AUTHORS +++ b/AUTHORS @@ -7,6 +7,7 @@ Alberto García Hierro Alex Gorbatchev Alex Zaitsev Alexander Fedorov +Alexander van Saase Alexey Stankevich Andre Bernet Andreas Tacke From 72ee8eacb988750434d62dbe118b0d6384f44726 Mon Sep 17 00:00:00 2001 From: luca Date: Sun, 7 Mar 2021 00:34:35 +0100 Subject: [PATCH 105/143] Motors spin rotation change --- src/main/drivers/pwm_output.c | 229 ++++++++++++++++++---------------- src/main/drivers/pwm_output.h | 9 +- src/main/fc/fc_core.c | 3 + src/main/flight/mixer.c | 43 ++++--- 4 files changed, 153 insertions(+), 131 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index acfc89df46d..1d246bd1e23 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -60,12 +60,14 @@ FILE_COMPILE_FOR_SPEED #define DSHOT_MOTOR_BITLENGTH 20 #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ + +#define DSHOT_COMMAND_DELAY_US 1000 #endif typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef struct { - TCH_t * tch; + TCH_t *tch; bool configured; uint16_t value; @@ -81,19 +83,19 @@ typedef struct { } pwmOutputPort_t; typedef struct { - pwmOutputPort_t * pwmPort; // May be NULL if motor doesn't use the PWM port - uint16_t value; // Used to keep track of last motor value - bool requestTelemetry; + pwmOutputPort_t *pwmPort; // May be NULL if motor doesn't use the PWM port + uint16_t value; // Used to keep track of last motor value + bool requestTelemetry; } pwmOutputMotor_t; static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; -static pwmOutputMotor_t motors[MAX_MOTORS]; +static pwmOutputMotor_t motors[MAX_MOTORS]; static motorPwmProtocolTypes_e initMotorProtocol; -static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write value to motors +static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write value to motors -static pwmOutputPort_t * servos[MAX_SERVOS]; -static pwmWriteFuncPtr servoWritePtr = NULL; // Function to write value to motors +static pwmOutputPort_t *servos[MAX_SERVOS]; +static pwmWriteFuncPtr servoWritePtr = NULL; // Function to write value to motors #if defined(USE_DSHOT) || defined(USE_SERIALSHOT) static timeUs_t digitalMotorUpdateIntervalUs = 0; @@ -110,8 +112,7 @@ static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; -static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) -{ +static void pwmOutConfigTimer(pwmOutputPort_t *p, TCH_t *tch, uint32_t hz, uint16_t period, uint16_t value) { p->tch = tch; timerConfigBase(p->tch, period, hz); @@ -124,8 +125,7 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin *p->ccr = 0; } -static pwmOutputPort_t *pwmOutAllocatePort(void) -{ +static pwmOutputPort_t *pwmOutAllocatePort(void) { if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) { LOG_E(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS"); return NULL; @@ -139,10 +139,10 @@ static pwmOutputPort_t *pwmOutAllocatePort(void) return p; } -static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) -{ +static pwmOutputPort_t * +pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) { // Attempt to allocate TCH - TCH_t * tch = timerGetTCH(timHw); + TCH_t *tch = timerGetTCH(timHw); if (tch == NULL) { return NULL; } @@ -158,8 +158,7 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t if (enableOutput) { IOConfigGPIOAF(io, IOCFG_AF_PP, timHw->alternateFunction); - } - else { + } else { // If PWM outputs are disabled - configure as GPIO and drive low IOConfigGPIO(io, IOCFG_OUT_OD); IOLo(io); @@ -169,28 +168,25 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t return p; } -static void pwmWriteNull(uint8_t index, uint16_t value) -{ - (void)index; - (void)value; +static void pwmWriteNull(uint8_t index, uint16_t value) { + (void) index; + (void) value; } -static void pwmWriteStandard(uint8_t index, uint16_t value) -{ +static void pwmWriteStandard(uint8_t index, uint16_t value) { if (motors[index].pwmPort) { - *(motors[index].pwmPort->ccr) = lrintf((value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset); + *(motors[index].pwmPort->ccr) = lrintf( + (value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset); } } -void pwmWriteMotor(uint8_t index, uint16_t value) -{ +void pwmWriteMotor(uint8_t index, uint16_t value) { if (motorWritePtr && index < MAX_MOTORS && pwmMotorsEnabled) { motorWritePtr(index, value); } } -void pwmShutdownPulsesForAllMotors(uint8_t motorCount) -{ +void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { for (int index = 0; index < motorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows if (motors[index].pwmPort) { @@ -199,29 +195,27 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) } } -void pwmDisableMotors(void) -{ +void pwmDisableMotors(void) { pwmMotorsEnabled = false; } -void pwmEnableMotors(void) -{ +void pwmEnableMotors(void) { pwmMotorsEnabled = true; } -bool isMotorBrushed(uint16_t motorPwmRateHz) -{ +bool isMotorBrushed(uint16_t motorPwmRateHz) { return (motorPwmRateHz > 500); } -static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, float sMin, float sLen, uint32_t motorPwmRateHz, bool enableOutput) -{ +static pwmOutputPort_t * +motorConfigPwm(const timerHardware_t *timerHardware, float sMin, float sLen, uint32_t motorPwmRateHz, + bool enableOutput) { const uint32_t baseClockHz = timerGetBaseClockHW(timerHardware); const uint32_t prescaler = ((baseClockHz / motorPwmRateHz) + 0xffff) / 0x10000; /* rounding up */ const uint32_t timerHz = baseClockHz / prescaler; const uint32_t period = timerHz / motorPwmRateHz; - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, timerHz, period, 0, enableOutput); + pwmOutputPort_t *port = pwmOutConfigMotor(timerHardware, timerHz, period, 0, enableOutput); if (port) { port->pulseScale = ((sLen == 0) ? period : (sLen * timerHz)) / 1000.0f; @@ -233,25 +227,24 @@ static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, fl } #ifdef USE_DSHOT -uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) -{ + +uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT1200): + case (PWM_TYPE_DSHOT1200): return MOTOR_DSHOT1200_HZ; - case(PWM_TYPE_DSHOT600): + case (PWM_TYPE_DSHOT600): return MOTOR_DSHOT600_HZ; - case(PWM_TYPE_DSHOT300): + case (PWM_TYPE_DSHOT300): return MOTOR_DSHOT300_HZ; default: - case(PWM_TYPE_DSHOT150): + case (PWM_TYPE_DSHOT150): return MOTOR_DSHOT150_HZ; } } -static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) -{ +static pwmOutputPort_t *motorConfigDshot(const timerHardware_t *timerHardware, uint32_t dshotHz, bool enableOutput) { // Try allocating new port - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput); + pwmOutputPort_t *port = pwmOutConfigMotor(timerHardware, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput); if (!port) { return NULL; @@ -267,23 +260,21 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, return port; } -static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) -{ +static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) { for (int i = 0; i < 16; i++) { dmaBuffer[i] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first packet <<= 1; } } -static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) -{ +static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { uint16_t packet = (value << 1) | (requestTelemetry ? 1 : 0); // compute checksum int csum = 0; int csum_data = packet; for (int i = 0; i < 3; i++) { - csum ^= csum_data; // xor data by nibbles + csum ^= csum_data; // xor data by nibbles csum_data >>= 4; } csum &= 0xf; @@ -293,41 +284,37 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) return packet; } + #endif #if defined(USE_DSHOT) || defined(USE_SERIALSHOT) -static void motorConfigDigitalUpdateInterval(uint16_t motorPwmRateHz) -{ + +static void motorConfigDigitalUpdateInterval(uint16_t motorPwmRateHz) { digitalMotorUpdateIntervalUs = 1000000 / motorPwmRateHz; digitalMotorLastUpdateUs = 0; } -static void pwmWriteDigital(uint8_t index, uint16_t value) -{ +static void pwmWriteDigital(uint8_t index, uint16_t value) { // Just keep track of motor value, actual update happens in pwmCompleteMotorUpdate() // DSHOT and some other digital protocols use 11-bit throttle range [0;2047] motors[index].value = constrain(value, 0, 2047); } -bool isMotorProtocolDshot(void) -{ +bool isMotorProtocolDshot(void) { // We look at cached `initMotorProtocol` to make sure we are consistent with the initialized config // motorConfig()->motorPwmProtocol may change at run time which will cause uninitialized structures to be used return getMotorProtocolProperties(initMotorProtocol)->isDSHOT; } -bool isMotorProtocolSerialShot(void) -{ +bool isMotorProtocolSerialShot(void) { return getMotorProtocolProperties(initMotorProtocol)->isSerialShot; } -bool isMotorProtocolDigital(void) -{ +bool isMotorProtocolDigital(void) { return isMotorProtocolDshot() || isMotorProtocolSerialShot(); } -void pwmRequestMotorTelemetry(int motorIndex) -{ +void pwmRequestMotorTelemetry(int motorIndex) { if (!isMotorProtocolDigital()) { return; } @@ -340,8 +327,7 @@ void pwmRequestMotorTelemetry(int motorIndex) } } -void pwmCompleteMotorUpdate(void) -{ +void pwmCompleteMotorUpdate(void) { // This only makes sense for digital motor protocols if (!isMotorProtocolDigital()) { return; @@ -351,7 +337,8 @@ void pwmCompleteMotorUpdate(void) timeUs_t currentTimeUs = micros(); // Enforce motor update rate - if ((digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) { + if ((digitalMotorUpdateIntervalUs == 0) || + ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) { return; } @@ -399,8 +386,7 @@ void pwmRequestMotorTelemetry(int motorIndex) #endif -void pwmMotorPreconfigure(void) -{ +void pwmMotorPreconfigure(void) { // Keep track of initial motor protocol initMotorProtocol = motorConfig()->motorPwmProtocol; @@ -443,84 +429,86 @@ void pwmMotorPreconfigure(void) } } -bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bool enableOutput) -{ +bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bool enableOutput) { switch (initMotorProtocol) { - case PWM_TYPE_BRUSHED: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorConfig()->motorPwmRate, enableOutput); - break; + case PWM_TYPE_BRUSHED: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorConfig()->motorPwmRate, + enableOutput); + break; - case PWM_TYPE_ONESHOT125: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorConfig()->motorPwmRate, enableOutput); - break; + case PWM_TYPE_ONESHOT125: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorConfig()->motorPwmRate, + enableOutput); + break; - case PWM_TYPE_ONESHOT42: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorConfig()->motorPwmRate, enableOutput); - break; + case PWM_TYPE_ONESHOT42: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorConfig()->motorPwmRate, + enableOutput); + break; - case PWM_TYPE_MULTISHOT: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorConfig()->motorPwmRate, enableOutput); - break; + case PWM_TYPE_MULTISHOT: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorConfig()->motorPwmRate, + enableOutput); + break; #ifdef USE_DSHOT - case PWM_TYPE_DSHOT1200: - case PWM_TYPE_DSHOT600: - case PWM_TYPE_DSHOT300: - case PWM_TYPE_DSHOT150: - motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput); - break; + case PWM_TYPE_DSHOT1200: + case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT300: + case PWM_TYPE_DSHOT150: + motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput); + break; #endif - case PWM_TYPE_STANDARD: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorConfig()->motorPwmRate, enableOutput); - break; + case PWM_TYPE_STANDARD: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorConfig()->motorPwmRate, + enableOutput); + break; - default: - motors[motorIndex].pwmPort = NULL; - break; + default: + motors[motorIndex].pwmPort = NULL; + break; } return (motors[motorIndex].pwmPort != NULL); } // Helper function for ESC passthrough -ioTag_t pwmGetMotorPinTag(int motorIndex) -{ +ioTag_t pwmGetMotorPinTag(int motorIndex) { if (motors[motorIndex].pwmPort) { return motors[motorIndex].pwmPort->tch->timHw->tag; - } - else { + } else { return IOTAG_NONE; } } -static void pwmServoWriteStandard(uint8_t index, uint16_t value) -{ +static void pwmServoWriteStandard(uint8_t index, uint16_t value) { if (servos[index]) { *servos[index]->ccr = value; } } #ifdef USE_SERVO_SBUS -static void sbusPwmWriteStandard(uint8_t index, uint16_t value) -{ + +static void sbusPwmWriteStandard(uint8_t index, uint16_t value) { pwmServoWriteStandard(index, value); sbusServoUpdate(index, value); } + #endif #ifdef USE_PWM_SERVO_DRIVER -static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) -{ + +static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) { // If PCA9685 is not detected, we do not want to write servo output anywhere if (STATE(PWM_DRIVER_AVAILABLE)) { pwmDriverSetPulse(index, value); } } + #endif -void pwmServoPreconfigure(void) -{ +void pwmServoPreconfigure(void) { // Protocol-specific configuration switch (servoConfig()->servo_protocol) { default: @@ -549,9 +537,10 @@ void pwmServoPreconfigure(void) } } -bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput) -{ - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, servoCenterPulse, enableOutput); +bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, + uint16_t servoCenterPulse, bool enableOutput) { + pwmOutputPort_t *port = pwmOutConfigMotor(timerHardware, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, + servoCenterPulse, enableOutput); if (port) { servos[servoIndex] = port; @@ -561,13 +550,31 @@ bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui return false; } -void pwmWriteServo(uint8_t index, uint16_t value) -{ +void pwmWriteServo(uint8_t index, uint16_t value) { if (servoWritePtr && index < MAX_SERVOS) { servoWritePtr(index, value); } } +#ifdef USE_DSHOT +void changeDshotSpinRotation(dshotDirectionCommands_e directionSpin) { + + const uint8_t number_of_packets = 10; + + for (uint8_t j = 0; j < number_of_packets; j++) { + + delayMicroseconds(DSHOT_COMMAND_DELAY_US); + for (uint8_t i = 0; i < getMotorCount(); i++) { + pwmRequestMotorTelemetry(i); + pwmWriteMotor(i, directionSpin); + } + delayMicroseconds(DSHOT_COMMAND_DELAY_US); + pwmCompleteMotorUpdate(); + + } +} +#endif + #ifdef BEEPER_PWM void pwmWriteBeeper(bool onoffBeep) { diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 93eafea87fe..8cb580027c5 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -20,6 +20,11 @@ #include "drivers/io_types.h" #include "drivers/time.h" +typedef enum { + DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, + DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, +} dshotDirectionCommands_e; + void pwmRequestMotorTelemetry(int motorIndex); ioTag_t pwmGetMotorPinTag(int motorIndex); @@ -42,4 +47,6 @@ bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIn void pwmServoPreconfigure(void); bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput); void pwmWriteBeeper(bool onoffBeep); -void beeperPwmInit(ioTag_t tag, uint16_t frequency); \ No newline at end of file +void beeperPwmInit(ioTag_t tag, uint16_t frequency); + +void changeDshotSpinRotation(dshotDirectionCommands_e directionSpin); \ No newline at end of file diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3ae4b4714c2..80a79d51831 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -399,6 +399,7 @@ void disarm(disarmReason_t disarmReason) } #endif if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) { + changeDshotSpinRotation(DSHOT_CMD_SPIN_DIRECTION_NORMAL); DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH); } @@ -469,6 +470,8 @@ void tryArm(void) !ARMING_FLAG(ARMING_DISABLED_THROTTLE) && !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH) ) { + + changeDshotSpinRotation(DSHOT_CMD_SPIN_DIRECTION_REVERSED); ENABLE_ARMING_FLAG(ARMED); enableFlightMode(FLIP_OVER_AFTER_CRASH); return; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d7aff4dc6a9..9e4f101a4f6 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -322,22 +322,23 @@ static uint16_t handleOutputScaling( #define CRASH_FLIP_DEADBAND 20.0f #define CRASH_FLIP_STICK_MINF 0.15f - -static void applyFlipOverAfterCrashModeToMotors(void) -{ +static void applyFlipOverAfterCrashModeToMotors(void) { //To convert in configs const float crashlip_expo = 35.0f; if (ARMING_FLAG(ARMED)) { const float flipPowerFactor = 1.0f - crashlip_expo / 100.0f; - const float stickDeflectionPitchAbs = ABS(((float)rcCommand[PITCH])/500.0f); - const float stickDeflectionRollAbs = ABS(((float)rcCommand[ROLL])/500.0f); - const float stickDeflectionYawAbs = ABS(((float)rcCommand[YAW])/500.0f); + const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f); + const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f); + const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f); //deflection stick position - const float stickDeflectionPitchExpo = flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor); - const float stickDeflectionRollExpo = flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor); - const float stickDeflectionYawExpo = flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor); + const float stickDeflectionPitchExpo = + flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor); + const float stickDeflectionRollExpo = + flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor); + const float stickDeflectionYawExpo = + flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor); float signPitch = rcCommand[PITCH] < 0 ? 1 : -1; float signRoll = rcCommand[ROLL] < 0 ? 1 : -1; @@ -358,8 +359,9 @@ static void applyFlipOverAfterCrashModeToMotors(void) signYaw = 0; } - const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0; - const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f) + const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / + (sqrtf(2.0f) * stickDeflectionLength) : 0; + const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(PI/6.0f) if (cosPhi < cosThreshold) { // Enforce either roll or pitch exclusively, if not on diagonal @@ -371,29 +373,32 @@ static void applyFlipOverAfterCrashModeToMotors(void) } // Apply a reasonable amount of stick deadband - const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor); + const float crashFlipStickMinExpo = + flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor); const float flipStickRange = 1.0f - crashFlipStickMinExpo; const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange; for (int i = 0; i < motorCount; ++i) { float motorOutputNormalised = - signPitch * currentMixer[i].pitch + //mixer, per ogni motore quanto interviene nel pitch, roll e yaw - signRoll * currentMixer[i].roll + - signYaw * currentMixer[i].yaw; + signPitch * currentMixer[i].pitch + + //mixer, per ogni motore quanto interviene nel pitch, roll e yaw + signRoll * currentMixer[i].roll + + signYaw * currentMixer[i].yaw; if (motorOutputNormalised < 0) { - motorOutputNormalised = 0; + motorOutputNormalised = 0; } motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised); - float motorOutput = motorConfig()->mincommand+ motorOutputNormalised * motorConfig()->maxthrottle; + float motorOutput = motorConfig()->mincommand + motorOutputNormalised * motorConfig()->maxthrottle; // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered - motorOutput = (motorOutput < motorConfig()->mincommand + CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (motorOutput - CRASH_FLIP_DEADBAND); + motorOutput = (motorOutput < motorConfig()->mincommand + CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : ( + motorOutput - CRASH_FLIP_DEADBAND); - uint16_t motorValue; + motor[i] = motorOutput; motorValue = handleOutputScaling( motorOutput, From ae7d9377ff04cc7f6a06e73d6b501db62ec8f753 Mon Sep 17 00:00:00 2001 From: luca Date: Sun, 7 Mar 2021 00:57:24 +0100 Subject: [PATCH 106/143] IDE Format adjustment --- src/main/drivers/pwm_output.c | 208 ++++++++++++++++++---------------- src/main/flight/mixer.c | 13 --- 2 files changed, 111 insertions(+), 110 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 1d246bd1e23..2a757b567f0 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -67,7 +67,7 @@ FILE_COMPILE_FOR_SPEED typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef struct { - TCH_t *tch; + TCH_t * tch; bool configured; uint16_t value; @@ -83,19 +83,19 @@ typedef struct { } pwmOutputPort_t; typedef struct { - pwmOutputPort_t *pwmPort; // May be NULL if motor doesn't use the PWM port - uint16_t value; // Used to keep track of last motor value - bool requestTelemetry; + pwmOutputPort_t * pwmPort; // May be NULL if motor doesn't use the PWM port + uint16_t value; // Used to keep track of last motor value + bool requestTelemetry; } pwmOutputMotor_t; static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; -static pwmOutputMotor_t motors[MAX_MOTORS]; +static pwmOutputMotor_t motors[MAX_MOTORS]; static motorPwmProtocolTypes_e initMotorProtocol; -static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write value to motors +static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write value to motors -static pwmOutputPort_t *servos[MAX_SERVOS]; -static pwmWriteFuncPtr servoWritePtr = NULL; // Function to write value to motors +static pwmOutputPort_t * servos[MAX_SERVOS]; +static pwmWriteFuncPtr servoWritePtr = NULL; // Function to write value to motors #if defined(USE_DSHOT) || defined(USE_SERIALSHOT) static timeUs_t digitalMotorUpdateIntervalUs = 0; @@ -112,7 +112,8 @@ static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; -static void pwmOutConfigTimer(pwmOutputPort_t *p, TCH_t *tch, uint32_t hz, uint16_t period, uint16_t value) { +static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) +{ p->tch = tch; timerConfigBase(p->tch, period, hz); @@ -125,7 +126,8 @@ static void pwmOutConfigTimer(pwmOutputPort_t *p, TCH_t *tch, uint32_t hz, uint1 *p->ccr = 0; } -static pwmOutputPort_t *pwmOutAllocatePort(void) { +static pwmOutputPort_t *pwmOutAllocatePort(void) +{ if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) { LOG_E(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS"); return NULL; @@ -139,10 +141,10 @@ static pwmOutputPort_t *pwmOutAllocatePort(void) { return p; } -static pwmOutputPort_t * -pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) { +static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) +{ // Attempt to allocate TCH - TCH_t *tch = timerGetTCH(timHw); + TCH_t * tch = timerGetTCH(timHw); if (tch == NULL) { return NULL; } @@ -158,7 +160,8 @@ pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, ui if (enableOutput) { IOConfigGPIOAF(io, IOCFG_AF_PP, timHw->alternateFunction); - } else { + } + else { // If PWM outputs are disabled - configure as GPIO and drive low IOConfigGPIO(io, IOCFG_OUT_OD); IOLo(io); @@ -168,25 +171,28 @@ pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, ui return p; } -static void pwmWriteNull(uint8_t index, uint16_t value) { - (void) index; - (void) value; +static void pwmWriteNull(uint8_t index, uint16_t value) +{ + (void)index; + (void)value; } -static void pwmWriteStandard(uint8_t index, uint16_t value) { +static void pwmWriteStandard(uint8_t index, uint16_t value) +{ if (motors[index].pwmPort) { - *(motors[index].pwmPort->ccr) = lrintf( - (value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset); + *(motors[index].pwmPort->ccr) = lrintf((value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset); } } -void pwmWriteMotor(uint8_t index, uint16_t value) { +void pwmWriteMotor(uint8_t index, uint16_t value) +{ if (motorWritePtr && index < MAX_MOTORS && pwmMotorsEnabled) { motorWritePtr(index, value); } } -void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { +void pwmShutdownPulsesForAllMotors(uint8_t motorCount) +{ for (int index = 0; index < motorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows if (motors[index].pwmPort) { @@ -195,27 +201,29 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { } } -void pwmDisableMotors(void) { +void pwmDisableMotors(void) +{ pwmMotorsEnabled = false; } -void pwmEnableMotors(void) { +void pwmEnableMotors(void) +{ pwmMotorsEnabled = true; } -bool isMotorBrushed(uint16_t motorPwmRateHz) { +bool isMotorBrushed(uint16_t motorPwmRateHz) +{ return (motorPwmRateHz > 500); } -static pwmOutputPort_t * -motorConfigPwm(const timerHardware_t *timerHardware, float sMin, float sLen, uint32_t motorPwmRateHz, - bool enableOutput) { +static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, float sMin, float sLen, uint32_t motorPwmRateHz, bool enableOutput) +{ const uint32_t baseClockHz = timerGetBaseClockHW(timerHardware); const uint32_t prescaler = ((baseClockHz / motorPwmRateHz) + 0xffff) / 0x10000; /* rounding up */ const uint32_t timerHz = baseClockHz / prescaler; const uint32_t period = timerHz / motorPwmRateHz; - pwmOutputPort_t *port = pwmOutConfigMotor(timerHardware, timerHz, period, 0, enableOutput); + pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, timerHz, period, 0, enableOutput); if (port) { port->pulseScale = ((sLen == 0) ? period : (sLen * timerHz)) / 1000.0f; @@ -227,24 +235,25 @@ motorConfigPwm(const timerHardware_t *timerHardware, float sMin, float sLen, uin } #ifdef USE_DSHOT - -uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) { +uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) +{ switch (pwmProtocolType) { - case (PWM_TYPE_DSHOT1200): + case(PWM_TYPE_DSHOT1200): return MOTOR_DSHOT1200_HZ; - case (PWM_TYPE_DSHOT600): + case(PWM_TYPE_DSHOT600): return MOTOR_DSHOT600_HZ; - case (PWM_TYPE_DSHOT300): + case(PWM_TYPE_DSHOT300): return MOTOR_DSHOT300_HZ; default: - case (PWM_TYPE_DSHOT150): + case(PWM_TYPE_DSHOT150): return MOTOR_DSHOT150_HZ; } } -static pwmOutputPort_t *motorConfigDshot(const timerHardware_t *timerHardware, uint32_t dshotHz, bool enableOutput) { +static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) +{ // Try allocating new port - pwmOutputPort_t *port = pwmOutConfigMotor(timerHardware, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput); + pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput); if (!port) { return NULL; @@ -260,21 +269,23 @@ static pwmOutputPort_t *motorConfigDshot(const timerHardware_t *timerHardware, u return port; } -static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) { +static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) +{ for (int i = 0; i < 16; i++) { dmaBuffer[i] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first packet <<= 1; } } -static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { +static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) +{ uint16_t packet = (value << 1) | (requestTelemetry ? 1 : 0); // compute checksum int csum = 0; int csum_data = packet; for (int i = 0; i < 3; i++) { - csum ^= csum_data; // xor data by nibbles + csum ^= csum_data; // xor data by nibbles csum_data >>= 4; } csum &= 0xf; @@ -284,37 +295,41 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) return packet; } - #endif #if defined(USE_DSHOT) || defined(USE_SERIALSHOT) - -static void motorConfigDigitalUpdateInterval(uint16_t motorPwmRateHz) { +static void motorConfigDigitalUpdateInterval(uint16_t motorPwmRateHz) +{ digitalMotorUpdateIntervalUs = 1000000 / motorPwmRateHz; digitalMotorLastUpdateUs = 0; } -static void pwmWriteDigital(uint8_t index, uint16_t value) { +static void pwmWriteDigital(uint8_t index, uint16_t value) +{ // Just keep track of motor value, actual update happens in pwmCompleteMotorUpdate() // DSHOT and some other digital protocols use 11-bit throttle range [0;2047] motors[index].value = constrain(value, 0, 2047); } -bool isMotorProtocolDshot(void) { +bool isMotorProtocolDshot(void) +{ // We look at cached `initMotorProtocol` to make sure we are consistent with the initialized config // motorConfig()->motorPwmProtocol may change at run time which will cause uninitialized structures to be used return getMotorProtocolProperties(initMotorProtocol)->isDSHOT; } -bool isMotorProtocolSerialShot(void) { +bool isMotorProtocolSerialShot(void) +{ return getMotorProtocolProperties(initMotorProtocol)->isSerialShot; } -bool isMotorProtocolDigital(void) { +bool isMotorProtocolDigital(void) +{ return isMotorProtocolDshot() || isMotorProtocolSerialShot(); } -void pwmRequestMotorTelemetry(int motorIndex) { +void pwmRequestMotorTelemetry(int motorIndex) +{ if (!isMotorProtocolDigital()) { return; } @@ -327,7 +342,8 @@ void pwmRequestMotorTelemetry(int motorIndex) { } } -void pwmCompleteMotorUpdate(void) { +void pwmCompleteMotorUpdate(void) +{ // This only makes sense for digital motor protocols if (!isMotorProtocolDigital()) { return; @@ -337,8 +353,7 @@ void pwmCompleteMotorUpdate(void) { timeUs_t currentTimeUs = micros(); // Enforce motor update rate - if ((digitalMotorUpdateIntervalUs == 0) || - ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) { + if ((digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) { return; } @@ -386,7 +401,8 @@ void pwmRequestMotorTelemetry(int motorIndex) #endif -void pwmMotorPreconfigure(void) { +void pwmMotorPreconfigure(void) +{ // Keep track of initial motor protocol initMotorProtocol = motorConfig()->motorPwmProtocol; @@ -429,86 +445,84 @@ void pwmMotorPreconfigure(void) { } } -bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bool enableOutput) { +bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bool enableOutput) +{ switch (initMotorProtocol) { - case PWM_TYPE_BRUSHED: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorConfig()->motorPwmRate, - enableOutput); - break; + case PWM_TYPE_BRUSHED: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorConfig()->motorPwmRate, enableOutput); + break; - case PWM_TYPE_ONESHOT125: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorConfig()->motorPwmRate, - enableOutput); - break; + case PWM_TYPE_ONESHOT125: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorConfig()->motorPwmRate, enableOutput); + break; - case PWM_TYPE_ONESHOT42: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorConfig()->motorPwmRate, - enableOutput); - break; + case PWM_TYPE_ONESHOT42: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorConfig()->motorPwmRate, enableOutput); + break; - case PWM_TYPE_MULTISHOT: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorConfig()->motorPwmRate, - enableOutput); - break; + case PWM_TYPE_MULTISHOT: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorConfig()->motorPwmRate, enableOutput); + break; #ifdef USE_DSHOT - case PWM_TYPE_DSHOT1200: - case PWM_TYPE_DSHOT600: - case PWM_TYPE_DSHOT300: - case PWM_TYPE_DSHOT150: - motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput); - break; + case PWM_TYPE_DSHOT1200: + case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT300: + case PWM_TYPE_DSHOT150: + motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput); + break; #endif - case PWM_TYPE_STANDARD: - motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorConfig()->motorPwmRate, - enableOutput); - break; + case PWM_TYPE_STANDARD: + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorConfig()->motorPwmRate, enableOutput); + break; - default: - motors[motorIndex].pwmPort = NULL; - break; + default: + motors[motorIndex].pwmPort = NULL; + break; } return (motors[motorIndex].pwmPort != NULL); } // Helper function for ESC passthrough -ioTag_t pwmGetMotorPinTag(int motorIndex) { +ioTag_t pwmGetMotorPinTag(int motorIndex) +{ if (motors[motorIndex].pwmPort) { return motors[motorIndex].pwmPort->tch->timHw->tag; - } else { + } + else { return IOTAG_NONE; } } -static void pwmServoWriteStandard(uint8_t index, uint16_t value) { +static void pwmServoWriteStandard(uint8_t index, uint16_t value) +{ if (servos[index]) { *servos[index]->ccr = value; } } #ifdef USE_SERVO_SBUS - -static void sbusPwmWriteStandard(uint8_t index, uint16_t value) { +static void sbusPwmWriteStandard(uint8_t index, uint16_t value) +{ pwmServoWriteStandard(index, value); sbusServoUpdate(index, value); } - #endif #ifdef USE_PWM_SERVO_DRIVER - -static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) { +static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) +{ // If PCA9685 is not detected, we do not want to write servo output anywhere if (STATE(PWM_DRIVER_AVAILABLE)) { pwmDriverSetPulse(index, value); } } - #endif -void pwmServoPreconfigure(void) { +void pwmServoPreconfigure(void) +{ // Protocol-specific configuration switch (servoConfig()->servo_protocol) { default: @@ -537,10 +551,9 @@ void pwmServoPreconfigure(void) { } } -bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, - uint16_t servoCenterPulse, bool enableOutput) { - pwmOutputPort_t *port = pwmOutConfigMotor(timerHardware, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, - servoCenterPulse, enableOutput); +bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput) +{ + pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, servoCenterPulse, enableOutput); if (port) { servos[servoIndex] = port; @@ -550,7 +563,8 @@ bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui return false; } -void pwmWriteServo(uint8_t index, uint16_t value) { +void pwmWriteServo(uint8_t index, uint16_t value) +{ if (servoWritePtr && index < MAX_SERVOS) { servoWritePtr(index, value); } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9e4f101a4f6..b57683fd93d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -399,19 +399,6 @@ static void applyFlipOverAfterCrashModeToMotors(void) { motorOutput - CRASH_FLIP_DEADBAND); motor[i] = motorOutput; - - motorValue = handleOutputScaling( - motorOutput, - throttleIdleValue, - DSHOT_DISARM_COMMAND, - motorConfig()->mincommand, - motorConfig()->maxthrottle, - DSHOT_MIN_THROTTLE, - DSHOT_3D_DEADBAND_LOW, - false - ); - - pwmWriteMotor(i,motorValue); } } else { // Disarmed mode From 4b970f5fd73e9aebc79d52f038f3f2c25ac9243c Mon Sep 17 00:00:00 2001 From: luca Date: Sun, 7 Mar 2021 01:11:24 +0100 Subject: [PATCH 107/143] Integrate all inside USE_DSHOT define --- src/main/fc/fc_core.c | 3 ++- src/main/flight/mixer.c | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 80a79d51831..49d8053d6d0 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -398,11 +398,12 @@ void disarm(disarmReason_t disarmReason) blackboxFinish(); } #endif +#ifdef USE_DSHOT if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) { changeDshotSpinRotation(DSHOT_CMD_SPIN_DIRECTION_NORMAL); DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH); } - +#endif statsOnDisarm(); logicConditionReset(); programmingPidReset(); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b57683fd93d..54a41ca7a73 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -529,10 +529,12 @@ static int getReversibleMotorsThrottleDeadband(void) void FAST_CODE mixTable(const float dT) { +#ifdef USE_DSHOT if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) { applyFlipOverAfterCrashModeToMotors(); return; } +#endif int16_t input[3]; // RPY, range [-500:+500] // Allow direct stick input to motors in passthrough mode on airplanes From 309908364c84ef144dc9a5de4ae1165c4dde620d Mon Sep 17 00:00:00 2001 From: luca Date: Sun, 7 Mar 2021 13:37:30 +0100 Subject: [PATCH 108/143] Final adjustment --- src/main/fc/controlrate_profile.h | 1 + src/main/fc/settings.yaml | 6 ++++++ src/main/flight/mixer.c | 23 ++++++++++------------- 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index b8428347aaf..34a723f2cd5 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -64,6 +64,7 @@ typedef struct controlRateConfig_s { struct { uint8_t fpvCamAngleDegrees; // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis) + uint8_t flipOverAfterPowerFactor; } misc; } controlRateConfig_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d462d72cc8d..58bdde0c7b2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1157,6 +1157,12 @@ groups: field: misc.fpvCamAngleDegrees min: 0 max: 50 + - name: flip_over_after_crash_power_factor + field: misc.flipOverAfterPowerFactor + default_value: "65" + description: "flip over after crash power factor" + min: 0 + max: 100 - name: PG_SERIAL_CONFIG type: serialConfig_t diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 54a41ca7a73..db500537c92 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -42,6 +42,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/controlrate_profile.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -115,6 +116,9 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); +#define CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND 20.0f +#define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f + typedef void (*motorRateLimitingApplyFnPtr)(const float dT); static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn; @@ -319,15 +323,10 @@ static uint16_t handleOutputScaling( } return value; } - -#define CRASH_FLIP_DEADBAND 20.0f -#define CRASH_FLIP_STICK_MINF 0.15f static void applyFlipOverAfterCrashModeToMotors(void) { - //To convert in configs - const float crashlip_expo = 35.0f; if (ARMING_FLAG(ARMED)) { - const float flipPowerFactor = 1.0f - crashlip_expo / 100.0f; + const float flipPowerFactor = ((float)currentControlRateProfile->misc.flipOverAfterPowerFactor/100.0f); const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f); const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f); const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f); @@ -342,8 +341,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) { float signPitch = rcCommand[PITCH] < 0 ? 1 : -1; float signRoll = rcCommand[ROLL] < 0 ? 1 : -1; - //float signYaw = (rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 1 : -1); - float signYaw = (rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1); + float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1)); float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs)); float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo)); @@ -374,7 +372,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) { // Apply a reasonable amount of stick deadband const float crashFlipStickMinExpo = - flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor); + flipPowerFactor * CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN + power3(CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN) * (1 - flipPowerFactor); const float flipStickRange = 1.0f - crashFlipStickMinExpo; const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange; @@ -382,7 +380,6 @@ static void applyFlipOverAfterCrashModeToMotors(void) { float motorOutputNormalised = signPitch * currentMixer[i].pitch + - //mixer, per ogni motore quanto interviene nel pitch, roll e yaw signRoll * currentMixer[i].roll + signYaw * currentMixer[i].yaw; @@ -392,11 +389,11 @@ static void applyFlipOverAfterCrashModeToMotors(void) { motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised); - float motorOutput = motorConfig()->mincommand + motorOutputNormalised * motorConfig()->maxthrottle; + float motorOutput = (float)motorConfig()->mincommand + motorOutputNormalised * (float)motorConfig()->maxthrottle; // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered - motorOutput = (motorOutput < motorConfig()->mincommand + CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : ( - motorOutput - CRASH_FLIP_DEADBAND); + motorOutput = (motorOutput < (float)motorConfig()->mincommand + CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : ( + motorOutput - CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND); motor[i] = motorOutput; } From 75d2a4314c48dc01f3ec5fe9dd2d7185120a284c Mon Sep 17 00:00:00 2001 From: luca Date: Sun, 7 Mar 2021 14:56:59 +0100 Subject: [PATCH 109/143] Final adjustment v2 --- src/main/fc/controlrate_profile.c | 3 ++- src/main/fc/fc_msp_box.c | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 49b5b2b2275..1b7e9f5d63f 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -64,7 +64,8 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) }, .misc = { - .fpvCamAngleDegrees = 0 + .fpvCamAngleDegrees = 0, + .flipOverAfterPowerFactor = 65 } ); } diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 270551dd513..452cf915bfd 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -309,7 +309,8 @@ void initActiveBoxIds(void) #endif #ifdef USE_DSHOT - activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH; + if(STATE(MULTIROTOR)) + activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH; #endif } From f81be80881cb7efa6c15376bb0b62101245b4490 Mon Sep 17 00:00:00 2001 From: luca Date: Sun, 7 Mar 2021 16:05:24 +0100 Subject: [PATCH 110/143] Removed 1 useless row --- src/main/drivers/pwm_output.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 2a757b567f0..ee57503f3a2 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -582,7 +582,6 @@ void changeDshotSpinRotation(dshotDirectionCommands_e directionSpin) { pwmRequestMotorTelemetry(i); pwmWriteMotor(i, directionSpin); } - delayMicroseconds(DSHOT_COMMAND_DELAY_US); pwmCompleteMotorUpdate(); } From 6e9ba4e899a0af04269fd75ef72cbd4a8cd806f3 Mon Sep 17 00:00:00 2001 From: luca Date: Sun, 7 Mar 2021 16:30:41 +0100 Subject: [PATCH 111/143] Settings.md updated --- docs/Settings.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Settings.md b/docs/Settings.md index d704df6aca2..2cdf5ff8f91 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -101,6 +101,7 @@ | failsafe_throttle_low_delay | 100 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout | | fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. | +| flip_over_after_crash_power_factor | 65 | flip over after crash power factor | | fpv_mix_degrees | | | | frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | | frsky_default_latitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | From 561f13bc839657ce6343254324ff2832819acae1 Mon Sep 17 00:00:00 2001 From: luca Date: Mon, 8 Mar 2021 19:18:58 +0100 Subject: [PATCH 112/143] moved flipOverAfterPowerFactor in motorConfig --- src/main/fc/controlrate_profile.c | 1 - src/main/fc/controlrate_profile.h | 1 - src/main/fc/settings.yaml | 12 ++++++------ src/main/flight/mixer.c | 7 ++++--- src/main/flight/mixer.h | 1 + 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 1b7e9f5d63f..1c633ec1bf0 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -65,7 +65,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) .misc = { .fpvCamAngleDegrees = 0, - .flipOverAfterPowerFactor = 65 } ); } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 34a723f2cd5..b8428347aaf 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -64,7 +64,6 @@ typedef struct controlRateConfig_s { struct { uint8_t fpvCamAngleDegrees; // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis) - uint8_t flipOverAfterPowerFactor; } misc; } controlRateConfig_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f36c9a50b27..c37e64cdcc9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -722,6 +722,12 @@ groups: default_value: "14" min: 4 max: 255 + - name: flip_over_after_crash_power_factor + field: flipOverAfterPowerFactor + default_value: "65" + description: "flip over after crash power factor" + min: 0 + max: 100 - name: PG_FAILSAFE_CONFIG type: failsafeConfig_t @@ -1159,12 +1165,6 @@ groups: field: misc.fpvCamAngleDegrees min: 0 max: 50 - - name: flip_over_after_crash_power_factor - field: misc.flipOverAfterPowerFactor - default_value: "65" - description: "flip over after crash power factor" - min: 0 - max: 100 - name: PG_SERIAL_CONFIG type: serialConfig_t diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index db500537c92..f5d7ba22790 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -100,7 +100,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, #define DEFAULT_MAX_THROTTLE 1850 -PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmProtocol = DEFAULT_PWM_PROTOCOL, @@ -111,7 +111,8 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorDecelTimeMs = 0, .throttleIdle = 15.0f, .throttleScale = 1.0f, - .motorPoleCount = 14 // Most brushless motors that we use are 14 poles + .motorPoleCount = 14, // Most brushless motors that we use are 14 poles + .flipOverAfterPowerFactor = 65 ); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); @@ -326,7 +327,7 @@ static uint16_t handleOutputScaling( static void applyFlipOverAfterCrashModeToMotors(void) { if (ARMING_FLAG(ARMED)) { - const float flipPowerFactor = ((float)currentControlRateProfile->misc.flipOverAfterPowerFactor/100.0f); + const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f; const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f); const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f); const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index dfe1abedb59..7f5088fafce 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -92,6 +92,7 @@ typedef struct motorConfig_s { float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent float throttleScale; // Scaling factor for throttle. uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry + uint8_t flipOverAfterPowerFactor; // Power factor from 0 to 100% of flip over after crash } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); From 0c1bc8d090addcc6000958a98a7a3fc40018d884 Mon Sep 17 00:00:00 2001 From: Tygrys-1 <38982807+Tygrys-1@users.noreply.github.com> Date: Tue, 9 Mar 2021 12:47:53 +0100 Subject: [PATCH 113/143] Update lights_io.c --- src/main/drivers/lights_io.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/lights_io.c b/src/main/drivers/lights_io.c index 1da350d0008..b6f453686eb 100644 --- a/src/main/drivers/lights_io.c +++ b/src/main/drivers/lights_io.c @@ -10,7 +10,7 @@ static IO_t lightsIO = DEFIO_IO(NONE); -bool lightsHardwareInit() +bool lightsHardwareInit(void) { lightsIO = IOGetByTag(IO_TAG(LIGHTS_PIN)); From 2ab79e01aa479bb7aa95cd0fd7f24f5b1a3c07ad Mon Sep 17 00:00:00 2001 From: Tygrys-1 <38982807+Tygrys-1@users.noreply.github.com> Date: Tue, 9 Mar 2021 12:51:24 +0100 Subject: [PATCH 114/143] Update lights_io.h --- src/main/drivers/lights_io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/lights_io.h b/src/main/drivers/lights_io.h index 521210869c5..9560320dd37 100644 --- a/src/main/drivers/lights_io.h +++ b/src/main/drivers/lights_io.h @@ -4,7 +4,7 @@ #ifdef USE_LIGHTS -bool lightsHardwareInit(); +bool lightsHardwareInit(void); void lightsHardwareSetStatus(bool status); #endif /* USE_LIGHTS */ From 3be64a3c1dbb35b9a77d45753bb75f67df7f0789 Mon Sep 17 00:00:00 2001 From: Tygrys-1 <38982807+Tygrys-1@users.noreply.github.com> Date: Tue, 9 Mar 2021 12:53:59 +0100 Subject: [PATCH 115/143] Update lights.c --- src/main/io/lights.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/lights.c b/src/main/io/lights.c index 6eb06f914d6..fa6bd2fe4a6 100644 --- a/src/main/io/lights.c +++ b/src/main/io/lights.c @@ -72,7 +72,7 @@ void lightsUpdate(timeUs_t currentTimeUs) lightsSetStatus(IS_RC_MODE_ACTIVE(BOXLIGHTS), currentTimeUs); } -void lightsInit() +void lightsInit(void) { lightsHardwareInit(); } From 3a40460cb6c98aef044727b66a35a7072553b813 Mon Sep 17 00:00:00 2001 From: Tygrys-1 <38982807+Tygrys-1@users.noreply.github.com> Date: Tue, 9 Mar 2021 12:54:28 +0100 Subject: [PATCH 116/143] Update lights.h --- src/main/io/lights.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/lights.h b/src/main/io/lights.h index 8d68ec5ffbe..935a540a811 100644 --- a/src/main/io/lights.h +++ b/src/main/io/lights.h @@ -33,6 +33,6 @@ typedef struct lightsConfig_s { PG_DECLARE(lightsConfig_t, lightsConfig); void lightsUpdate(timeUs_t currentTimeUs); -void lightsInit(); +void lightsInit(void); #endif /* USE_LIGHTS */ From a6cef5ecf6563b7f3f34fcd403429208acccc791 Mon Sep 17 00:00:00 2001 From: Tygrys-1 <38982807+Tygrys-1@users.noreply.github.com> Date: Tue, 9 Mar 2021 13:13:46 +0100 Subject: [PATCH 117/143] Update fc_core.c Fixed compilation for F3 targets, broken in d32fe6dea50a512e40a02a913e1d2ac856937333 Added #ifdef USE_PROGRAMMING_FRAMEWORK for programmingPidReset(); --- src/main/fc/fc_core.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b1fe71ca436..ced7fca4299 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -401,7 +401,9 @@ void disarm(disarmReason_t disarmReason) statsOnDisarm(); logicConditionReset(); +#ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); +#endif beeper(BEEPER_DISARMING); // emit disarm tone } } @@ -492,7 +494,9 @@ void tryArm(void) //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); logicConditionReset(); +#ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); +#endif headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); From d769ddbbac8b38f9557e77d9d483923af1767520 Mon Sep 17 00:00:00 2001 From: Tygrys-1 <38982807+Tygrys-1@users.noreply.github.com> Date: Tue, 9 Mar 2021 13:21:24 +0100 Subject: [PATCH 118/143] Update fc_core.c New branch --- src/main/fc/fc_core.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index ced7fca4299..fb040e97b71 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -494,6 +494,7 @@ void tryArm(void) //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); logicConditionReset(); + #ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); #endif From 27dbee79df1f91a97cf4c9b446688067ed05a5f2 Mon Sep 17 00:00:00 2001 From: Tony Cake Date: Tue, 9 Mar 2021 05:44:41 -0800 Subject: [PATCH 119/143] Add support for GHST GPS Telemetry --- src/main/rx/ghst_protocol.h | 9 +++-- src/main/telemetry/ghst.c | 67 ++++++++++++++++++++++++++++++++++++- 2 files changed, 73 insertions(+), 3 deletions(-) diff --git a/src/main/rx/ghst_protocol.h b/src/main/rx/ghst_protocol.h index e40986384bf..e5616c02fcf 100644 --- a/src/main/rx/ghst_protocol.h +++ b/src/main/rx/ghst_protocol.h @@ -65,6 +65,8 @@ typedef enum { GHST_DL_LINK_STAT = 0x21, GHST_DL_VTX_STAT = 0x22, GHST_DL_PACK_STAT = 0x23, // Battery (Pack) Status + GHST_DL_GPS_PRIMARY = 0x25, // Primary GPS data (position) + GHST_DL_GPS_SECONDARY = 0x26 // Secondary GPS data (auxiliary) } ghstDl_e; #define GHST_RC_CTR_VAL_12BIT 0x7C0 // servo center for 12 bit values (0x3e0 << 1) @@ -72,9 +74,12 @@ typedef enum { #define GHST_FRAME_SIZE 14 // including addr, type, len, crc, and payload -#define GHST_PAYLOAD_SIZE_MAX 14 +#define GHST_PAYLOAD_SIZE_MAX 14 -#define GHST_FRAME_SIZE_MAX 24 +#define GHST_FRAME_SIZE_MAX 24 + +#define GPS_FLAGS_FIX 0x01 +#define GPS_FLAGS_FIX_HOME 0x02 typedef struct ghstFrameDef_s { uint8_t addr; diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index dc9c02579e0..17c5bce1086 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -43,6 +43,7 @@ #include "drivers/nvic.h" +#include "fc/config.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -51,6 +52,8 @@ #include "io/gps.h" #include "io/serial.h" +#include "navigation/navigation.h" + #include "rx/rx.h" #include "rx/ghst.h" #include "rx/ghst_protocol.h" @@ -67,6 +70,7 @@ #define GHST_CYCLETIME_US 100000 // 10x/sec #define GHST_FRAME_PACK_PAYLOAD_SIZE 10 +#define GHST_FRAME_GPS_PAYLOAD_SIZE 10 #define GHST_FRAME_LENGTH_CRC 1 #define GHST_FRAME_LENGTH_TYPE 1 @@ -112,11 +116,51 @@ void ghstFramePackTelemetry(sbuf_t *dst) sbufWriteU8(dst, 0x00); // tbd3 } + +// GPS data, primary, positional data +void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE); + sbufWriteU8(dst, GHST_DL_GPS_PRIMARY); + + sbufWriteU32(dst, gpsSol.llh.lat); + sbufWriteU32(dst, gpsSol.llh.lon); + + // constrain alt. from -32,000m to +32,000m, units of meters + const int16_t altitude = (constrain(getEstimatedActualPosition(Z), -32000 * 100, 32000 * 100) / 100); + sbufWriteU16(dst, altitude); +.} + +// GPS data, secondary, auxiliary data +void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE); + sbufWriteU8(dst, GHST_DL_GPS_SECONDARY); + + sbufWriteU16(dst, gpsSol.groundSpeed); // speed in 0.1m/s + sbufWriteU16(dst, gpsSol.groundCourse); // degrees * 10 + sbufWriteU8(dst, gpsSol.numSat); + + sbufWriteU16(dst, (uint16_t) (GPS_distanceToHome / 10)); // use units of 10m to increase range of U16 to 655.36km + sbufWriteU16(dst, GPS_directionToHome); + + uint8_t gpsFlags = 0; + if(STATE(GPS_FIX)) + gpsFlags |= GPS_FLAGS_FIX; + if(STATE(GPS_FIX_HOME)) + gpsFlags |= GPS_FLAGS_FIX_HOME; + sbufWriteU8(dst, gpsFlags); +} + // schedule array to decide how often each type of frame is sent typedef enum { GHST_FRAME_START_INDEX = 0, GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data - GHST_SCHEDULE_COUNT_MAX + GHST_FRAME_GPS_PRIMARY_INDEX, // GPS, primary values (Lat, Long, Alt) + GHST_FRAME_GPS_SECONDARY_INDEX, // GPS, secondary values (Sat Count, HDOP, etc.) + GHST_SCHEDULE_COUNT_MAX } ghstFrameTypeIndex_e; static uint8_t ghstScheduleCount; @@ -136,6 +180,19 @@ static void processGhst(void) ghstFramePackTelemetry(dst); ghstFinalize(dst); } + + if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) { + ghstInitializeFrame(dst); + ghstFrameGpsPrimaryTelemetry(dst); + ghstFinalize(dst); + } + + if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) { + ghstInitializeFrame(dst); + ghstFrameGpsSecondaryTelemetry(dst); + ghstFinalize(dst); + } + ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount; } @@ -152,6 +209,14 @@ void initGhstTelemetry(void) if (isBatteryVoltageConfigured() || isAmperageConfigured()) { ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX); } + +#ifdef USE_GPS + if (feature(FEATURE_GPS)) { + ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX); + ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX); + } +#endif + ghstScheduleCount = (uint8_t)index; } From 3294b793a1341d1e165bcf65aeaebb7a949c3601 Mon Sep 17 00:00:00 2001 From: Tony Cake Date: Tue, 9 Mar 2021 06:11:40 -0800 Subject: [PATCH 120/143] Fix careless typo --- src/main/telemetry/ghst.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index 17c5bce1086..c2aca3ac512 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -130,7 +130,7 @@ void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst) // constrain alt. from -32,000m to +32,000m, units of meters const int16_t altitude = (constrain(getEstimatedActualPosition(Z), -32000 * 100, 32000 * 100) / 100); sbufWriteU16(dst, altitude); -.} +} // GPS data, secondary, auxiliary data void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) From 2a5ce2e0b31ff2eff44b273d4dd37567058baa50 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 9 Mar 2021 22:08:07 +0100 Subject: [PATCH 121/143] Do not slow down in WP mission when approaching a waypoint --- src/main/navigation/navigation_multicopter.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index adfdb28ec0d..bb8d4d51052 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -438,7 +438,19 @@ static void updatePositionVelocityController_MC(const float maxSpeed) // Scale velocity to respect max_speed float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY)); - if (newVelTotal > maxSpeed) { + + /* + * We override computed speed with max speed in following cases: + * 1 - computed velocity is > maxSpeed + * 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed + */ + if ( + (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && + !isApproachingLastWaypoint() && + newVelTotal < maxSpeed && + !navConfig()->mc.slowDownForTurning + ) || newVelTotal > maxSpeed + ) { newVelX = maxSpeed * (newVelX / newVelTotal); newVelY = maxSpeed * (newVelY / newVelTotal); newVelTotal = maxSpeed; From 7363df0a7926095541a90d95074dc12bfd393024 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 9 Mar 2021 22:15:52 +0100 Subject: [PATCH 122/143] Fix obvious complitation error --- src/main/navigation/navigation_multicopter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index bb8d4d51052..a0beb4149c5 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -445,7 +445,7 @@ static void updatePositionVelocityController_MC(const float maxSpeed) * 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed */ if ( - (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && + (navGetCurrentStateFlags() & NAV_AUTO_WP && !isApproachingLastWaypoint() && newVelTotal < maxSpeed && !navConfig()->mc.slowDownForTurning From 7fa82f0a2faf2d7887a6044ad0ecb0bad2c00c1f Mon Sep 17 00:00:00 2001 From: Tygrys-1 <38982807+Tygrys-1@users.noreply.github.com> Date: Wed, 10 Mar 2021 11:17:31 +0100 Subject: [PATCH 123/143] Update target.c Fix timer and DMA conflicts in the target, resolves #6303 --- src/main/target/DALRCF405/target.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/DALRCF405/target.c b/src/main/target/DALRCF405/target.c index a8d8deb8f34..1dbe7eac09c 100644 --- a/src/main/target/DALRCF405/target.c +++ b/src/main/target/DALRCF405/target.c @@ -28,9 +28,9 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 (2,2) DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 (2,6) DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 (2,1) (2.3 2.6) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 (2,4) (2.2) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 (1,2) - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 (1,5) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S5 (2,4) (2.2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 (1,2) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S7 (2,3) DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 (2,7) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED STRIP(1,0) From 0132c471c95ba9de6001377b1de625b7895e0403 Mon Sep 17 00:00:00 2001 From: luca Date: Wed, 10 Mar 2021 17:09:33 +0100 Subject: [PATCH 124/143] Show turtle mode in configurator only if dshot is active --- src/main/fc/fc_msp_box.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 452cf915bfd..8734305d480 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -33,6 +33,8 @@ #include "io/osd.h" +#include "drivers/pwm_output.h" + #include "sensors/diagnostics.h" #include "sensors/sensors.h" @@ -309,7 +311,7 @@ void initActiveBoxIds(void) #endif #ifdef USE_DSHOT - if(STATE(MULTIROTOR)) + if(STATE(MULTIROTOR) && isMotorProtocolDshot()) activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH; #endif } From 2eecf30603811d8517748c73081424174f404134 Mon Sep 17 00:00:00 2001 From: Mingchen Zhang Date: Thu, 11 Mar 2021 08:12:49 +0000 Subject: [PATCH 125/143] Add modulus operator support --- docs/Programming Framework.md | 1 + src/main/programming/logic_condition.c | 8 +++++ src/main/programming/logic_condition.h | 47 +++++++++++++------------- 3 files changed, 33 insertions(+), 23 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 173e613901e..bc3eea0c69c 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -51,6 +51,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 15 | SUB | Substract `Operand B` from `Operand A` and returns the result | | 16 | MUL | Multiply `Operand A` by `Operand B` and returns the result | | 17 | DIV | Divide `Operand A` by `Operand B` and returns the result | +| 17 | MOD | Divide `Operand A` by `Operand B` and returns the remainder | | 18 | GVAR SET | Store value from `Operand B` into the Global Variable addressed by `Operand B`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | | 19 | GVAR INC | Increase the GVAR indexed by `Operand A` with value from `Operand B` | | 20 | GVAR DEC | Decrease the GVAR indexed by `Operand A` with value from `Operand B` | diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 47ca866cb83..ea006bafeec 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -193,6 +193,14 @@ static int logicConditionCompute( return operandA; } break; + + case LOGIC_CONDITION_MODULUS: + if (operandB != 0) { + return constrain(operandA % operandB, INT16_MIN, INT16_MAX); + } else { + return operandA; + } + break; case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY); diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index f6d68307038..ca4d17b984e 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -47,29 +47,30 @@ typedef enum { LOGIC_CONDITION_SUB = 15, LOGIC_CONDITION_MUL = 16, LOGIC_CONDITION_DIV = 17, - LOGIC_CONDITION_GVAR_SET = 18, - LOGIC_CONDITION_GVAR_INC = 19, - LOGIC_CONDITION_GVAR_DEC = 20, - LOGIC_CONDITION_PORT_SET = 21, - LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY = 22, - LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE = 23, - LOGIC_CONDITION_SWAP_ROLL_YAW = 24, - LOGIC_CONDITION_SET_VTX_POWER_LEVEL = 25, - LOGIC_CONDITION_INVERT_ROLL = 26, - LOGIC_CONDITION_INVERT_PITCH = 27, - LOGIC_CONDITION_INVERT_YAW = 28, - LOGIC_CONDITION_OVERRIDE_THROTTLE = 29, - LOGIC_CONDITION_SET_VTX_BAND = 30, - LOGIC_CONDITION_SET_VTX_CHANNEL = 31, - LOGIC_CONDITION_SET_OSD_LAYOUT = 32, - LOGIC_CONDITION_SIN = 33, - LOGIC_CONDITION_COS = 34, - LOGIC_CONDITION_TAN = 35, - LOGIC_CONDITION_MAP_INPUT = 36, - LOGIC_CONDITION_MAP_OUTPUT = 37, - LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38, - LOGIC_CONDITION_SET_HEADING_TARGET = 39, - LOGIC_CONDITION_LAST = 40, + LOGIC_CONDITION_MODULUS = 18, + LOGIC_CONDITION_GVAR_SET = 19, + LOGIC_CONDITION_GVAR_INC = 20, + LOGIC_CONDITION_GVAR_DEC = 21, + LOGIC_CONDITION_PORT_SET = 22, + LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY = 23, + LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE = 24, + LOGIC_CONDITION_SWAP_ROLL_YAW = 25, + LOGIC_CONDITION_SET_VTX_POWER_LEVEL = 26, + LOGIC_CONDITION_INVERT_ROLL = 27, + LOGIC_CONDITION_INVERT_PITCH = 28, + LOGIC_CONDITION_INVERT_YAW = 29, + LOGIC_CONDITION_OVERRIDE_THROTTLE = 30, + LOGIC_CONDITION_SET_VTX_BAND = 31, + LOGIC_CONDITION_SET_VTX_CHANNEL = 32, + LOGIC_CONDITION_SET_OSD_LAYOUT = 33, + LOGIC_CONDITION_SIN = 34, + LOGIC_CONDITION_COS = 35, + LOGIC_CONDITION_TAN = 36, + LOGIC_CONDITION_MAP_INPUT = 37, + LOGIC_CONDITION_MAP_OUTPUT = 38, + LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 39, + LOGIC_CONDITION_SET_HEADING_TARGET = 40, + LOGIC_CONDITION_LAST = 41, } logicOperation_e; typedef enum logicOperandType_s { From cce8345d9056d62752e304fe2a61f7104b637f0e Mon Sep 17 00:00:00 2001 From: Mingchen Zhang Date: Fri, 12 Mar 2021 02:14:22 +0000 Subject: [PATCH 126/143] move the modulus to the last entry --- docs/Programming Framework.md | 2 +- src/main/programming/logic_condition.c | 16 ++++----- src/main/programming/logic_condition.h | 46 +++++++++++++------------- 3 files changed, 32 insertions(+), 32 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index bc3eea0c69c..6d258ba8c97 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -51,7 +51,6 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 15 | SUB | Substract `Operand B` from `Operand A` and returns the result | | 16 | MUL | Multiply `Operand A` by `Operand B` and returns the result | | 17 | DIV | Divide `Operand A` by `Operand B` and returns the result | -| 17 | MOD | Divide `Operand A` by `Operand B` and returns the remainder | | 18 | GVAR SET | Store value from `Operand B` into the Global Variable addressed by `Operand B`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | | 19 | GVAR INC | Increase the GVAR indexed by `Operand A` with value from `Operand B` | | 20 | GVAR DEC | Decrease the GVAR indexed by `Operand A` with value from `Operand B` | @@ -74,6 +73,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | | 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` | | 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. | +| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder | ### Operands diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index ea006bafeec..b1b9cefaee4 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -193,14 +193,6 @@ static int logicConditionCompute( return operandA; } break; - - case LOGIC_CONDITION_MODULUS: - if (operandB != 0) { - return constrain(operandA % operandB, INT16_MIN, INT16_MAX); - } else { - return operandA; - } - break; case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY); @@ -332,6 +324,14 @@ static int logicConditionCompute( return temporaryValue; break; + case LOGIC_CONDITION_MODULUS: + if (operandB != 0) { + return constrain(operandA % operandB, INT16_MIN, INT16_MAX); + } else { + return operandA; + } + break; + default: return false; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index ca4d17b984e..28cbdbb4dc0 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -47,29 +47,29 @@ typedef enum { LOGIC_CONDITION_SUB = 15, LOGIC_CONDITION_MUL = 16, LOGIC_CONDITION_DIV = 17, - LOGIC_CONDITION_MODULUS = 18, - LOGIC_CONDITION_GVAR_SET = 19, - LOGIC_CONDITION_GVAR_INC = 20, - LOGIC_CONDITION_GVAR_DEC = 21, - LOGIC_CONDITION_PORT_SET = 22, - LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY = 23, - LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE = 24, - LOGIC_CONDITION_SWAP_ROLL_YAW = 25, - LOGIC_CONDITION_SET_VTX_POWER_LEVEL = 26, - LOGIC_CONDITION_INVERT_ROLL = 27, - LOGIC_CONDITION_INVERT_PITCH = 28, - LOGIC_CONDITION_INVERT_YAW = 29, - LOGIC_CONDITION_OVERRIDE_THROTTLE = 30, - LOGIC_CONDITION_SET_VTX_BAND = 31, - LOGIC_CONDITION_SET_VTX_CHANNEL = 32, - LOGIC_CONDITION_SET_OSD_LAYOUT = 33, - LOGIC_CONDITION_SIN = 34, - LOGIC_CONDITION_COS = 35, - LOGIC_CONDITION_TAN = 36, - LOGIC_CONDITION_MAP_INPUT = 37, - LOGIC_CONDITION_MAP_OUTPUT = 38, - LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 39, - LOGIC_CONDITION_SET_HEADING_TARGET = 40, + LOGIC_CONDITION_GVAR_SET = 18, + LOGIC_CONDITION_GVAR_INC = 19, + LOGIC_CONDITION_GVAR_DEC = 20, + LOGIC_CONDITION_PORT_SET = 21, + LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY = 22, + LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE = 23, + LOGIC_CONDITION_SWAP_ROLL_YAW = 24, + LOGIC_CONDITION_SET_VTX_POWER_LEVEL = 25, + LOGIC_CONDITION_INVERT_ROLL = 26, + LOGIC_CONDITION_INVERT_PITCH = 27, + LOGIC_CONDITION_INVERT_YAW = 28, + LOGIC_CONDITION_OVERRIDE_THROTTLE = 29, + LOGIC_CONDITION_SET_VTX_BAND = 30, + LOGIC_CONDITION_SET_VTX_CHANNEL = 31, + LOGIC_CONDITION_SET_OSD_LAYOUT = 32, + LOGIC_CONDITION_SIN = 33, + LOGIC_CONDITION_COS = 34, + LOGIC_CONDITION_TAN = 35, + LOGIC_CONDITION_MAP_INPUT = 36, + LOGIC_CONDITION_MAP_OUTPUT = 37, + LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38, + LOGIC_CONDITION_SET_HEADING_TARGET = 39, + LOGIC_CONDITION_MODULUS = 40, LOGIC_CONDITION_LAST = 41, } logicOperation_e; From 2e2d48655cb8996cb7211c528e3d8ece4075106d Mon Sep 17 00:00:00 2001 From: Jeff Kubascik Date: Thu, 11 Mar 2021 23:08:20 -0500 Subject: [PATCH 127/143] Enable report_cell_voltage support for CRSF telemetry --- src/main/telemetry/crsf.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 8ac00c39f93..d72175cf248 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -246,7 +246,11 @@ static void crsfFrameBatterySensor(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); - crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V + if (telemetryConfig()->report_cell_voltage) { + crsfSerialize16(dst, getBatteryAverageCellVoltage() / 10); + } else { + crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V + } crsfSerialize16(dst, getAmperage() / 10); const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); crsfSerialize8(dst, (getMAhDrawn() >> 16)); From ed13700d1c08a7cc2dc38146682c7158d53860a6 Mon Sep 17 00:00:00 2001 From: luca Date: Sun, 14 Mar 2021 20:37:32 +0100 Subject: [PATCH 128/143] Circular queue in common libs --- src/main/common/circular_queue.c | 42 ++++++ src/main/common/circular_queue.h | 23 ++++ src/test/unit/CMakeLists.txt | 2 + src/test/unit/circular_queue_unittest.cc | 166 +++++++++++++++++++++++ 4 files changed, 233 insertions(+) create mode 100644 src/main/common/circular_queue.c create mode 100644 src/main/common/circular_queue.h create mode 100644 src/test/unit/circular_queue_unittest.cc diff --git a/src/main/common/circular_queue.c b/src/main/common/circular_queue.c new file mode 100644 index 00000000000..c7e2b6fffc4 --- /dev/null +++ b/src/main/common/circular_queue.c @@ -0,0 +1,42 @@ +#include "circular_queue.h" + +void circularBufferInit(circularBuffer_t *circular_buffer, uint8_t *buffer, size_t buffer_size, + size_t buffer_element_size) { + circular_buffer->buffer = buffer; + circular_buffer->bufferSize = buffer_size; + circular_buffer->elementSize = buffer_element_size; + circular_buffer->head = 0; + circular_buffer->tail = 0; + circular_buffer->size = 0; +} + +void circularBufferPushElement(circularBuffer_t *circularBuffer, uint8_t *element) { + if (circularBufferIsFull(circularBuffer)) + return; + + memcpy(circularBuffer->buffer + circularBuffer->tail, element, circularBuffer->elementSize); + + circularBuffer->tail += circularBuffer->elementSize; + circularBuffer->tail %= circularBuffer->bufferSize; + circularBuffer->size += 1; +} + +void circularBufferPopHead(circularBuffer_t *circularBuffer, uint8_t *element) { + memcpy(element, circularBuffer->buffer + circularBuffer->head, circularBuffer->elementSize); + + circularBuffer->head += circularBuffer->elementSize; + circularBuffer->head %= circularBuffer->bufferSize; + circularBuffer->size -= 1; +} + +uint8_t circularBufferIsFull(circularBuffer_t *circularBuffer) { + return circularBufferCountElements(circularBuffer) * circularBuffer->elementSize == circularBuffer->bufferSize; +} + +uint8_t circularBufferIsEmpty(circularBuffer_t *circularBuffer) { + return circularBufferCountElements(circularBuffer) == 0; +} + +size_t circularBufferCountElements(circularBuffer_t *circularBuffer) { + return circularBuffer->size; +} \ No newline at end of file diff --git a/src/main/common/circular_queue.h b/src/main/common/circular_queue.h new file mode 100644 index 00000000000..0ff716f9a16 --- /dev/null +++ b/src/main/common/circular_queue.h @@ -0,0 +1,23 @@ +#ifndef INAV_CIRCULAR_QUEUE_H +#define INAV_CIRCULAR_QUEUE_H + +#include "stdint.h" +#include "string.h" + +typedef struct circularBuffer_s{ + size_t head; + size_t tail; + size_t bufferSize; + uint8_t * buffer; + size_t elementSize; + size_t size; +}circularBuffer_t; + +void circularBufferInit(circularBuffer_t * circularBuffer, uint8_t * buffer, size_t bufferSize, size_t bufferElementSize); +void circularBufferPushElement(circularBuffer_t * circularBuffer, uint8_t * element); +void circularBufferPopHead(circularBuffer_t * circularBuffer, uint8_t * element); +uint8_t circularBufferIsFull(circularBuffer_t * circularBuffer); +uint8_t circularBufferIsEmpty(circularBuffer_t *circularBuffer); +size_t circularBufferCountElements(circularBuffer_t * circularBuffer); + +#endif //INAV_CIRCULAR_QUEUE_H diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 42567db71a1..d1aebcd5e02 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -27,6 +27,8 @@ set_property(SOURCE telemetry_hott_unittest.cc PROPERTY depends set_property(SOURCE time_unittest.cc PROPERTY depends "drivers/time.c") +set_property(SOURCE circular_queue_unittest.cc PROPERTY depends "common/circular_queue.c") + function(unit_test src) get_filename_component(basename ${src} NAME) string(REPLACE ".cc" "" name ${basename} ) diff --git a/src/test/unit/circular_queue_unittest.cc b/src/test/unit/circular_queue_unittest.cc new file mode 100644 index 00000000000..64dd5d04880 --- /dev/null +++ b/src/test/unit/circular_queue_unittest.cc @@ -0,0 +1,166 @@ +#include "unittest_macros.h" +#include "gtest/gtest.h" +#include + +#include +#include + +extern "C" { + #include "common/circular_queue.h" +} +#include +#include + + +TEST(CircularQueue, ElementsAreCorrect){ + srand (time(NULL)); + + circularBuffer_t buffer; + + const size_t bufferSize = 16; + uint8_t buff[bufferSize]; + + circularBufferInit(&buffer, buff, bufferSize, sizeof(char)); + + const uint8_t testBurst = 3; + + const uint8_t dataToInsertSize[testBurst] = {5, 10, 3}; + const uint8_t dataToRemoveSize[testBurst] = {3, 7, 2}; + + std::queue queue; + + for(uint8_t j=0; jqueue; + + EXPECT_EQ(circularBufferIsEmpty(&buffer),true); + EXPECT_EQ(circularBufferIsFull(&buffer),false); + + for(uint8_t j=0; jqueue; + + EXPECT_EQ(circularBufferIsEmpty(&buffer),true); + EXPECT_EQ(circularBufferIsFull(&buffer),false); + + for(uint8_t j=0; j Date: Sun, 14 Mar 2021 20:39:56 +0100 Subject: [PATCH 129/143] DShot commands with queue --- src/main/CMakeLists.txt | 2 ++ src/main/drivers/pwm_output.c | 52 ++++++++++++++++++++++++++++------- src/main/drivers/pwm_output.h | 5 ++-- src/main/fc/fc_core.c | 5 ++-- src/main/fc/fc_init.c | 4 +++ src/main/fc/settings.yaml | 1 + 6 files changed, 54 insertions(+), 15 deletions(-) diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index d37646abc49..676a2822c66 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -48,6 +48,8 @@ main_sources(COMMON_SRC common/typeconversion.h common/uvarint.c common/uvarint.h + common/circular_queue.c + common/circular_queue.h config/config_eeprom.c config/config_eeprom.h diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index ee57503f3a2..71bc4f1b4b0 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SPEED #include "common/log.h" #include "common/maths.h" +#include "common/circular_queue.h" #include "drivers/io.h" #include "drivers/timer.h" @@ -61,7 +62,8 @@ FILE_COMPILE_FOR_SPEED #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ -#define DSHOT_COMMAND_DELAY_US 1000 +#define DSHOT_COMMAND_INTERVAL_US 1000 +#define DSHOT_COMMAND_QUEUE_SIZE 16 #endif typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors @@ -112,6 +114,12 @@ static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; +#ifdef USE_DSHOT + circularBuffer_t commandsCircularBuffer; + uint8_t commandsBuff[DSHOT_COMMAND_QUEUE_SIZE * sizeof(dshotCommands_e)]; + timeUs_t lastCommandSent = 0; +#endif + static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) { p->tch = tch; @@ -361,6 +369,22 @@ void pwmCompleteMotorUpdate(void) #ifdef USE_DSHOT if (isMotorProtocolDshot()) { + + //Check if there are commands to send + if(!circularBufferIsEmpty(&commandsCircularBuffer) && + micros() - lastCommandSent > DSHOT_COMMAND_INTERVAL_US){ + + dshotCommands_e cmd; + circularBufferPopHead(&commandsCircularBuffer,(uint8_t*) &cmd); + + for (uint8_t i = 0; i < getMotorCount(); i++) { + pwmRequestMotorTelemetry(i); + pwmWriteMotor(i, cmd); + } + lastCommandSent = micros(); + } + + // Generate DMA buffers for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { @@ -571,21 +595,29 @@ void pwmWriteServo(uint8_t index, uint16_t value) } #ifdef USE_DSHOT -void changeDshotSpinRotation(dshotDirectionCommands_e directionSpin) { - const uint8_t number_of_packets = 10; +void sendDShotCommand(dshotCommands_e cmd) { - for (uint8_t j = 0; j < number_of_packets; j++) { + uint8_t repeats = 1; - delayMicroseconds(DSHOT_COMMAND_DELAY_US); - for (uint8_t i = 0; i < getMotorCount(); i++) { - pwmRequestMotorTelemetry(i); - pwmWriteMotor(i, directionSpin); - } - pwmCompleteMotorUpdate(); + //Some commands must be repeated more times + switch (cmd) { + case DSHOT_CMD_SPIN_DIRECTION_NORMAL: + case DSHOT_CMD_SPIN_DIRECTION_REVERSED: + repeats = 10; + break; + + default: + break; + } + for(uint8_t i = 0; i Date: Thu, 18 Mar 2021 21:35:18 +0100 Subject: [PATCH 130/143] Some optimizations --- src/main/common/circular_queue.c | 30 ++++++++- src/main/common/circular_queue.h | 28 +++++++- src/main/drivers/pwm_output.c | 106 +++++++++++++++++-------------- src/main/drivers/pwm_output.h | 5 ++ src/main/fc/fc_core.c | 2 +- 5 files changed, 119 insertions(+), 52 deletions(-) diff --git a/src/main/common/circular_queue.c b/src/main/common/circular_queue.c index c7e2b6fffc4..2a3c8a8bc2c 100644 --- a/src/main/common/circular_queue.c +++ b/src/main/common/circular_queue.c @@ -1,3 +1,27 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + #include "circular_queue.h" void circularBufferInit(circularBuffer_t *circular_buffer, uint8_t *buffer, size_t buffer_size, @@ -29,12 +53,12 @@ void circularBufferPopHead(circularBuffer_t *circularBuffer, uint8_t *element) { circularBuffer->size -= 1; } -uint8_t circularBufferIsFull(circularBuffer_t *circularBuffer) { +int circularBufferIsFull(circularBuffer_t *circularBuffer) { return circularBufferCountElements(circularBuffer) * circularBuffer->elementSize == circularBuffer->bufferSize; } -uint8_t circularBufferIsEmpty(circularBuffer_t *circularBuffer) { - return circularBufferCountElements(circularBuffer) == 0; +int circularBufferIsEmpty(circularBuffer_t *circularBuffer) { + return circularBuffer==NULL || circularBufferCountElements(circularBuffer) == 0; } size_t circularBufferCountElements(circularBuffer_t *circularBuffer) { diff --git a/src/main/common/circular_queue.h b/src/main/common/circular_queue.h index 0ff716f9a16..94bfaffa0da 100644 --- a/src/main/common/circular_queue.h +++ b/src/main/common/circular_queue.h @@ -1,3 +1,27 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 program. If not, see http://www.gnu.org/licenses/. + */ + #ifndef INAV_CIRCULAR_QUEUE_H #define INAV_CIRCULAR_QUEUE_H @@ -16,8 +40,8 @@ typedef struct circularBuffer_s{ void circularBufferInit(circularBuffer_t * circularBuffer, uint8_t * buffer, size_t bufferSize, size_t bufferElementSize); void circularBufferPushElement(circularBuffer_t * circularBuffer, uint8_t * element); void circularBufferPopHead(circularBuffer_t * circularBuffer, uint8_t * element); -uint8_t circularBufferIsFull(circularBuffer_t * circularBuffer); -uint8_t circularBufferIsEmpty(circularBuffer_t *circularBuffer); +int circularBufferIsFull(circularBuffer_t * circularBuffer); +int circularBufferIsEmpty(circularBuffer_t *circularBuffer); size_t circularBufferCountElements(circularBuffer_t * circularBuffer); #endif //INAV_CIRCULAR_QUEUE_H diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 71bc4f1b4b0..48f7a10d5c9 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -63,7 +63,8 @@ FILE_COMPILE_FOR_SPEED #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ #define DSHOT_COMMAND_INTERVAL_US 1000 -#define DSHOT_COMMAND_QUEUE_SIZE 16 +#define DSHOT_COMMAND_QUEUE_LENGTH 8 +#define DHSOT_COMMAND_QUEUE_SIZE DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e) #endif typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors @@ -115,9 +116,9 @@ static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; #ifdef USE_DSHOT - circularBuffer_t commandsCircularBuffer; - uint8_t commandsBuff[DSHOT_COMMAND_QUEUE_SIZE * sizeof(dshotCommands_e)]; - timeUs_t lastCommandSent = 0; +static circularBuffer_t commandsCircularBuffer; +static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; +static currentExecutingCommand_t currentExecutingCommand; #endif static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) @@ -350,8 +351,60 @@ void pwmRequestMotorTelemetry(int motorIndex) } } -void pwmCompleteMotorUpdate(void) -{ +#ifdef USE_DSHOT +void sendDShotCommand(dshotCommands_e cmd) { + circularBufferPushElement(&commandsCircularBuffer, (uint8_t *) &cmd); +} + +void initDShotCommands(void) { + circularBufferInit(&commandsCircularBuffer, commandsBuff,DHSOT_COMMAND_QUEUE_SIZE, sizeof(dshotCommands_e)); + + currentExecutingCommand.remainingRepeats = 0; +} + +static int getDShotCommandRepeats(dshotCommands_e cmd) { + int repeats = 1; + + switch (cmd) { + case DSHOT_CMD_SPIN_DIRECTION_NORMAL: + case DSHOT_CMD_SPIN_DIRECTION_REVERSED: + repeats = 6; + break; + default: + break; + } + + return repeats; +} + +static void executeDShotCommands(void){ + + if(currentExecutingCommand.remainingRepeats == 0) { + + const int isTherePendingCommands = !circularBufferIsEmpty(&commandsCircularBuffer); + + if (isTherePendingCommands) { + //Load the command + dshotCommands_e cmd; + circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &cmd); + + currentExecutingCommand.cmd = cmd; + currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd); + } + else { + return; + } + } + + for (uint8_t i = 0; i < getMotorCount(); i++) { + motors[i].requestTelemetry = true; + motors[i].value = currentExecutingCommand.cmd; + } + currentExecutingCommand.remainingRepeats--; +} +#endif + +void pwmCompleteMotorUpdate(void) { // This only makes sense for digital motor protocols if (!isMotorProtocolDigital()) { return; @@ -370,20 +423,7 @@ void pwmCompleteMotorUpdate(void) #ifdef USE_DSHOT if (isMotorProtocolDshot()) { - //Check if there are commands to send - if(!circularBufferIsEmpty(&commandsCircularBuffer) && - micros() - lastCommandSent > DSHOT_COMMAND_INTERVAL_US){ - - dshotCommands_e cmd; - circularBufferPopHead(&commandsCircularBuffer,(uint8_t*) &cmd); - - for (uint8_t i = 0; i < getMotorCount(); i++) { - pwmRequestMotorTelemetry(i); - pwmWriteMotor(i, cmd); - } - lastCommandSent = micros(); - } - + executeDShotCommands(); // Generate DMA buffers for (int index = 0; index < motorCount; index++) { @@ -594,32 +634,6 @@ void pwmWriteServo(uint8_t index, uint16_t value) } } -#ifdef USE_DSHOT - -void sendDShotCommand(dshotCommands_e cmd) { - - uint8_t repeats = 1; - - //Some commands must be repeated more times - switch (cmd) { - case DSHOT_CMD_SPIN_DIRECTION_NORMAL: - case DSHOT_CMD_SPIN_DIRECTION_REVERSED: - repeats = 10; - break; - - default: - break; - } - - for(uint8_t i = 0; i Date: Fri, 19 Mar 2021 22:39:22 +0100 Subject: [PATCH 131/143] detect launch on gps speed --- src/main/navigation/navigation_fw_launch.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 0b3c1633bed..2ddc0e56bcb 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -316,10 +316,11 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); + const bool isForwardLaunched = (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh); applyThrottleIdleLogic(false); - if (isBungeeLaunched || isSwingLaunched) { + if (isBungeeLaunched || isSwingLaunched || isForwardLaunched) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED } From 749111df549e5e22da61243a7ff5309c087caa84 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Fri, 19 Mar 2021 22:46:43 +0100 Subject: [PATCH 132/143] add acceleration check --- src/main/navigation/navigation_fw_launch.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 2ddc0e56bcb..8dc9cb68a91 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -316,7 +316,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - const bool isForwardLaunched = (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh); + const bool isForwardLaunched = (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); applyThrottleIdleLogic(false); From 23ef95f17e9076864cf08cf58b10465a9756a3d4 Mon Sep 17 00:00:00 2001 From: luca Date: Sat, 20 Mar 2021 18:37:18 +0100 Subject: [PATCH 133/143] Failsafe procedure from OSD --- src/main/cms/cms_menu_misc.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 15916826fa0..11809325051 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -47,6 +47,7 @@ static const OSD_Entry menuMiscEntries[]= OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), #endif + OSD_SETTING_ENTRY("FS PROCEDURE", SETTING_FAILSAFE_PROCEDURE), OSD_BACK_AND_END_ENTRY, }; From 6e9ccb93fcbd1540cb12cd05a4f30f1151387e67 Mon Sep 17 00:00:00 2001 From: luca Date: Sat, 20 Mar 2021 19:35:42 +0100 Subject: [PATCH 134/143] trigger GitHub actions From c2a98768c0ae0362e00d60f6b595a5fce0dcbcb2 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 20 Mar 2021 20:49:48 +0100 Subject: [PATCH 135/143] check gps --- src/main/navigation/navigation_fw_launch.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 8dc9cb68a91..6db4f8f8496 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -48,6 +48,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "io/gps.h" + #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms #define UNUSED(x) ((void)(x)) @@ -316,7 +318,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - const bool isForwardLaunched = (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); + const bool isForwardLaunched = isGPSHeadingValid() && (imuMeasuredAccelBF.x > 0); applyThrottleIdleLogic(false); From 3a82e26b8bdefa00df6b30aaee78ab703778508b Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 22 Mar 2021 21:58:23 +0100 Subject: [PATCH 136/143] add groundspeed check --- src/main/navigation/navigation_fw_launch.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 6db4f8f8496..66ee070c1f0 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -318,7 +318,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - const bool isForwardLaunched = isGPSHeadingValid() && (imuMeasuredAccelBF.x > 0); + const bool isForwardLaunched = isGPSHeadingValid() && (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); applyThrottleIdleLogic(false); From 0af4298d136e9de2aa3b5a3e661f03b60e191138 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Thu, 25 Mar 2021 20:30:55 +0000 Subject: [PATCH 137/143] fix incorrect sdmmc_sdio_f4xx error state (#6748) --- src/main/drivers/sdcard/sdmmc_sdio_f4xx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c index 2734de00adc..592ce4cf641 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c @@ -1081,7 +1081,7 @@ SD_Error_t SD_GetStatus(void) } } else { - ErrorState = SD_CARD_ERROR; + ErrorState = SD_ERROR; } return ErrorState; From 8e33c9c5808304c2b64dd7c4f45f9a6a27b508ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcin=20Ga=C5=82czy=C5=84ski?= Date: Fri, 26 Mar 2021 19:54:42 +0100 Subject: [PATCH 138/143] remove unused functions, add static, check init status --- .../drivers/rangefinder/rangefinder_vl53l1x.c | 981 +++++++++--------- 1 file changed, 492 insertions(+), 489 deletions(-) diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c index adc251275b4..839d06e9dc4 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -350,165 +350,165 @@ typedef struct { /** * @brief This function returns the SW driver version */ -VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion); +// VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion); /** * @brief This function sets the sensor I2C address used in case multiple devices application, default address 0x52 */ -VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address); +// // VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address); -/** - * @brief This function loads the 135 bytes default values to initialize the sensor. - * @param dev Device address - * @return 0:success, != 0:failed - */ +// /** +// * @brief This function loads the 135 bytes default values to initialize the sensor. +// * @param dev Device address +// * @return 0:success, != 0:failed +// */ VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev); /** * @brief This function clears the interrupt, to be called after a ranging data reading * to arm the interrupt for the next data ready event. */ -VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev); +static VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev); /** * @brief This function programs the interrupt polarity\n * 1=active high (default), 0=active low */ -VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t IntPol); +// VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t IntPol); /** * @brief This function returns the current interrupt polarity\n * 1=active high (default), 0=active low */ -VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pIntPol); +static VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pIntPol); /** * @brief This function starts the ranging distance operation\n * The ranging operation is continuous. The clear interrupt has to be done after each get data to allow the interrupt to raise when the next data is ready\n * 1=active high (default), 0=active low, use SetInterruptPolarity() to change the interrupt polarity if required. */ -VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev); +static VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev); /** * @brief This function stops the ranging. */ -VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev); +static VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev); /** * @brief This function checks if the new ranging data is available by polling the dedicated register. * @param : isDataReady==0 -> not ready; isDataReady==1 -> ready */ -VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady); +static VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady); /** * @brief This function programs the timing budget in ms. * Predefined values = 15, 20, 33, 50, 100(default), 200, 500. */ -VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs); +static VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs); /** * @brief This function returns the current timing budget in ms. */ -VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudgetInMs); +static VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudgetInMs); /** * @brief This function programs the distance mode (1=short, 2=long(default)). * Short mode max distance is limited to 1.3 m but better ambient immunity.\n * Long mode can range up to 4 m in the dark with 200 ms timing budget. */ -VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DistanceMode); +static VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DistanceMode); /** * @brief This function returns the current distance mode (1=short, 2=long). */ -VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *pDistanceMode); +static VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *pDistanceMode); /** * @brief This function programs the Intermeasurement period in ms\n * Intermeasurement period must be >/= timing budget. This condition is not checked by the API, * the customer has the duty to check the condition. Default = 100 ms */ -VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, +static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasurementInMs); /** * @brief This function returns the Intermeasurement period in ms. */ -VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t * pIM); +// VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t * pIM); /** * @brief This function returns the boot state of the device (1:booted, 0:not booted) */ -VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state); +// VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state); /** * @brief This function returns the sensor id, sensor Id must be 0xEEAC */ -VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *id); +// VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *id); /** * @brief This function returns the distance measured by the sensor in mm */ -VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); +static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); /** * @brief This function returns the returned signal per SPAD in kcps/SPAD. * With kcps stands for Kilo Count Per Second */ -VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalPerSp); +// VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalPerSp); /** * @brief This function returns the ambient per SPAD in kcps/SPAD */ -VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *amb); +// VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *amb); /** * @brief This function returns the returned signal in kcps. */ -VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signalRate); +// static VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signalRate); /** * @brief This function returns the current number of enabled SPADs */ -VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb); +// static VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb); /** * @brief This function returns the ambient rate in kcps */ -VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate); +// VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate); /** * @brief This function returns the ranging status error \n * (0:no error, 1:sigma failed, 2:signal failed, ..., 7:wrap-around) */ -VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus); +// VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus); /** * @brief This function returns measurements and the range status in a single read access */ -VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult); +// VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult); /** * @brief This function programs the offset correction in mm * @param OffsetValue:the offset correction value to program in mm */ -VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue); +// VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue); /** * @brief This function returns the programmed offset correction value in mm */ -VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *Offset); +// VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *Offset); /** * @brief This function programs the xtalk correction value in cps (Count Per Second).\n * This is the number of photons reflected back from the cover glass in cps. */ -VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue); +// VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue); /** * @brief This function returns the current programmed xtalk correction value in cps */ -VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); +// VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); /** * @brief This function programs the threshold detection mode\n @@ -523,24 +523,24 @@ VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); * @param Window detection mode : 0=below, 1=above, 2=out, 3=in * @param IntOnNoTarget = 0 (No longer used - just use 0) */ -VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, - uint16_t ThreshHigh, uint8_t Window, - uint8_t IntOnNoTarget); +// VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, +// uint16_t ThreshHigh, uint8_t Window, +// uint8_t IntOnNoTarget); /** * @brief This function returns the window detection mode (0=below; 1=above; 2=out; 3=in) */ -VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window); +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window); /** * @brief This function returns the low threshold in mm */ -VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low); +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low); /** * @brief This function returns the high threshold in mm */ -VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high); +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high); /** * @brief This function programs the ROI (Region of Interest)\n @@ -548,50 +548,50 @@ VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high * The smallest acceptable ROI size = 4\n * @param X:ROI Width; Y=ROI Height */ -VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y); +// VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y); /** *@brief This function returns width X and height Y */ -VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y); +// VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y); /** *@brief This function programs the new user ROI center, please to be aware that there is no check in this function. *if the ROI center vs ROI size is out of border the ranging function return error #13 */ -VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter); +// VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter); /** *@brief This function returns the current user ROI center */ -VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter); +// VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter); /** * @brief This function programs a new signal threshold in kcps (default=1024 kcps\n */ -VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t signal); +// VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t signal); /** * @brief This function returns the current signal threshold in kcps */ -VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal); +// VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal); /** * @brief This function programs a new sigma threshold in mm (default=15 mm) */ -VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t sigma); +// VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t sigma); /** * @brief This function returns the current sigma threshold in mm */ -VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *signal); +// VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *signal); /** * @brief This function performs the temperature calibration. * It is recommended to call this function any time the temperature might have changed by more than 8 deg C * without sensor ranging activity for an extended period. */ -VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); +// VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); /** * @brief This function performs the offset calibration.\n @@ -601,7 +601,7 @@ VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); * @return 0:success, !=0: failed * @return offset pointer contains the offset found in mm */ -int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset); +// int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset); /** * @brief This function performs the xtalk calibration.\n @@ -614,7 +614,7 @@ int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16 * @return 0: success, !=0: failed * @return xtalk pointer contains the xtalk value found in cps (number of photons in count per second) */ -int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk); +// int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk); @@ -712,10 +712,10 @@ const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = { 0x00 /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */ }; -static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0, - 255, 255, 9, 13, 255, 255, 255, 255, 10, 6, - 255, 255, 11, 12 -}; +// static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0, +// 255, 255, 9, 13, 255, 255, 255, 255, 10, 6, +// 255, 255, 11, 12 +// }; static uint8_t _I2CBuffer[256]; @@ -892,24 +892,24 @@ VL53L1X_ERROR VL53L1_RdDWord(busDevice_t * Dev, uint16_t index, uint32_t *data) return Status; } -VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) -{ - VL53L1X_ERROR Status = 0; +// VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) +// { +// VL53L1X_ERROR Status = 0; - pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR; - pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR; - pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB; - pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION; - return Status; -} +// pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR; +// pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR; +// pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB; +// pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION; +// return Status; +// } -VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address) -{ - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address) +// { +// VL53L1X_ERROR status = 0; - status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1); - return status; -} +// status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1); +// return status; +// } VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev) { @@ -931,7 +931,7 @@ VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev) return status; } -VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) +static VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) { VL53L1X_ERROR status = 0; @@ -939,18 +939,18 @@ VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) return status; } -VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t NewPolarity) -{ - uint8_t Temp; - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t NewPolarity) +// { +// uint8_t Temp; +// VL53L1X_ERROR status = 0; - status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); - Temp = Temp & 0xEF; - status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4); - return status; -} +// status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); +// Temp = Temp & 0xEF; +// status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterruptPolarity) +static VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterruptPolarity) { uint8_t Temp; VL53L1X_ERROR status = 0; @@ -961,7 +961,7 @@ VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterrup return status; } -VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) +static VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) { VL53L1X_ERROR status = 0; @@ -969,7 +969,7 @@ VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) return status; } -VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) +static VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) { VL53L1X_ERROR status = 0; @@ -977,7 +977,7 @@ VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) return status; } -VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) +static VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) { uint8_t Temp; uint8_t IntPol; @@ -995,7 +995,7 @@ VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) return status; } -VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs) +static VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs) { uint16_t DM; VL53L1X_ERROR status=0; @@ -1097,7 +1097,7 @@ VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudg return status; } -VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudget) +static VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudget) { uint16_t Temp; VL53L1X_ERROR status = 0; @@ -1138,7 +1138,7 @@ VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBu return status; } -VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) +static VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) { uint16_t TB; VL53L1X_ERROR status = 0; @@ -1173,7 +1173,7 @@ VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) return status; } -VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) +static VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) { uint8_t TempDM, status=0; @@ -1185,7 +1185,7 @@ VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) return status; } -VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasMs) +static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasMs) { uint16_t ClockPLL; VL53L1X_ERROR status = 0; @@ -1198,41 +1198,41 @@ VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterM } -VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t *pIM) -{ - uint16_t ClockPLL; - VL53L1X_ERROR status = 0; - uint32_t tmp; - - status = VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp); - *pIM = (uint16_t)tmp; - status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); - ClockPLL = ClockPLL&0x3FF; - *pIM= (uint16_t)(*pIM/(ClockPLL*1.065)); - return status; -} - -VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp = 0; - - status = VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp); - *state = tmp; - return status; -} - -VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *sensorId) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp = 0; - - status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp); - *sensorId = tmp; - return status; -} - -VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) +// VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t *pIM) +// { +// uint16_t ClockPLL; +// VL53L1X_ERROR status = 0; +// uint32_t tmp; + +// status = VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp); +// *pIM = (uint16_t)tmp; +// status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); +// ClockPLL = ClockPLL&0x3FF; +// *pIM= (uint16_t)(*pIM/(ClockPLL*1.065)); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp = 0; + +// status = VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp); +// *state = tmp; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *sensorId) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp = 0; + +// status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp); +// *sensorId = tmp; +// return status; +// } + +static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) { VL53L1X_ERROR status = 0; uint16_t tmp; @@ -1243,375 +1243,378 @@ VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) return status; } -VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate) -{ - VL53L1X_ERROR status = 0; - uint16_t SpNb=1, signal; - - status = VL53L1_RdWord(dev, - VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal); - status = VL53L1_RdWord(dev, - VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); - *signalRate = (uint16_t) (200.0*signal/SpNb); - return status; -} - -VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *ambPerSp) -{ - VL53L1X_ERROR status = 0; - uint16_t AmbientRate, SpNb = 1; - - status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate); - status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); - *ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb); - return status; -} - -VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signal) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; - - status = VL53L1_RdWord(dev, - VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp); - *signal = tmp*8; - return status; -} - -VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; - - status = VL53L1_RdWord(dev, - VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp); - *spNb = tmp >> 8; - return status; -} - -VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; - - status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp); - *ambRate = tmp*8; - return status; -} - -VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus) -{ - VL53L1X_ERROR status = 0; - uint8_t RgSt; - - *rangeStatus = 255; - status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt); - RgSt = RgSt & 0x1F; - if (RgSt < 24) - *rangeStatus = status_rtn[RgSt]; - return status; -} - -VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult) -{ - VL53L1X_ERROR status = 0; - uint8_t Temp[17]; - uint8_t RgSt = 255; - - status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17); - RgSt = Temp[0] & 0x1F; - if (RgSt < 24) - RgSt = status_rtn[RgSt]; - pResult->Status = RgSt; - pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8; - pResult->NumSPADs = Temp[3]; - pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8; - pResult->Distance = Temp[13] << 8 | Temp[14]; - - return status; -} - -VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue) -{ - VL53L1X_ERROR status = 0; - int16_t Temp; - - Temp = (OffsetValue*4); - VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, - (uint16_t)Temp); - VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); - VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); - return status; -} - -VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *offset) -{ - VL53L1X_ERROR status = 0; - uint16_t Temp; - - status = VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp); - Temp = Temp<<3; - Temp = Temp>>5; - *offset = (int16_t)(Temp); - return status; -} - -VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue) -{ -/* XTalkValue in count per second to avoid float type */ - VL53L1X_ERROR status = 0; - - status = VL53L1_WrWord(dev, - ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS, - 0x0000); - status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS, - 0x0000); - status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, - (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */ - return status; -} - -VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *xtalk ) -{ - VL53L1X_ERROR status = 0; - - status = VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk); - *xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */ - return status; -} - -VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, - uint16_t ThreshHigh, uint8_t Window, - uint8_t IntOnNoTarget) -{ - VL53L1X_ERROR status = 0; - uint8_t Temp = 0; - - status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp); - Temp = Temp & 0x47; - if (IntOnNoTarget == 0) { - status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, - (Temp | (Window & 0x07))); - } else { - status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, - ((Temp | (Window & 0x07)) | 0x40)); - } - status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh); - status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow); - return status; -} - -VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp; - status = VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp); - *window = (uint16_t)(tmp & 0x7); - return status; -} - -VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; - - status = VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp); - *low = tmp; - return status; -} - -VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; - - status = VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp); - *high = tmp; - return status; -} - -VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter) -{ - VL53L1X_ERROR status = 0; - status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter); - return status; -} - -VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp; - status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp); - *ROICenter = tmp; - return status; -} - -VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y) -{ - uint8_t OpticalCenter; - VL53L1X_ERROR status = 0; - - status =VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter); - if (X > 16) - X = 16; - if (Y > 16) - Y = 16; - if (X > 10 || Y > 10){ - OpticalCenter = 199; - } - status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter); - status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, - (Y - 1) << 4 | (X - 1)); - return status; -} - -VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp; - - status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp); - *ROI_X = ((uint16_t)tmp & 0x0F) + 1; - *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1; - return status; -} - -VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t Signal) -{ - VL53L1X_ERROR status = 0; - - VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3); - return status; -} - -VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; - - status = VL53L1_RdWord(dev, - RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp); - *signal = tmp <<3; - return status; -} - -VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t Sigma) -{ - VL53L1X_ERROR status = 0; - - if(Sigma>(0xFFFF>>2)){ - return 1; - } - /* 16 bits register 14.2 format */ - status = VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2); - return status; -} - -VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *sigma) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; - - status = VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp); - *sigma = tmp >> 2; - return status; - -} - -VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp=0; - - status = VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */ - status = VL53L1_WrByte(dev,0x0B,0x92); - status = VL53L1X_StartRanging(dev); - while(tmp==0){ - status = VL53L1X_CheckForDataReady(dev, &tmp); - } - tmp = 0; - status = VL53L1X_ClearInterrupt(dev); - status = VL53L1X_StopRanging(dev); - status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ - status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ - return status; -} - - -int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset) -{ - uint8_t i, tmp; - int16_t AverageDistance = 0; - uint16_t distance; - VL53L1X_ERROR status = 0; - - status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0); - status = VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); - status = VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); - status = VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */ - for (i = 0; i < 50; i++) { - tmp = 0; - while (tmp == 0){ - status = VL53L1X_CheckForDataReady(dev, &tmp); - } - status = VL53L1X_GetDistance(dev, &distance); - status = VL53L1X_ClearInterrupt(dev); - AverageDistance = AverageDistance + distance; - } - status = VL53L1X_StopRanging(dev); - AverageDistance = AverageDistance / 50; - *offset = TargetDistInMm - AverageDistance; - status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4); - return status; -} - -int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk) -{ - uint8_t i, tmp; - float AverageSignalRate = 0; - float AverageDistance = 0; - float AverageSpadNb = 0; - uint16_t distance = 0, spadNum; - uint16_t sr; - VL53L1X_ERROR status = 0; - uint32_t calXtalk; - - status = VL53L1_WrWord(dev, 0x0016,0); - status = VL53L1X_StartRanging(dev); - for (i = 0; i < 50; i++) { - tmp = 0; - while (tmp == 0){ - status = VL53L1X_CheckForDataReady(dev, &tmp); - } - status= VL53L1X_GetSignalRate(dev, &sr); - status= VL53L1X_GetDistance(dev, &distance); - status = VL53L1X_ClearInterrupt(dev); - AverageDistance = AverageDistance + distance; - status = VL53L1X_GetSpadNb(dev, &spadNum); - AverageSpadNb = AverageSpadNb + spadNum; - AverageSignalRate = - AverageSignalRate + sr; - } - status = VL53L1X_StopRanging(dev); - AverageDistance = AverageDistance / 50; - AverageSpadNb = AverageSpadNb / 50; - AverageSignalRate = AverageSignalRate / 50; - /* Calculate Xtalk value */ - calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb); - *xtalk = (uint16_t)((calXtalk*1000)>>9); - status = VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk); - return status; -} +// VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate) +// { +// VL53L1X_ERROR status = 0; +// uint16_t SpNb=1, signal; + +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal); +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); +// *signalRate = (uint16_t) (200.0*signal/SpNb); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *ambPerSp) +// { +// VL53L1X_ERROR status = 0; +// uint16_t AmbientRate, SpNb = 1; + +// status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate); +// status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); +// *ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb); +// return status; +// } + +// static VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signal) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp); +// *signal = tmp*8; +// return status; +// } + +// static VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp); +// *spNb = tmp >> 8; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp); +// *ambRate = tmp*8; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus) +// { +// VL53L1X_ERROR status = 0; +// uint8_t RgSt; + +// *rangeStatus = 255; +// status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt); +// RgSt = RgSt & 0x1F; +// if (RgSt < 24) +// *rangeStatus = status_rtn[RgSt]; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult) +// { +// VL53L1X_ERROR status = 0; +// uint8_t Temp[17]; +// uint8_t RgSt = 255; + +// status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17); +// RgSt = Temp[0] & 0x1F; +// if (RgSt < 24) +// RgSt = status_rtn[RgSt]; +// pResult->Status = RgSt; +// pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8; +// pResult->NumSPADs = Temp[3]; +// pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8; +// pResult->Distance = Temp[13] << 8 | Temp[14]; + +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue) +// { +// VL53L1X_ERROR status = 0; +// int16_t Temp; + +// Temp = (OffsetValue*4); +// VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, +// (uint16_t)Temp); +// VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); +// VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *offset) +// { +// VL53L1X_ERROR status = 0; +// uint16_t Temp; + +// status = VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp); +// Temp = Temp<<3; +// Temp = Temp>>5; +// *offset = (int16_t)(Temp); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue) +// { +// /* XTalkValue in count per second to avoid float type */ +// VL53L1X_ERROR status = 0; + +// status = VL53L1_WrWord(dev, +// ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS, +// 0x0000); +// status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS, +// 0x0000); +// status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, +// (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */ +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *xtalk ) +// { +// VL53L1X_ERROR status = 0; + +// status = VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk); +// *xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */ +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, +// uint16_t ThreshHigh, uint8_t Window, +// uint8_t IntOnNoTarget) +// { +// VL53L1X_ERROR status = 0; +// uint8_t Temp = 0; + +// status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp); +// Temp = Temp & 0x47; +// if (IntOnNoTarget == 0) { +// status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, +// (Temp | (Window & 0x07))); +// } else { +// status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, +// ((Temp | (Window & 0x07)) | 0x40)); +// } +// status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh); +// status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; +// status = VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp); +// *window = (uint16_t)(tmp & 0x7); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp); +// *low = tmp; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp); +// *high = tmp; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter) +// { +// VL53L1X_ERROR status = 0; +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; +// status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp); +// *ROICenter = tmp; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y) +// { +// uint8_t OpticalCenter; +// VL53L1X_ERROR status = 0; + +// status =VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter); +// if (X > 16) +// X = 16; +// if (Y > 16) +// Y = 16; +// if (X > 10 || Y > 10){ +// OpticalCenter = 199; +// } +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter); +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, +// (Y - 1) << 4 | (X - 1)); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; + +// status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp); +// *ROI_X = ((uint16_t)tmp & 0x0F) + 1; +// *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t Signal) +// { +// VL53L1X_ERROR status = 0; + +// VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev, +// RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp); +// *signal = tmp <<3; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t Sigma) +// { +// VL53L1X_ERROR status = 0; + +// if(Sigma>(0xFFFF>>2)){ +// return 1; +// } +// /* 16 bits register 14.2 format */ +// status = VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *sigma) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp); +// *sigma = tmp >> 2; +// return status; + +// } + +// VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp=0; + +// status = VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */ +// status = VL53L1_WrByte(dev,0x0B,0x92); +// status = VL53L1X_StartRanging(dev); +// while(tmp==0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// tmp = 0; +// status = VL53L1X_ClearInterrupt(dev); +// status = VL53L1X_StopRanging(dev); +// status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ +// status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ +// return status; +// } + + +// int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset) +// { +// uint8_t i, tmp; +// int16_t AverageDistance = 0; +// uint16_t distance; +// VL53L1X_ERROR status = 0; + +// status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0); +// status = VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); +// status = VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); +// status = VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */ +// for (i = 0; i < 50; i++) { +// tmp = 0; +// while (tmp == 0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// status = VL53L1X_GetDistance(dev, &distance); +// status = VL53L1X_ClearInterrupt(dev); +// AverageDistance = AverageDistance + distance; +// } +// status = VL53L1X_StopRanging(dev); +// AverageDistance = AverageDistance / 50; +// *offset = TargetDistInMm - AverageDistance; +// status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4); +// return status; +// } + +// int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk) +// { +// uint8_t i, tmp; +// float AverageSignalRate = 0; +// float AverageDistance = 0; +// float AverageSpadNb = 0; +// uint16_t distance = 0, spadNum; +// uint16_t sr; +// VL53L1X_ERROR status = 0; +// uint32_t calXtalk; + +// status = VL53L1_WrWord(dev, 0x0016,0); +// status = VL53L1X_StartRanging(dev); +// for (i = 0; i < 50; i++) { +// tmp = 0; +// while (tmp == 0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// status= VL53L1X_GetSignalRate(dev, &sr); +// status= VL53L1X_GetDistance(dev, &distance); +// status = VL53L1X_ClearInterrupt(dev); +// AverageDistance = AverageDistance + distance; +// status = VL53L1X_GetSpadNb(dev, &spadNum); +// AverageSpadNb = AverageSpadNb + spadNum; +// AverageSignalRate = +// AverageSignalRate + sr; +// } +// status = VL53L1X_StopRanging(dev); +// AverageDistance = AverageDistance / 50; +// AverageSpadNb = AverageSpadNb / 50; +// AverageSignalRate = AverageSignalRate / 50; +// /* Calculate Xtalk value */ +// calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb); +// *xtalk = (uint16_t)((calXtalk*1000)>>9); +// status = VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk); +// return status; +// } static void vl53l1x_Init(rangefinderDev_t * rangefinder) { + VL53L1X_ERROR status = VL53L1_ERROR_NONE; isInitialized = false; - VL53L1X_SensorInit(rangefinder->busDev); - VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ - VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, VL53L1X_TIMING_BUDGET); /* in ms possible values [20, 50, 100, 200, 500] */ - VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */ - VL53L1X_StartRanging(rangefinder->busDev); - isInitialized = true; + status = VL53L1X_SensorInit(rangefinder->busDev); + if (status == VL53L1_ERROR_NONE) { + VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ + VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */ + VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */ + status = VL53L1X_StartRanging(rangefinder->busDev); + } + isInitialized = (status == VL53L1_ERROR_NONE); } void vl53l1x_Update(rangefinderDev_t * rangefinder) @@ -1639,7 +1642,7 @@ int32_t vl53l1x_GetDistance(rangefinderDev_t *dev) if (isResponding && isInitialized) { if (lastMeasurementIsNew) { lastMeasurementIsNew = false; - return lastMeasurementCm < VL53L1X_MAX_RANGE_CM ? lastMeasurementCm : RANGEFINDER_OUT_OF_RANGE; + return (lastMeasurementCm < VL53L1X_MAX_RANGE_CM) ? lastMeasurementCm : RANGEFINDER_OUT_OF_RANGE; } else { return RANGEFINDER_NO_NEW_DATA; From 2d07330d429a367672e0823be03003695e0c33ac Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 27 Mar 2021 10:17:09 +0000 Subject: [PATCH 139/143] Update logic_condition.h Added GPS Valid Fix to flight parameters --- src/main/programming/logic_condition.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 28cbdbb4dc0..005c7a85f4f 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -119,6 +119,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33 + LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34 } logicFlightOperands_e; @@ -195,4 +196,4 @@ void logicConditionReset(void); float getThrottleScale(float globalThrottleScale); int16_t getRcCommandOverride(int16_t command[], uint8_t axis); -int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); \ No newline at end of file +int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); From 4306010588122afff21fa895d1c731972f27e0c7 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 27 Mar 2021 10:19:04 +0000 Subject: [PATCH 140/143] Added GPS Valid Fix to programming Added GPS Valid fix flight parameter. Also have set the GPS sats to 0 when a GPS sensor loss occurs. This is much more useful than a false reading. --- src/main/programming/logic_condition.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index b1b9cefaee4..017d585b0ad 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -45,6 +45,7 @@ #include "flight/pid.h" #include "drivers/io_port_expander.h" #include "io/osd_common.h" +#include "sensors/diagnostics.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -408,7 +409,15 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: - return gpsSol.numSat; + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + return 0; + } else { + return gpsSol.numSat; + } + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1 + return STATE(GPS_FIX) ? 1 : 0; break; case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s From 2a69ee3107577a320f592eebb411b1a7040d5694 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 27 Mar 2021 10:21:14 +0000 Subject: [PATCH 141/143] Added GPS Valid Fix --- docs/Programming Framework.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 6d258ba8c97..ca5cc65f7ff 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -125,7 +125,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | | 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | | 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | -| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | +| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | +| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | #### ACTIVE_WAYPOINT_ACTION @@ -261,4 +262,4 @@ Steps: 1. Normalize range [1000:2000] to [0:1000] by substracting `1000` 2. Scale range [0:1000] to [0:3] 3. Increase range by `1` to have the range of [1:4] -4. Assign LC#2 to VTX power function \ No newline at end of file +4. Assign LC#2 to VTX power function From 91a5a51cc09836a4745397bad1a84f9eb8176b50 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 29 Mar 2021 12:08:39 +0200 Subject: [PATCH 142/143] Version bump to 3.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dc9a5385d39..b73616735b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 2.7.0) +project(INAV VERSION 3.0.0) enable_language(ASM) From b1c0f473ed674e5315874453cd81843183f23841 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Mon, 29 Mar 2021 15:29:45 +0100 Subject: [PATCH 143/143] add additional MC_MOTORS (#6758) --- src/main/target/HGLRCF722/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/HGLRCF722/target.c b/src/main/target/HGLRCF722/target.c index ebabbd28a12..6924d5ac934 100644 --- a/src/main/target/HGLRCF722/target.c +++ b/src/main/target/HGLRCF722/target.c @@ -42,8 +42,8 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5) DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2