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/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 diff --git a/docs/Settings.md b/docs/Settings.md index e0a8c522421..0662d982ec0 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 | @@ -133,6 +134,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. | @@ -390,6 +392,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_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/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 diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index a80fdd9feb4..d37646abc49 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -73,6 +73,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 2d7a51fc979..26d70e2cb88 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/fc_core.c b/src/main/fc/fc_core.c index 71f758dc408..b1fe71ca436 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -295,6 +295,16 @@ 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); + } + } else { + DISABLE_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 +516,7 @@ void tryArm(void) #else beeper(BEEPER_ARMING); #endif + statsOnArm(); return; 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/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.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) 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/fc/settings.yaml b/src/main/fc/settings.yaml index a49d69ff33d..d462d72cc8d 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", "UNUSED", "BENEWAKE"] @@ -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"] @@ -461,7 +464,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" @@ -1594,6 +1597,12 @@ groups: field: fixedWingItermLimitOnStickPosition min: 0 max: 1 + - 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: fixedWingYawItermBankFreeze + min: 0 + 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" @@ -2799,8 +2808,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%" @@ -3021,6 +3030,20 @@ groups: default_value: "ON" description: Should home position coordinates be displayed on the arming screen. + - name: osd_pan_servo_index + 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_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 + + - name: PG_SYSTEM_CONFIG type: systemConfig_t headers: ["fc/config.h"] @@ -3249,3 +3272,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/flight/pid.c b/src/main/flight/pid.c index 23fde837b74..17ba8a758bf 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -104,6 +104,7 @@ typedef struct { uint16_t pidSumLimit; filterApply4FnPtr ptermFilterApplyFn; bool itermLimitActive; + bool itermFreezeActive; biquadFilter_t rateTargetFilter; } pidState_t; @@ -260,6 +261,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, + .fixedWingYawItermBankFreeze = 0, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, @@ -611,7 +613,8 @@ 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); } } @@ -628,8 +631,12 @@ 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); @@ -861,7 +868,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, float pitchAngleTarget) { fpVector3_t targetRates; targetRates.x = 0.0f; @@ -889,8 +896,9 @@ 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; + 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; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); } @@ -951,6 +959,24 @@ void checkItermLimitingActive(pidState_t *pidState) pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; } +void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) +{ + 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())){ + pidState->itermFreezeActive = true; + } else + { + pidState->itermFreezeActive = false; + } + } else + { + pidState->itermFreezeActive = false; + } + +} + void FAST_CODE pidController(float dT) { if (!pidFiltersConfigured) { @@ -1003,8 +1029,10 @@ void FAST_CODE pidController(float dT) levelingEnabled = false; } - if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - pidTurnAssistant(pidState); + 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])); + 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 } @@ -1021,6 +1049,8 @@ void FAST_CODE pidController(float dT) // Step 4: Run gyro-driven control checkItermLimitingActive(&pidState[axis]); + checkItermFreezingActive(&pidState[axis], axis); + pidControllerApplyFn(&pidState[axis], axis, dT); } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 83b7b6380ca..d627ed4b6c7 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 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; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3612ef00d5a..ef76c0cbe7f 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) @@ -712,6 +713,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; @@ -785,7 +788,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); @@ -945,6 +948,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_pwm2centideg); +} + // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle) { @@ -1118,13 +1129,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) { @@ -1334,7 +1338,11 @@ static bool osdDrawSingleElement(uint8_t item) } else { - int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); + int16_t panHomeDirOffset = 0; + if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + panHomeDirOffset = osdPanServoHomeDirectionOffset(); + } + int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset; osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection); } } else { @@ -1685,7 +1693,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) { @@ -1762,9 +1770,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) { @@ -2593,6 +2598,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_pwm2centideg = 0, .units = OSD_UNIT_METRIC, .main_voltage_decimals = 1, @@ -3381,7 +3388,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 @@ -3391,8 +3398,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..9fc2cfb5399 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!" @@ -292,7 +293,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 +338,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 @@ -348,7 +349,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 + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; } osdConfig_t; diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index c7975652650..d785eac9798 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 @@ -152,3 +154,12 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas) } #endif + +#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 \ No newline at end of file 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..ef6ad6fcd1f 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 @@ -500,6 +503,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; @@ -639,13 +644,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 +764,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 +1002,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/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 - 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; } diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index d4465c4cec6..9cda8418558 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" @@ -317,6 +318,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; @@ -402,7 +409,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 @@ -418,7 +425,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: // % 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 { 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 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/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 ); 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; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 3428d118a40..0f72aa9f7b4 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" @@ -207,6 +208,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 737055af1f0..95174bb055a 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/RUSH_BLADE_F7/CMakeLists.txt b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt new file mode 100644 index 00000000000..5d612d6c68b --- /dev/null +++ b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(RUSH_BLADE_F7 SKIP_RELEASES) \ 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 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