Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/master' into dzikuvx-adaptive-filter

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2024-04-05 14:40:01 +02:00
commit 724d4ded5e
20 changed files with 239 additions and 53 deletions
Download patch file Download diff file Expand all files Collapse all files

2
cmake/arm-none-eabi-checks.cmake
View file

@ -47,7 +47,7 @@ function(arm_none_eabi_gcc_install)
host_uname_machine(machine)
if(machine STREQUAL "x86_64" OR machine STREQUAL "amd64")
set(dist ${arm_none_eabi_darwin_amd64})
elseif(machine STREQUAL "aarch64")
elseif(machine STREQUAL "aarch64" OR machine STREQUAL "arm64")
set(dist ${arm_none_eabi_darwin_aarch64})
else()
message("-- no precompiled ${arm_none_eabi_triplet} toolchain for machine ${machine}")

1
docs/Rangefinder.md
View file

@ -20,6 +20,7 @@ Following rangefinders are supported:
* UIB - experimental
* MSP - experimental
* TOF10120 - small & lightweight laser range sensor, usable up to 200cm
* TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
## Connections

10
docs/Settings.md
View file

@ -4372,6 +4372,16 @@ Use wind estimation for remaining flight time/distance estimation
---
### osd_estimations_wind_mps
Wind speed estimation in m/s
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### osd_failsafe_switch_layout
If enabled the OSD automatically switches to the first layout during failsafe

2
src/main/CMakeLists.txt
View file

@ -237,6 +237,8 @@ main_sources(COMMON_SRC
drivers/rangefinder/rangefinder_us42.h
drivers/rangefinder/rangefinder_tof10120_i2c.c
drivers/rangefinder/rangefinder_tof10120_i2c.h
drivers/rangefinder/rangefinder_teraranger_evo.c
drivers/rangefinder/rangefinder_teraranger_evo.h
drivers/resource.c
drivers/resource.h

2
src/main/common/maths.h
View file

@ -56,6 +56,8 @@
#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD)
#define MILLIMETERS_TO_CENTIMETERS(mm) (mm / 10.0f)
#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f)
#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f)
#define CENTIMETERS_TO_METERS(cm) (cm * 0.01f)

1
src/main/drivers/bus.h
View file

@ -135,6 +135,7 @@ typedef enum {
DEVHW_VL53L1X,
DEVHW_US42,
DEVHW_TOF10120_I2C,
DEVHW_TERARANGER_EVO_I2C,
/* Other hardware */
DEVHW_MS4525, // Pitot meter

128
src/main/drivers/rangefinder/rangefinder_teraranger_evo.c Normal file
View file

@ -0,0 +1,128 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/maths.h"
#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
#include "build/build_config.h"
#include "common/crc.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_teraranger_evo.h"
#include "build/debug.h"
#define TERARANGER_EVO_DETECTION_CONE_DECIDEGREES 900
#define TERARANGER_EVO_DETECTION_CONE_EXTENDED_DECIDEGREES 900
#define TERARANGER_EVO_I2C_ADDRESS 0x31
#define TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING 0x00 // Write this command to the TeraRanger Evo and after a wait of approximately 500us read the 3 byte distance response
#define TERARANGER_EVO_I2C_REGISTRY_WHO_AM_I 0x01 // Write this value to TeraRanger Evo via I2C and the device responds with 0xA
#define TERARANGER_EVO_I2C_REGISTRY_CHANGE_BASE_ADDR 0xA2 // This command assigns a base address that will be memorised by the TerRanger Evo ie. power cycling the Evo will not restore the default I2C address.
#define TERARANGER_EVO_I2C_ANSWER_WHO_AM_I 0xA1
#define TERARANGER_EVO_VALUE_TOO_CLOSE 0x0000
#define TERARANGER_EVO_VALUE_OUT_OF_RANGE 0xffff
static int16_t minimumReadingIntervalMs = 50;
uint8_t triggerValue[0];
volatile int32_t teraRangerMeasurementCm;
static uint32_t timeOfLastMeasurementMs;
static uint8_t dataBuff[3];
static void teraRangerInit(rangefinderDev_t *rangefinder){
busWriteBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, triggerValue, 1);
}
void teraRangerUpdate(rangefinderDev_t *rangefinder){
if (busReadBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, dataBuff, 3)) {
teraRangerMeasurementCm = MILLIMETERS_TO_CENTIMETERS(((int32_t)dataBuff[0] << 8 | (int32_t)dataBuff[1]));
if(dataBuff[2] == crc8_update(0, &dataBuff, 2)){
if (teraRangerMeasurementCm == TERARANGER_EVO_VALUE_TOO_CLOSE || teraRangerMeasurementCm == TERARANGER_EVO_VALUE_OUT_OF_RANGE) {
teraRangerMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
}
}
} else {
teraRangerMeasurementCm = RANGEFINDER_HARDWARE_FAILURE;
}
const timeMs_t timeNowMs = millis();
if (timeNowMs > timeOfLastMeasurementMs + minimumReadingIntervalMs) {
// measurement repeat interval should be greater than minimumReadingIntervalMs
// to avoid interference between connective measurements.
timeOfLastMeasurementMs = timeNowMs;
busWriteBuf(rangefinder->busDev, TERARANGER_EVO_I2C_ADDRESS, triggerValue, 1);
}
}
int32_t teraRangerGetDistance(rangefinderDev_t *rangefinder){
UNUSED(rangefinder);
return teraRangerMeasurementCm;
}
static bool deviceDetect(busDevice_t * busDev){
for (int retry = 0; retry < 5; retry++) {
uint8_t whoIamResult;
delay(150);
bool ack = busRead(busDev, TERARANGER_EVO_I2C_REGISTRY_WHO_AM_I, &whoIamResult);
if (ack && whoIamResult == TERARANGER_EVO_I2C_ANSWER_WHO_AM_I) {
return true;
}
};
return false;
}
bool teraRangerDetect(rangefinderDev_t *rangefinder){
rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_TERARANGER_EVO_I2C, 0, OWNER_RANGEFINDER);
if (rangefinder->busDev == NULL) {
return false;
}
if (!deviceDetect(rangefinder->busDev)) {
busDeviceDeInit(rangefinder->busDev);
return false;
}
rangefinder->delayMs = RANGEFINDER_TERA_EVO_TASK_PERIOD_MS;
rangefinder->maxRangeCm = RANGEFINDER_TERA_EVO_MAX_RANGE_CM;
rangefinder->detectionConeDeciDegrees = TERARANGER_EVO_DETECTION_CONE_DECIDEGREES;
rangefinder->detectionConeExtendedDeciDegrees = TERARANGER_EVO_DETECTION_CONE_EXTENDED_DECIDEGREES;
rangefinder->init = &teraRangerInit;
rangefinder->update = &teraRangerUpdate;
rangefinder->read = &teraRangerGetDistance;
return true;
}
#endif

29
src/main/drivers/rangefinder/rangefinder_teraranger_evo.h Normal file
View file

@ -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_TERA_EVO_TASK_PERIOD_MS 70
#define RANGEFINDER_TERA_EVO_MAX_RANGE_CM 600 // max distance depends on model https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
bool teraRangerDetect(rangefinderDev_t *dev);

3
src/main/fc/fc_msp_box.c
View file

@ -219,9 +219,6 @@ void initActiveBoxIds(void)
const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);
if (STATE(MULTIROTOR)) {
navReadyPosControl = navReadyPosControl && getHwCompassStatus() != HW_SENSOR_NONE;
}
if (STATE(ALTITUDE_CONTROL) && navReadyAltControl && (navReadyPosControl || navFlowDeadReckoning)) {
ADD_ACTIVE_BOX(BOXNAVPOSHOLD);

10
src/main/fc/settings.yaml
View file

@ -7,7 +7,7 @@ tables:
values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"]
enum: accelerationSensor_e
- name: rangefinder_hardware
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE"]
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"]
enum: rangefinderType_e
- name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
@ -179,7 +179,7 @@ tables:
values: ["OFF", "ON", "FS"]
enum: rthTrackbackMode_e
- name: dynamic_gyro_notch_mode
values: ["2D", "3D_R", "3D_P", "3D_Y", "3D_RP", "3D_RY", "3D_PY", "3D"]
values: ["2D", "3D"]
enum: dynamicGyroNotchMode_e
- name: nav_fw_wp_turn_smoothing
values: ["OFF", "ON", "ON-CUT"]
@ -3501,6 +3501,12 @@ groups:
condition: USE_WIND_ESTIMATOR
field: estimations_wind_compensation
type: bool
- name: osd_estimations_wind_mps
description: "Wind speed estimation in m/s"
default_value: OFF
condition: USE_WIND_ESTIMATOR
field: estimations_wind_mps
type: bool
- name: osd_failsafe_switch_layout
description: "If enabled the OSD automatically switches to the first layout during failsafe"

23
src/main/flight/secondary_dynamic_gyro_notch.c
View file

@ -44,46 +44,25 @@ void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *stat
state->enabled = gyroConfig()->dynamicGyroNotchMode != DYNAMIC_NOTCH_MODE_2D;
state->looptime = getLooptime();
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_R ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
if (gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D) {
/*
* Enable ROLL filter
*/
biquadFilterInit(&state->filters[FD_ROLL], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_ROLL] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_P ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
/*
* Enable PITCH filter
*/
biquadFilterInit(&state->filters[FD_PITCH], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_PITCH] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_Y ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
/*
* Enable YAW filter
*/
biquadFilterInit(&state->filters[FD_YAW], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_YAW] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
}
void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) {

14
src/main/io/osd.c
View file

@ -223,7 +223,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, 9);
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 10);
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
void osdStartedSaveProcess(void) {
@ -499,8 +499,16 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
break;
default:
case OSD_UNIT_METRIC:
if (osdConfig()->estimations_wind_mps)
{
centivalue = ws;
suffix = SYM_MS;
}
else
{
centivalue = CMSEC_TO_CENTIKPH(ws);
suffix = SYM_KMH;
}
break;
}
@ -839,7 +847,7 @@ void osdFormatPilotName(char *buff)
static const char * osdArmingDisabledReasonMessage(void)
{
const char *message = NULL;
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
static char messageBuf[OSD_MESSAGE_LENGTH+1];
switch (isArmingDisabledReason()) {
case ARMING_DISABLED_FAILSAFE_SYSTEM:
@ -5665,7 +5673,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (buff != NULL) {
const char *message = NULL;
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; //warning: shared buffer. Make sure it is used by single message in code below!
// We might have up to 5 messages to show.
const char *messages[5];
unsigned messageCount = 0;

1
src/main/io/osd.h
View file

@ -418,6 +418,7 @@ typedef struct osdConfig_s {
#ifdef USE_WIND_ESTIMATOR
bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
bool estimations_wind_mps; // wind speed estimation in m/s
#endif
uint8_t coordinate_digits;
bool osd_failsafe_switch_layout;

2
src/main/sensors/gyro.c
View file

@ -96,7 +96,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 6);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 7);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,

6
src/main/sensors/gyro.h
View file

@ -49,12 +49,6 @@ typedef enum {
typedef enum {
DYNAMIC_NOTCH_MODE_2D = 0,
DYNAMIC_NOTCH_MODE_R,
DYNAMIC_NOTCH_MODE_P,
DYNAMIC_NOTCH_MODE_Y,
DYNAMIC_NOTCH_MODE_RP,
DYNAMIC_NOTCH_MODE_RY,
DYNAMIC_NOTCH_MODE_PY,
DYNAMIC_NOTCH_MODE_3D
} dynamicGyroNotchMode_e;

9
src/main/sensors/rangefinder.c
View file

@ -41,6 +41,7 @@
#include "drivers/rangefinder/rangefinder_vl53l1x.h"
#include "drivers/rangefinder/rangefinder_virtual.h"
#include "drivers/rangefinder/rangefinder_us42.h"
#include "drivers/rangefinder/rangefinder_teraranger_evo.h"
#include "drivers/rangefinder/rangefinder_tof10120_i2c.h"
#include "fc/config.h"
@ -88,6 +89,14 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
#endif
break;
case RANGEFINDER_TERARANGER_EVO:
#if defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
if (teraRangerDetect(dev)) {
rangefinderHardware = RANGEFINDER_TERARANGER_EVO;
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TERA_EVO_TASK_PERIOD_MS));
}
#endif
break;
case RANGEFINDER_VL53L0X:
#if defined(USE_RANGEFINDER_VL53L0X)
if (vl53l0xDetect(dev)) {

1
src/main/sensors/rangefinder.h
View file

@ -31,6 +31,7 @@ typedef enum {
RANGEFINDER_US42 = 6,
RANGEFINDER_TOF10102I2C = 7,
RANGEFINDER_FAKE = 8,
RANGEFINDER_TERARANGER_EVO = 9,
} rangefinderType_e;
typedef struct rangefinderConfig_s {

3
src/main/target/AOCODARCF405AIO/target.h
View file

@ -142,7 +142,8 @@
#define SERIALRX_UART SERIAL_PORT_USART2
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_CRSF
#define CURRENT_METER_SCALE 500
#define CURRENT_METER_SCALE 589
#define CURRENT_METER_OFFSET -204
/*** Optical Flow & Lidar ***/
#define USE_RANGEFINDER

10
src/main/target/common.h
View file

@ -83,6 +83,7 @@
#define USE_RANGEFINDER_VL53L1X
#define USE_RANGEFINDER_US42
#define USE_RANGEFINDER_TOF10120_I2C
#define USE_RANGEFINDER_TERARANGER_EVO_I2C
// Allow default optic flow boards
#define USE_OPFLOW
@ -111,7 +112,6 @@
#define USE_FRSKYOSD
#define USE_DJI_HD_OSD
#define USE_MSP_OSD
#define USE_SMARTPORT_MASTER
#define NAV_NON_VOLATILE_WAYPOINT_CLI
@ -199,9 +199,17 @@
#define USE_HOTT_TEXTMODE
#define USE_24CHANNELS
#define MAX_MIXER_PROFILE_COUNT 2
#define USE_SMARTPORT_MASTER
#elif !defined(STM32F7)
#define MAX_MIXER_PROFILE_COUNT 1
#endif
#if (MCU_FLASH_SIZE <= 512)
#define SKIP_CLI_COMMAND_HELP
#undef USE_SERIALRX_SPEKTRUM
#undef USE_TELEMETRY_SRXL
#endif
#define USE_EZ_TUNE
#ifdef STM32H7

9
src/main/target/common_hardware.c
View file

@ -341,6 +341,15 @@
#endif
#endif
#if defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
#if !defined(TERARANGER_EVO_I2C_BUS) && defined(RANGEFINDER_I2C_BUS)
#define TERARANGER_EVO_I2C_BUS RANGEFINDER_I2C_BUS
#endif
#if defined(TERARANGER_EVO_I2C_BUS)
BUSDEV_REGISTER_I2C(busdev_teraranger_evo, DEVHW_TERARANGER_EVO_I2C, TERARANGER_EVO_I2C_BUS, 0x31, NONE, DEVFLAGS_NONE, 0);
#endif
#endif
#if defined(USE_RANGEFINDER_TOF10120_I2C) && (defined(TOF10120_I2C_BUS) || defined(RANGEFINDER_I2C_BUS))
#if !defined(TOF10120_I2C_BUS)
#define TOF10120_I2C_BUS RANGEFINDER_I2C_BUS