mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 06:45:14 +03:00
Merge pull request #3350 from iNavFlight/yg_de_posest_and_opflow
Refactor position estimation code. Add support for optic flow for position estimation (not recommended to be used at the moment, barely tested)
This commit is contained in:
Konstantin Sharlaimov
GitHub
commit
ee73372cf6
19 changed files with 842 additions and 499 deletions
|
|
@ -164,6 +164,8 @@ COMMON_SRC = \
|
|||
navigation/navigation_geo.c \
|
||||
navigation/navigation_multicopter.c \
|
||||
navigation/navigation_pos_estimator.c \
|
||||
navigation/navigation_pos_estimator_agl.c \
|
||||
navigation/navigation_pos_estimator_flow.c \
|
||||
sensors/barometer.c \
|
||||
sensors/pitotmeter.c \
|
||||
sensors/rangefinder.c \
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@ typedef enum {
|
|||
DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
|
||||
DEBUG_AGL,
|
||||
DEBUG_FLOW_RAW,
|
||||
DEBUG_FLOW,
|
||||
DEBUG_SBUS,
|
||||
DEBUG_FPORT,
|
||||
DEBUG_ALWAYS,
|
||||
|
|
|
|||
|
|
@ -41,6 +41,13 @@ typedef struct {
|
|||
void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles);
|
||||
void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a);
|
||||
|
||||
static inline void vectorZero(fpVector3_t * v)
|
||||
{
|
||||
v->x = 0.0f;
|
||||
v->y = 0.0f;
|
||||
v->z = 0.0f;
|
||||
}
|
||||
|
||||
static inline fpVector3_t * rotationMatrixRotateVector(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat)
|
||||
{
|
||||
fpVector3_t r;
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ struct opflowDev_s;
|
|||
|
||||
typedef struct opflowData_s {
|
||||
timeDelta_t deltaTime; // Integration timeframe of motionX/Y
|
||||
int32_t flowRateRaw[2]; // Flow rotation in raw sensor uints (per deltaTime interval)
|
||||
int32_t flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions
|
||||
int16_t quality;
|
||||
} opflowData_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@ void fakeOpflowSet(timeDelta_t deltaTime, int32_t flowRateX, int32_t flowRateY,
|
|||
fakeData.deltaTime = deltaTime;
|
||||
fakeData.flowRateRaw[0] = flowRateX;
|
||||
fakeData.flowRateRaw[1] = flowRateY;
|
||||
fakeData.flowRateRaw[2] = 0;
|
||||
fakeData.quality = quality;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -182,13 +182,22 @@ void initActiveBoxIds(void)
|
|||
activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
|
||||
}
|
||||
|
||||
if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
|
||||
const bool navReadyQuads = !STATE(FIXED_WING) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
|
||||
const bool navReadyPlanes = STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
|
||||
const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
|
||||
if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
|
||||
}
|
||||
|
||||
if (navReadyQuads || navReadyPlanes) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
|
||||
|
||||
if (feature(FEATURE_GPS)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
|
|
@ -323,7 +332,7 @@ uint16_t packSensorStatus(void)
|
|||
IS_ENABLED(sensors(SENSOR_MAG)) << 2 |
|
||||
IS_ENABLED(sensors(SENSOR_GPS)) << 3 |
|
||||
IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 |
|
||||
//IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 |
|
||||
IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 |
|
||||
IS_ENABLED(sensors(SENSOR_PITOT)) << 6;
|
||||
|
||||
// Hardware failure indication bit
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ tables:
|
|||
- name: i2c_speed
|
||||
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
|
||||
- name: debug_modes
|
||||
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
|
||||
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
|
||||
- name: async_mode
|
||||
values: ["NONE", "GYRO", "ALL"]
|
||||
- name: aux_operator
|
||||
|
|
@ -231,6 +231,9 @@ groups:
|
|||
members:
|
||||
- name: opflow_hardware
|
||||
table: opflow_hardware
|
||||
- name: opflow_align
|
||||
type: uint8_t
|
||||
table: alignment
|
||||
- name: opflow_scale
|
||||
min: 0
|
||||
max: 10000
|
||||
|
|
@ -1097,6 +1100,9 @@ groups:
|
|||
- name: inav_use_gps_velned
|
||||
field: use_gps_velned
|
||||
type: bool
|
||||
- name: inav_allow_dead_reckoning
|
||||
field: allow_dead_reckoning
|
||||
type: bool
|
||||
- name: inav_reset_altitude
|
||||
field: reset_altitude_type
|
||||
table: reset_type
|
||||
|
|
@ -1115,6 +1121,14 @@ groups:
|
|||
field: w_z_surface_v
|
||||
min: 0
|
||||
max: 10
|
||||
- name: inav_w_xy_flow_p
|
||||
field: w_xy_flow_p
|
||||
min: 0
|
||||
max: 10
|
||||
- name: inav_w_xy_flow_v
|
||||
field: w_xy_flow_v
|
||||
min: 0
|
||||
max: 10
|
||||
- name: inav_w_z_baro_p
|
||||
field: w_z_baro_p
|
||||
min: 0
|
||||
|
|
|
|||
|
|
@ -113,6 +113,7 @@ static bool cxofOpflowUpdate(opflowData_t * data)
|
|||
tmpData.deltaTime += (currentTimeUs - previousTimeUs);
|
||||
tmpData.flowRateRaw[0] += pkt->motionX;
|
||||
tmpData.flowRateRaw[1] += pkt->motionY;
|
||||
tmpData.flowRateRaw[2] = 0;
|
||||
tmpData.quality = (constrain(pkt->squal, 64, 78) - 64) * 100 / 14;
|
||||
|
||||
previousTimeUs = currentTimeUs;
|
||||
|
|
|
|||
|
|
@ -81,6 +81,7 @@ void mspOpflowReceiveNewData(uint8_t * bufferPtr)
|
|||
sensorData.deltaTime = currentTimeUs - updatedTimeUs;
|
||||
sensorData.flowRateRaw[0] = pkt->motionX;
|
||||
sensorData.flowRateRaw[1] = pkt->motionY;
|
||||
sensorData.flowRateRaw[2] = 0;
|
||||
sensorData.quality = (int)pkt->quality * 100 / 255;
|
||||
hasNewData = true;
|
||||
|
||||
|
|
|
|||
|
|
@ -173,7 +173,6 @@ static void setupAltitudeController(void);
|
|||
static void resetHeadingController(void);
|
||||
void resetGCSFlags(void);
|
||||
|
||||
static bool posEstimationHasGlobalReference(void);
|
||||
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
|
||||
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
|
||||
void calculateInitialHoldPosition(fpVector3_t * pos);
|
||||
|
|
@ -769,7 +768,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
{
|
||||
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME) || !posEstimationHasGlobalReference()) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) {
|
||||
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
|
||||
// If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
|
|
@ -1467,7 +1466,7 @@ bool checkForPositionSensorTimeout(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to XY-position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
|
||||
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
|
||||
{
|
||||
posControl.actualState.abs.pos.x = newX;
|
||||
posControl.actualState.abs.pos.y = newY;
|
||||
|
|
@ -1481,13 +1480,24 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
|
|||
|
||||
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
|
||||
|
||||
if (estimateValid) {
|
||||
// CASE 1: POS & VEL valid
|
||||
if (estPosValid && estVelValid) {
|
||||
posControl.flags.estPosStatus = EST_TRUSTED;
|
||||
posControl.flags.estVelStatus = EST_TRUSTED;
|
||||
posControl.flags.horizontalPositionDataNew = 1;
|
||||
posControl.lastValidPositionTimeMs = millis();
|
||||
}
|
||||
// CASE 1: POS invalid, VEL valid
|
||||
else if (!estPosValid && estVelValid) {
|
||||
posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted
|
||||
posControl.flags.estVelStatus = EST_TRUSTED;
|
||||
posControl.flags.horizontalPositionDataNew = 1;
|
||||
posControl.lastValidPositionTimeMs = millis();
|
||||
}
|
||||
// CASE 3: can't use pos/vel data
|
||||
else {
|
||||
posControl.flags.estPosStatus = EST_NONE;
|
||||
posControl.flags.estVelStatus = EST_NONE;
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
|
|
@ -2116,7 +2126,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
// WP #255 - special waypoint - directly set desiredPosition
|
||||
// Only valid when armed and in poshold mode
|
||||
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
|
||||
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
(posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
|
||||
// Convert to local coordinates
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
|
|
@ -2395,12 +2405,12 @@ static bool canActivateAltHoldMode(void)
|
|||
|
||||
static bool canActivatePosHoldMode(void)
|
||||
{
|
||||
return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static bool posEstimationHasGlobalReference(void)
|
||||
static bool canActivateNavigationModes(void)
|
||||
{
|
||||
return true; // For now assume that we always have global coordinates available
|
||||
return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
||||
|
|
@ -2418,6 +2428,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
// Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
|
||||
bool canActivateAltHold = canActivateAltHoldMode();
|
||||
bool canActivatePosHold = canActivatePosHoldMode();
|
||||
bool canActivateNavigation = canActivateNavigationModes();
|
||||
|
||||
// LAUNCH mode has priority over any other NAV mode
|
||||
if (STATE(FIXED_WING)) {
|
||||
|
|
@ -2444,7 +2455,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
}
|
||||
|
||||
// RTH/Failsafe_RTH can override MANUAL
|
||||
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) {
|
||||
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
|
||||
// If we request forced RTH - attempt to activate it no matter what
|
||||
// This might switch to emergency landing controller if GPS is unavailable
|
||||
canActivateWaypoint = false; // Block WP mode if we switched to RTH for whatever reason
|
||||
|
|
@ -2458,7 +2469,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
|
||||
if ((FLIGHT_MODE(NAV_WP_MODE)) || (posEstimationHasGlobalReference() && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
|
||||
if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateNavigation && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
|
||||
}
|
||||
else {
|
||||
|
|
@ -2697,6 +2708,7 @@ void navigationInit(void)
|
|||
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estPosStatus = EST_NONE;
|
||||
posControl.flags.estVelStatus = EST_NONE;
|
||||
posControl.flags.estHeadingStatus = EST_NONE;
|
||||
posControl.flags.estAglStatus = EST_NONE;
|
||||
|
||||
|
|
|
|||
|
|
@ -80,6 +80,7 @@ typedef struct positionEstimationConfig_s {
|
|||
uint8_t reset_home_type; // nav_reset_type_e
|
||||
uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s)
|
||||
uint8_t use_gps_velned;
|
||||
uint8_t allow_dead_reckoning;
|
||||
|
||||
uint16_t max_surface_altitude;
|
||||
|
||||
|
|
@ -94,6 +95,9 @@ typedef struct positionEstimationConfig_s {
|
|||
float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements
|
||||
float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements
|
||||
|
||||
float w_xy_flow_p;
|
||||
float w_xy_flow_v;
|
||||
|
||||
float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight
|
||||
float w_xy_res_v;
|
||||
|
||||
|
|
@ -280,9 +284,20 @@ bool navIsCalibrationComplete(void);
|
|||
bool navigationTerrainFollowingEnabled(void);
|
||||
|
||||
/* Access to estimated position and velocity */
|
||||
typedef struct {
|
||||
uint8_t altStatus;
|
||||
uint8_t posStatus;
|
||||
uint8_t velStatus;
|
||||
uint8_t aglStatus;
|
||||
fpVector3_t pos;
|
||||
fpVector3_t vel;
|
||||
float agl;
|
||||
} navPositionAndVelocity_t;
|
||||
|
||||
float getEstimatedActualVelocity(int axis);
|
||||
float getEstimatedActualPosition(int axis);
|
||||
int32_t getTotalTravelDistance(void);
|
||||
void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos);
|
||||
|
||||
/* Waypoint list access functions */
|
||||
int getWaypointCount(void);
|
||||
|
|
|
|||
|
|
@ -36,146 +36,22 @@
|
|||
#include "drivers/time.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
#include "navigation/navigation_private.h"
|
||||
#include "navigation/navigation_pos_estimator_private.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/pitotmeter.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/opflow.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
|
||||
/**
|
||||
* Model-identification based position estimator
|
||||
* Based on INAV position estimator for PX4 by Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
|
||||
*/
|
||||
#define INAV_GPS_DEFAULT_EPH 200.0f // 2m GPS HDOP (gives about 1.6s of dead-reckoning if GPS is temporary lost)
|
||||
#define INAV_GPS_DEFAULT_EPV 500.0f // 5m GPS VDOP
|
||||
|
||||
#define INAV_GPS_ACCEPTANCE_EPE 500.0f // 5m acceptance radius
|
||||
|
||||
#define INAV_ACC_BIAS_ACCEPTANCE_VALUE (GRAVITY_CMSS * 0.25f) // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway
|
||||
|
||||
#define INAV_GPS_GLITCH_RADIUS 250.0f // 2.5m GPS glitch radius
|
||||
#define INAV_GPS_GLITCH_ACCEL 1000.0f // 10m/s/s max possible acceleration for GPS glitch detection
|
||||
|
||||
#define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate
|
||||
#define INAV_PITOT_UPDATE_RATE 10
|
||||
|
||||
#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout
|
||||
#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout
|
||||
#define INAV_SURFACE_TIMEOUT_MS 300 // Surface timeout (missed 3 readings in a row)
|
||||
|
||||
#define INAV_HISTORY_BUF_SIZE (INAV_POSITION_PUBLISH_RATE_HZ / 2) // Enough to hold 0.5 sec historical data
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastTriggeredTime;
|
||||
timeUs_t deltaTime;
|
||||
} navigationTimer_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
bool glitchDetected;
|
||||
bool glitchRecovery;
|
||||
#endif
|
||||
fpVector3_t pos; // GPS position in NEU coordinate system (cm)
|
||||
fpVector3_t vel; // GPS velocity (cms)
|
||||
float eph;
|
||||
float epv;
|
||||
} navPositionEstimatorGPS_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
float alt; // Raw barometric altitude (cm)
|
||||
float epv;
|
||||
} navPositionEstimatorBARO_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
float airspeed; // airspeed (cm/s)
|
||||
} navPositionEstimatorPITOT_t;
|
||||
|
||||
typedef enum {
|
||||
SURFACE_QUAL_LOW, // Surface sensor signal lost long ago - most likely surface distance is incorrect
|
||||
SURFACE_QUAL_MID, // Surface sensor is not available but we can somewhat trust the estimate
|
||||
SURFACE_QUAL_HIGH // All good
|
||||
} navAGLEstimateQuality_e;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
float alt; // Raw altitude measurement (cm)
|
||||
float reliability;
|
||||
} navPositionEstimatorSURFACE_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
bool isValid;
|
||||
float quality;
|
||||
float flowRate[2];
|
||||
float bodyRate[2];
|
||||
} navPositionEstimatorFLOW_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
|
||||
// 3D position, velocity and confidence
|
||||
fpVector3_t pos;
|
||||
fpVector3_t vel;
|
||||
float eph;
|
||||
float epv;
|
||||
|
||||
// AGL
|
||||
navAGLEstimateQuality_e aglQual;
|
||||
float aglOffset; // Offset between surface and pos.Z
|
||||
float aglAlt;
|
||||
float aglVel;
|
||||
} navPositionEstimatorESTIMATE_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t baroGroundTimeout;
|
||||
float baroGroundAlt;
|
||||
bool isBaroGroundValid;
|
||||
} navPositionEstimatorSTATE_t;
|
||||
|
||||
typedef struct {
|
||||
fpVector3_t accelNEU;
|
||||
fpVector3_t accelBias;
|
||||
bool gravityCalibrationComplete;
|
||||
} navPosisitonEstimatorIMU_t;
|
||||
|
||||
typedef struct {
|
||||
// Data sources
|
||||
navPositionEstimatorGPS_t gps;
|
||||
navPositionEstimatorBARO_t baro;
|
||||
navPositionEstimatorSURFACE_t surface;
|
||||
navPositionEstimatorPITOT_t pitot;
|
||||
navPositionEstimatorFLOW_t flow;
|
||||
|
||||
// IMU data
|
||||
navPosisitonEstimatorIMU_t imu;
|
||||
|
||||
// Estimate
|
||||
navPositionEstimatorESTIMATE_t est;
|
||||
|
||||
// Extra state variables
|
||||
navPositionEstimatorSTATE_t state;
|
||||
} navigationPosEstimator_t;
|
||||
|
||||
static navigationPosEstimator_t posEstimator;
|
||||
navigationPosEstimator_t posEstimator;
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 3);
|
||||
|
||||
|
|
@ -186,6 +62,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
|
|||
.reset_home_type = NAV_RESET_ON_EACH_ARM,
|
||||
.gravity_calibration_tolerance = 5, // 5 cm/s/s calibration error accepted (0.5% of gravity)
|
||||
.use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian
|
||||
.allow_dead_reckoning = 0,
|
||||
|
||||
.max_surface_altitude = 200,
|
||||
|
||||
|
|
@ -195,11 +72,14 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
|
|||
.w_z_surface_v = 6.100f,
|
||||
|
||||
.w_z_gps_p = 0.2f,
|
||||
.w_z_gps_v = 0.5f,
|
||||
.w_z_gps_v = 0.1f,
|
||||
|
||||
.w_xy_gps_p = 1.0f,
|
||||
.w_xy_gps_v = 2.0f,
|
||||
|
||||
.w_xy_flow_p = 1.0f,
|
||||
.w_xy_flow_v = 2.0f,
|
||||
|
||||
.w_z_res_v = 0.5f,
|
||||
.w_xy_res_v = 0.5f,
|
||||
|
||||
|
|
@ -209,25 +89,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
|
|||
.baro_epv = 100.0f
|
||||
);
|
||||
|
||||
/* Inertial filter, implementation taken from PX4 implementation by Anton Babushkin <rk3dov@gmail.com> */
|
||||
static void inavFilterPredict(int axis, float dt, float acc)
|
||||
{
|
||||
posEstimator.est.pos.v[axis] += posEstimator.est.vel.v[axis] * dt + acc * dt * dt / 2.0f;
|
||||
posEstimator.est.vel.v[axis] += acc * dt;
|
||||
}
|
||||
|
||||
static void inavFilterCorrectPos(int axis, float dt, float e, float w)
|
||||
{
|
||||
float ewdt = e * w * dt;
|
||||
posEstimator.est.pos.v[axis] += ewdt;
|
||||
posEstimator.est.vel.v[axis] += w * ewdt;
|
||||
}
|
||||
|
||||
static void inavFilterCorrectVel(int axis, float dt, float e, float w)
|
||||
{
|
||||
posEstimator.est.vel.v[axis] += e * w * dt;
|
||||
}
|
||||
|
||||
#define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
|
||||
#define getTimerDeltaMicros(tim) ((tim)->deltaTime)
|
||||
static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t currentTimeUs)
|
||||
|
|
@ -480,64 +341,6 @@ static void updatePitotTopic(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
/**
|
||||
* Read surface and update alt/vel topic
|
||||
* Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
|
||||
*/
|
||||
#define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f)
|
||||
#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
|
||||
#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f)
|
||||
#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD (0.75f)
|
||||
|
||||
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
|
||||
{
|
||||
const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime);
|
||||
float newReliabilityMeasurement = 0;
|
||||
|
||||
posEstimator.surface.lastUpdateTime = currentTimeUs;
|
||||
|
||||
if (newSurfaceAlt >= 0) {
|
||||
if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
|
||||
newReliabilityMeasurement = 1.0f;
|
||||
posEstimator.surface.alt = newSurfaceAlt;
|
||||
}
|
||||
else {
|
||||
newReliabilityMeasurement = 0.0f;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Negative values - out of range or failed hardware
|
||||
newReliabilityMeasurement = 0.0f;
|
||||
}
|
||||
|
||||
/* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
|
||||
if (dt > 0.5f) {
|
||||
posEstimator.surface.reliability = 0.0f;
|
||||
}
|
||||
else {
|
||||
const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
|
||||
posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_OPTICAL_FLOW
|
||||
/**
|
||||
* Read optical flow topic
|
||||
* Function is called by OPFLOW task as soon as new update is available
|
||||
*/
|
||||
void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
|
||||
{
|
||||
posEstimator.flow.lastUpdateTime = currentTimeUs;
|
||||
posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID);
|
||||
posEstimator.flow.flowRate[X] = opflow.flowRate[X];
|
||||
posEstimator.flow.flowRate[Y] = opflow.flowRate[Y];
|
||||
posEstimator.flow.bodyRate[X] = opflow.bodyRate[X];
|
||||
posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y];
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Update IMU topic
|
||||
* Function is called at main loop rate
|
||||
|
|
@ -611,62 +414,97 @@ static void updateIMUTopic(void)
|
|||
}
|
||||
}
|
||||
|
||||
static float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w)
|
||||
float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w)
|
||||
{
|
||||
return oldEPE + (newEPE - oldEPE) * w * dt;
|
||||
}
|
||||
|
||||
static bool navIsAccelerationUsable(void)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool navIsHeadingUsable(void)
|
||||
{
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
// If we have GPS - we need true IMU north (valid heading)
|
||||
return isImuHeadingValid();
|
||||
}
|
||||
else {
|
||||
// If we don't have GPS - we may use whatever we have, other sensors are operating in body frame
|
||||
return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
|
||||
* Function is called at main loop rate
|
||||
*/
|
||||
static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
||||
static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
|
||||
{
|
||||
/* Calculate dT */
|
||||
float dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
|
||||
posEstimator.est.lastUpdateTime = currentTimeUs;
|
||||
|
||||
/* If IMU is not ready we can't estimate anything */
|
||||
if (!isImuReady()) {
|
||||
posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
|
||||
posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
|
||||
return;
|
||||
}
|
||||
|
||||
/* Calculate new EPH and EPV for the case we didn't update postion */
|
||||
float newEPH = posEstimator.est.eph;
|
||||
float newEPV = posEstimator.est.epv;
|
||||
|
||||
if (newEPH <= positionEstimationConfig()->max_eph_epv) {
|
||||
newEPH *= 1.0f + dt;
|
||||
}
|
||||
|
||||
if (newEPV <= positionEstimationConfig()->max_eph_epv) {
|
||||
newEPV *= 1.0f + dt;
|
||||
}
|
||||
|
||||
/* Figure out if we have valid position data from our data sources */
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
//isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected;
|
||||
#endif
|
||||
const bool isGPSValid = sensors(SENSOR_GPS) &&
|
||||
posControl.gpsOrigin.valid &&
|
||||
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
|
||||
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv);
|
||||
const bool isGPSZValid = isGPSValid && (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv);
|
||||
const bool isBaroValid = sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS));
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
const bool isSurfaceValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS));
|
||||
#endif
|
||||
uint32_t newFlags = 0;
|
||||
|
||||
if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid &&
|
||||
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
|
||||
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
|
||||
if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
|
||||
}
|
||||
else {
|
||||
newFlags |= EST_GPS_XY_VALID;
|
||||
}
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS))) {
|
||||
newFlags |= EST_BARO_VALID;
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS))) {
|
||||
newFlags |= EST_SURFACE_VALID;
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_OPFLOW) && posEstimator.flow.isValid && ((currentTimeUs - posEstimator.flow.lastUpdateTime) <= MS2US(INAV_FLOW_TIMEOUT_MS))) {
|
||||
newFlags |= EST_FLOW_VALID;
|
||||
}
|
||||
|
||||
if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
|
||||
newFlags |= EST_XY_VALID;
|
||||
}
|
||||
|
||||
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
newFlags |= EST_Z_VALID;
|
||||
}
|
||||
|
||||
return newFlags;
|
||||
}
|
||||
|
||||
static void estimationPredict(estimationContext_t * ctx)
|
||||
{
|
||||
/* Prediction step: Z-axis */
|
||||
if ((ctx->newFlags & EST_Z_VALID)) {
|
||||
posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
|
||||
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
|
||||
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
|
||||
}
|
||||
|
||||
/* Prediction step: XY-axis */
|
||||
if ((ctx->newFlags & EST_XY_VALID)) {
|
||||
// Predict based on known velocity
|
||||
posEstimator.est.pos.x += posEstimator.est.vel.x * ctx->dt;
|
||||
posEstimator.est.pos.y += posEstimator.est.vel.y * ctx->dt;
|
||||
|
||||
// If heading is valid, accelNEU is valid as well. Account for acceleration
|
||||
if (navIsHeadingUsable() && navIsAccelerationUsable()) {
|
||||
posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
|
||||
posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
|
||||
posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
|
||||
posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
|
||||
{
|
||||
if (ctx->newFlags & EST_BARO_VALID) {
|
||||
timeUs_t currentTimeUs = micros();
|
||||
|
||||
/* Do some preparations to data */
|
||||
if (isBaroValid) {
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
posEstimator.state.baroGroundAlt = posEstimator.est.pos.z;
|
||||
posEstimator.state.isBaroGroundValid = true;
|
||||
|
|
@ -682,261 +520,189 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
posEstimator.state.isBaroGroundValid = false;
|
||||
}
|
||||
|
||||
#if defined(USE_BARO)
|
||||
/* We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it */
|
||||
// We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
|
||||
bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
|
||||
((isSurfaceValid && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
|
||||
(isBaroValid && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
|
||||
#endif
|
||||
(((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
|
||||
((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
|
||||
|
||||
/* Validate EPV for GPS and calculate altitude/climb rate correction flags */
|
||||
const bool useGpsZPos = STATE(FIXED_WING) && !sensors(SENSOR_BARO) && isGPSValid && isGPSZValid;
|
||||
const bool useGpsZVel = isGPSValid && isGPSZValid;
|
||||
// Altitude
|
||||
const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
|
||||
ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
|
||||
ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
|
||||
|
||||
/* Estimate validity */
|
||||
const bool isEstXYValid = (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv);
|
||||
const bool isEstZValid = (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv);
|
||||
|
||||
/* Prediction step: Z-axis */
|
||||
if (isEstZValid) {
|
||||
inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.z);
|
||||
// If GPS is available - also use GPS climb rate
|
||||
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);
|
||||
ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
|
||||
}
|
||||
|
||||
/* Prediction step: XY-axis */
|
||||
if (isEstXYValid) {
|
||||
if (navIsHeadingUsable()) {
|
||||
inavFilterPredict(X, dt, posEstimator.imu.accelNEU.x);
|
||||
inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.y);
|
||||
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
|
||||
|
||||
// Accelerometer bias
|
||||
if (!isAirCushionEffectDetected) {
|
||||
ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
else if (STATE(FIXED_WING) && (ctx->newFlags & EST_GPS_Z_VALID)) {
|
||||
// If baro is not available - use GPS Z for correction on a plane
|
||||
// Reset current estimate to GPS altitude if estimate not valid
|
||||
if (!(ctx->newFlags & EST_Z_VALID)) {
|
||||
ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
|
||||
ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z;
|
||||
ctx->newEPV = posEstimator.gps.epv;
|
||||
}
|
||||
else {
|
||||
inavFilterPredict(X, dt, 0.0f);
|
||||
inavFilterPredict(Y, dt, 0.0f);
|
||||
}
|
||||
// Altitude
|
||||
const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
|
||||
|
||||
ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
|
||||
ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
|
||||
ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
|
||||
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
|
||||
|
||||
// Accelerometer bias
|
||||
ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
|
||||
}
|
||||
|
||||
/* Accelerometer bias correction */
|
||||
const bool updateAccBias = (positionEstimationConfig()->w_acc_bias > 0);
|
||||
fpVector3_t accelBiasCorr = { { 0, 0, 0} };
|
||||
|
||||
/* Correction step: Z-axis */
|
||||
if (useGpsZPos || isBaroValid) {
|
||||
float gpsWeightScaler = 1.0f;
|
||||
|
||||
/* Handle Z-axis reset */
|
||||
if (!isEstZValid && useGpsZPos) {
|
||||
posEstimator.est.pos.z = posEstimator.gps.pos.z;
|
||||
posEstimator.est.vel.z = posEstimator.gps.vel.z;
|
||||
newEPV = posEstimator.gps.epv;
|
||||
}
|
||||
else {
|
||||
#if defined(USE_BARO)
|
||||
/* Apply BARO correction to altitude */
|
||||
if (isBaroValid) {
|
||||
const float baroResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
|
||||
inavFilterCorrectPos(Z, dt, baroResidual, positionEstimationConfig()->w_z_baro_p);
|
||||
newEPV = updateEPE(posEstimator.est.epv, dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
|
||||
|
||||
/* accelerometer bias correction for baro */
|
||||
if (updateAccBias && !isAirCushionEffectDetected) {
|
||||
accelBiasCorr.z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/* Apply GPS correction to altitude */
|
||||
if (useGpsZPos) {
|
||||
const float gpsResidualZ = posEstimator.gps.pos.z - posEstimator.est.pos.z;
|
||||
inavFilterCorrectPos(Z, dt, gpsResidualZ, positionEstimationConfig()->w_z_gps_p * gpsWeightScaler);
|
||||
newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, gpsResidualZ), positionEstimationConfig()->w_z_gps_p);
|
||||
|
||||
if (updateAccBias) {
|
||||
accelBiasCorr.z -= gpsResidualZ * sq(positionEstimationConfig()->w_z_gps_p);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Apply GPS correction to climb rate */
|
||||
if (useGpsZVel) {
|
||||
const float gpsResidualZVel = posEstimator.gps.vel.z - posEstimator.est.vel.z;
|
||||
inavFilterCorrectVel(Z, dt, gpsResidualZVel, positionEstimationConfig()->w_z_gps_v * sq(gpsWeightScaler));
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.z, positionEstimationConfig()->w_z_res_v);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Correction step: XY-axis */
|
||||
/* GPS */
|
||||
if (isGPSValid) {
|
||||
static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
|
||||
{
|
||||
if (ctx->newFlags & EST_GPS_XY_VALID) {
|
||||
/* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */
|
||||
if (!isEstXYValid) {
|
||||
posEstimator.est.pos.x = posEstimator.gps.pos.x;
|
||||
posEstimator.est.pos.y = posEstimator.gps.pos.y;
|
||||
posEstimator.est.vel.x = posEstimator.gps.vel.x;
|
||||
posEstimator.est.vel.y = posEstimator.gps.vel.y;
|
||||
newEPH = posEstimator.gps.eph;
|
||||
if (!(ctx->newFlags & EST_XY_VALID)) {
|
||||
ctx->estPosCorr.x += posEstimator.gps.pos.x - posEstimator.est.pos.x;
|
||||
ctx->estPosCorr.y += posEstimator.gps.pos.y - posEstimator.est.pos.y;
|
||||
ctx->estVelCorr.x += posEstimator.gps.vel.x - posEstimator.est.vel.x;
|
||||
ctx->estVelCorr.y += posEstimator.gps.vel.y - posEstimator.est.vel.y;
|
||||
ctx->newEPH = posEstimator.gps.epv;
|
||||
}
|
||||
else {
|
||||
const float gpsResidualX = posEstimator.gps.pos.x - posEstimator.est.pos.x;
|
||||
const float gpsResidualY = posEstimator.gps.pos.y - posEstimator.est.pos.y;
|
||||
const float gpsResidualXVel = posEstimator.gps.vel.x - posEstimator.est.vel.x;
|
||||
const float gpsResidualYVel = posEstimator.gps.vel.y - posEstimator.est.vel.y;
|
||||
const float gpsResidualXYMagnitude = sqrtf(sq(gpsResidualX) + sq(gpsResidualY));
|
||||
const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x;
|
||||
const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y;
|
||||
const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x;
|
||||
const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y;
|
||||
const float gpsPosResidualMag = sqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
|
||||
|
||||
//const float gpsWeightScaler = scaleRangef(bellCurve(gpsResidualXYMagnitude, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
//const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
const float gpsWeightScaler = 1.0f;
|
||||
|
||||
const float w_xy_gps_p = positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler;
|
||||
const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler);
|
||||
|
||||
inavFilterCorrectPos(X, dt, gpsResidualX, w_xy_gps_p);
|
||||
inavFilterCorrectPos(Y, dt, gpsResidualY, w_xy_gps_p);
|
||||
// Coordinates
|
||||
ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt;
|
||||
ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt;
|
||||
|
||||
inavFilterCorrectVel(X, dt, gpsResidualXVel, w_xy_gps_v);
|
||||
inavFilterCorrectVel(Y, dt, gpsResidualYVel, w_xy_gps_v);
|
||||
// Velocity from coordinates
|
||||
ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt;
|
||||
ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt;
|
||||
|
||||
// Velocity from direct measurement
|
||||
ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt;
|
||||
ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt;
|
||||
|
||||
// Accelerometer bias
|
||||
ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p);
|
||||
ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p);
|
||||
|
||||
/* Adjust EPH */
|
||||
newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsResidualXYMagnitude), positionEstimationConfig()->w_xy_gps_p);
|
||||
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.x, positionEstimationConfig()->w_xy_res_v);
|
||||
inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.y, positionEstimationConfig()->w_xy_res_v);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
|
||||
* Function is called at main loop rate
|
||||
*/
|
||||
static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
||||
{
|
||||
estimationContext_t ctx;
|
||||
|
||||
/* Calculate dT */
|
||||
ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
|
||||
posEstimator.est.lastUpdateTime = currentTimeUs;
|
||||
|
||||
/* If IMU is not ready we can't estimate anything */
|
||||
if (!isImuReady()) {
|
||||
posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
|
||||
posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
|
||||
posEstimator.flags = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* Calculate new EPH and EPV for the case we didn't update postion */
|
||||
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
|
||||
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
|
||||
ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
|
||||
vectorZero(&ctx.estPosCorr);
|
||||
vectorZero(&ctx.estVelCorr);
|
||||
vectorZero(&ctx.accBiasCorr);
|
||||
|
||||
/* AGL estimation - separate process, decouples from Z coordinate */
|
||||
estimationCalculateAGL(&ctx);
|
||||
|
||||
/* Prediction stage: X,Y,Z */
|
||||
estimationPredict(&ctx);
|
||||
|
||||
/* Correction stage: Z */
|
||||
const bool estZCorrectOk =
|
||||
estimationCalculateCorrection_Z(&ctx);
|
||||
|
||||
/* Correction stage: XY: GPS, FLOW */
|
||||
// FIXME: Handle transition from FLOW to GPS and back - seamlessly fly indoor/outdoor
|
||||
const bool estXYCorrectOk =
|
||||
estimationCalculateCorrection_XY_GPS(&ctx) ||
|
||||
estimationCalculateCorrection_XY_FLOW(&ctx);
|
||||
|
||||
// If we can't apply correction or accuracy is off the charts - decay velocity to zero
|
||||
if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
|
||||
ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
|
||||
ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
|
||||
}
|
||||
|
||||
if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
|
||||
ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
|
||||
}
|
||||
|
||||
// Apply corrections
|
||||
vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
|
||||
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
|
||||
|
||||
/* Correct accelerometer bias */
|
||||
if (updateAccBias) {
|
||||
const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);
|
||||
if (positionEstimationConfig()->w_acc_bias > 0.0f) {
|
||||
const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
|
||||
if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
|
||||
/* transform error vector from NEU frame to body frame */
|
||||
imuTransformVectorEarthToBody(&accelBiasCorr);
|
||||
imuTransformVectorEarthToBody(&ctx.accBiasCorr);
|
||||
|
||||
/* Correct accel bias */
|
||||
posEstimator.imu.accelBias.x += accelBiasCorr.x * positionEstimationConfig()->w_acc_bias * dt;
|
||||
posEstimator.imu.accelBias.y += accelBiasCorr.y * positionEstimationConfig()->w_acc_bias * dt;
|
||||
posEstimator.imu.accelBias.z += accelBiasCorr.z * positionEstimationConfig()->w_acc_bias * dt;
|
||||
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
|
||||
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
|
||||
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
|
||||
}
|
||||
}
|
||||
|
||||
/* Update uncertainty */
|
||||
posEstimator.est.eph = newEPH;
|
||||
posEstimator.est.epv = newEPV;
|
||||
posEstimator.est.eph = ctx.newEPH;
|
||||
posEstimator.est.epv = ctx.newEPV;
|
||||
|
||||
/* AGL estimation */
|
||||
#if defined(USE_RANGEFINDER) && defined(USE_BARO)
|
||||
if (isSurfaceValid && isBaroValid) {
|
||||
navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
|
||||
bool resetSurfaceEstimate = false;
|
||||
switch (posEstimator.est.aglQual) {
|
||||
case SURFACE_QUAL_LOW:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
resetSurfaceEstimate = true;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case SURFACE_QUAL_MID:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_MID;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case SURFACE_QUAL_HIGH:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_MID;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
posEstimator.est.aglQual = newAglQuality;
|
||||
|
||||
if (resetSurfaceEstimate) {
|
||||
posEstimator.est.aglAlt = posEstimator.surface.alt;
|
||||
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
|
||||
}
|
||||
else {
|
||||
posEstimator.est.aglVel = 0;
|
||||
posEstimator.est.aglOffset = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Update estimate
|
||||
posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.z * dt * dt * 0.5f;
|
||||
posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * dt;
|
||||
|
||||
// Apply correction
|
||||
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
|
||||
// Correct estimate from rangefinder
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
|
||||
const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
|
||||
posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * dt;
|
||||
posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * dt;
|
||||
|
||||
// Update estimate offset
|
||||
if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
|
||||
}
|
||||
}
|
||||
else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
|
||||
// Correct estimate from altitude fused from rangefinder and global altitude
|
||||
const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
|
||||
const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
|
||||
const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
|
||||
|
||||
posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * dt;
|
||||
posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * dt;
|
||||
}
|
||||
else { // SURFACE_QUAL_LOW
|
||||
// In this case rangefinder can't be trusted - simply use global altitude
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
}
|
||||
}
|
||||
else {
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
|
||||
DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
|
||||
DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
|
||||
DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
|
||||
#endif
|
||||
|
||||
#else
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
#endif
|
||||
// Keep flags for further usage
|
||||
posEstimator.flags = ctx.newFlags;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -954,10 +720,11 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
|
||||
/* Publish position update */
|
||||
if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
|
||||
updateActualHorizontalPositionAndVelocity(true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
|
||||
// FIXME!!!!!
|
||||
updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
|
||||
}
|
||||
else {
|
||||
updateActualHorizontalPositionAndVelocity(false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
|
||||
updateActualHorizontalPositionAndVelocity(false, false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
|
||||
}
|
||||
|
||||
/* Publish altitude update and set altitude validity */
|
||||
|
|
@ -1001,6 +768,9 @@ void initializePositionEstimator(void)
|
|||
posEstimator.est.aglAlt = 0;
|
||||
posEstimator.est.aglVel = 0;
|
||||
|
||||
posEstimator.est.flowCoordinates[X] = 0;
|
||||
posEstimator.est.flowCoordinates[Y] = 0;
|
||||
|
||||
posEstimator.imu.gravityCalibrationComplete = false;
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
|
|
|
|||
195
src/main/navigation/navigation_pos_estimator_agl.c
Normal file
195
src/main/navigation/navigation_pos_estimator_agl.c
Normal file
|
|
@ -0,0 +1,195 @@
|
|||
/*
|
||||
* 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 <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#if defined(USE_NAV)
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
#include "navigation/navigation_private.h"
|
||||
#include "navigation/navigation_pos_estimator_private.h"
|
||||
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/barometer.h"
|
||||
|
||||
extern navigationPosEstimator_t posEstimator;
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
/**
|
||||
* Read surface and update alt/vel topic
|
||||
* Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
|
||||
*/
|
||||
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
|
||||
{
|
||||
const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime);
|
||||
float newReliabilityMeasurement = 0;
|
||||
|
||||
posEstimator.surface.lastUpdateTime = currentTimeUs;
|
||||
|
||||
if (newSurfaceAlt >= 0) {
|
||||
if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
|
||||
newReliabilityMeasurement = 1.0f;
|
||||
posEstimator.surface.alt = newSurfaceAlt;
|
||||
}
|
||||
else {
|
||||
newReliabilityMeasurement = 0.0f;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Negative values - out of range or failed hardware
|
||||
newReliabilityMeasurement = 0.0f;
|
||||
}
|
||||
|
||||
/* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
|
||||
if (dt > 0.5f) {
|
||||
posEstimator.surface.reliability = 0.0f;
|
||||
}
|
||||
else {
|
||||
const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
|
||||
posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void estimationCalculateAGL(estimationContext_t * ctx)
|
||||
{
|
||||
#if defined(USE_RANGEFINDER) && defined(USE_BARO)
|
||||
if ((ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_BARO_VALID)) {
|
||||
navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
|
||||
bool resetSurfaceEstimate = false;
|
||||
switch (posEstimator.est.aglQual) {
|
||||
case SURFACE_QUAL_LOW:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
resetSurfaceEstimate = true;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case SURFACE_QUAL_MID:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_MID;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case SURFACE_QUAL_HIGH:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_MID;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
posEstimator.est.aglQual = newAglQuality;
|
||||
|
||||
if (resetSurfaceEstimate) {
|
||||
posEstimator.est.aglAlt = posEstimator.surface.alt;
|
||||
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
|
||||
}
|
||||
else {
|
||||
posEstimator.est.aglVel = 0;
|
||||
posEstimator.est.aglOffset = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Update estimate
|
||||
posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * ctx->dt;
|
||||
posEstimator.est.aglAlt = posEstimator.imu.accelNEU.z * sq(ctx->dt) * 0.5f;
|
||||
posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * ctx->dt;
|
||||
|
||||
// Apply correction
|
||||
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
|
||||
// Correct estimate from rangefinder
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
|
||||
const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
|
||||
posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * ctx->dt;
|
||||
posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * ctx->dt;
|
||||
|
||||
// Update estimate offset
|
||||
if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
|
||||
}
|
||||
}
|
||||
else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
|
||||
// Correct estimate from altitude fused from rangefinder and global altitude
|
||||
const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
|
||||
const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
|
||||
const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
|
||||
|
||||
posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * ctx->dt;
|
||||
posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * ctx->dt;
|
||||
}
|
||||
else { // SURFACE_QUAL_LOW
|
||||
// In this case rangefinder can't be trusted - simply use global altitude
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
}
|
||||
}
|
||||
else {
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
|
||||
DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
|
||||
DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
|
||||
DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
|
||||
|
||||
#else
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.z;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.z;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // NAV
|
||||
128
src/main/navigation/navigation_pos_estimator_flow.c
Normal file
128
src/main/navigation/navigation_pos_estimator_flow.c
Normal file
|
|
@ -0,0 +1,128 @@
|
|||
/*
|
||||
* 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 <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#if defined(USE_NAV)
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
#include "navigation/navigation_private.h"
|
||||
#include "navigation/navigation_pos_estimator_private.h"
|
||||
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/opflow.h"
|
||||
|
||||
#include "flight/imu.h"
|
||||
|
||||
extern navigationPosEstimator_t posEstimator;
|
||||
|
||||
#ifdef USE_OPTICAL_FLOW
|
||||
/**
|
||||
* Read optical flow topic
|
||||
* Function is called by OPFLOW task as soon as new update is available
|
||||
*/
|
||||
void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
|
||||
{
|
||||
posEstimator.flow.lastUpdateTime = currentTimeUs;
|
||||
posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID);
|
||||
posEstimator.flow.flowRate[X] = opflow.flowRate[X];
|
||||
posEstimator.flow.flowRate[Y] = opflow.flowRate[Y];
|
||||
posEstimator.flow.bodyRate[X] = opflow.bodyRate[X];
|
||||
posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y];
|
||||
}
|
||||
#endif
|
||||
|
||||
bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
|
||||
{
|
||||
#if defined(USE_RANGEFINDER) && defined(USE_OPTICAL_FLOW)
|
||||
if (!((ctx->newFlags & EST_FLOW_VALID) && (ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_Z_VALID))) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// FIXME: flow may use AGL estimate if available
|
||||
const bool canUseFlow = (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD);
|
||||
|
||||
if (!canUseFlow) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Calculate linear velocity based on angular velocity and altitude
|
||||
// Technically we should calculate arc length here, but for fast sampling this is accurate enough
|
||||
fpVector3_t flowVel = {
|
||||
.x = - (posEstimator.flow.flowRate[Y] - posEstimator.flow.bodyRate[Y]) * posEstimator.surface.alt,
|
||||
.y = (posEstimator.flow.flowRate[X] - posEstimator.flow.bodyRate[X]) * posEstimator.surface.alt,
|
||||
.z = posEstimator.est.vel.z
|
||||
};
|
||||
|
||||
// At this point flowVel will hold linear velocities in earth frame
|
||||
imuTransformVectorBodyToEarth(&flowVel);
|
||||
|
||||
// Calculate velocity correction
|
||||
const float flowVelXInnov = flowVel.x - posEstimator.est.vel.x;
|
||||
const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y;
|
||||
|
||||
ctx->estVelCorr.x = flowVelXInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt;
|
||||
ctx->estVelCorr.y = flowVelYInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt;
|
||||
|
||||
// Calculate position correction if possible/allowed
|
||||
if ((ctx->newFlags & EST_GPS_XY_VALID)) {
|
||||
// If GPS is valid - reset flow estimated coordinates to GPS
|
||||
posEstimator.est.flowCoordinates[X] = posEstimator.gps.pos.x;
|
||||
posEstimator.est.flowCoordinates[Y] = posEstimator.gps.pos.y;
|
||||
}
|
||||
else if (positionEstimationConfig()->allow_dead_reckoning) {
|
||||
posEstimator.est.flowCoordinates[X] += flowVel.x * ctx->dt;
|
||||
posEstimator.est.flowCoordinates[Y] += flowVel.y * ctx->dt;
|
||||
|
||||
const float flowResidualX = posEstimator.est.flowCoordinates[X] - posEstimator.est.pos.x;
|
||||
const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y;
|
||||
|
||||
ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
|
||||
ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
|
||||
|
||||
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, sqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[Y]));
|
||||
DEBUG_SET(DEBUG_FLOW, 1, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X]));
|
||||
DEBUG_SET(DEBUG_FLOW, 2, posEstimator.est.flowCoordinates[X]);
|
||||
DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]);
|
||||
|
||||
return true;
|
||||
#else
|
||||
UNUSED(ctx);
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#endif // NAV
|
||||
175
src/main/navigation/navigation_pos_estimator_private.h
Normal file
175
src/main/navigation/navigation_pos_estimator_private.h
Normal file
|
|
@ -0,0 +1,175 @@
|
|||
/*
|
||||
* 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 <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#define INAV_GPS_DEFAULT_EPH 200.0f // 2m GPS HDOP (gives about 1.6s of dead-reckoning if GPS is temporary lost)
|
||||
#define INAV_GPS_DEFAULT_EPV 500.0f // 5m GPS VDOP
|
||||
|
||||
#define INAV_GPS_ACCEPTANCE_EPE 500.0f // 5m acceptance radius
|
||||
|
||||
#define INAV_ACC_BIAS_ACCEPTANCE_VALUE (GRAVITY_CMSS * 0.25f) // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway
|
||||
|
||||
#define INAV_GPS_GLITCH_RADIUS 250.0f // 2.5m GPS glitch radius
|
||||
#define INAV_GPS_GLITCH_ACCEL 1000.0f // 10m/s/s max possible acceleration for GPS glitch detection
|
||||
|
||||
#define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate
|
||||
#define INAV_PITOT_UPDATE_RATE 10
|
||||
|
||||
#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout
|
||||
#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout
|
||||
#define INAV_SURFACE_TIMEOUT_MS 300 // Surface timeout (missed 3 readings in a row)
|
||||
#define INAV_FLOW_TIMEOUT_MS 200
|
||||
|
||||
#define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f)
|
||||
#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
|
||||
#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f)
|
||||
#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD (0.75f)
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastTriggeredTime;
|
||||
timeUs_t deltaTime;
|
||||
} navigationTimer_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
bool glitchDetected;
|
||||
bool glitchRecovery;
|
||||
#endif
|
||||
fpVector3_t pos; // GPS position in NEU coordinate system (cm)
|
||||
fpVector3_t vel; // GPS velocity (cms)
|
||||
float eph;
|
||||
float epv;
|
||||
} navPositionEstimatorGPS_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
float alt; // Raw barometric altitude (cm)
|
||||
float epv;
|
||||
} navPositionEstimatorBARO_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
float airspeed; // airspeed (cm/s)
|
||||
} navPositionEstimatorPITOT_t;
|
||||
|
||||
typedef enum {
|
||||
SURFACE_QUAL_LOW, // Surface sensor signal lost long ago - most likely surface distance is incorrect
|
||||
SURFACE_QUAL_MID, // Surface sensor is not available but we can somewhat trust the estimate
|
||||
SURFACE_QUAL_HIGH // All good
|
||||
} navAGLEstimateQuality_e;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
float alt; // Raw altitude measurement (cm)
|
||||
float reliability;
|
||||
} navPositionEstimatorSURFACE_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
bool isValid;
|
||||
float quality;
|
||||
float flowRate[2];
|
||||
float bodyRate[2];
|
||||
} navPositionEstimatorFLOW_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
|
||||
// 3D position, velocity and confidence
|
||||
fpVector3_t pos;
|
||||
fpVector3_t vel;
|
||||
float eph;
|
||||
float epv;
|
||||
|
||||
// AGL
|
||||
navAGLEstimateQuality_e aglQual;
|
||||
float aglOffset; // Offset between surface and pos.Z
|
||||
float aglAlt;
|
||||
float aglVel;
|
||||
|
||||
// FLOW
|
||||
float flowCoordinates[2];
|
||||
} navPositionEstimatorESTIMATE_t;
|
||||
|
||||
typedef struct {
|
||||
fpVector3_t accelNEU;
|
||||
fpVector3_t accelBias;
|
||||
bool gravityCalibrationComplete;
|
||||
} navPosisitonEstimatorIMU_t;
|
||||
|
||||
typedef enum {
|
||||
EST_GPS_XY_VALID = (1 << 0),
|
||||
EST_GPS_Z_VALID = (1 << 1),
|
||||
EST_BARO_VALID = (1 << 2),
|
||||
EST_SURFACE_VALID = (1 << 3),
|
||||
EST_FLOW_VALID = (1 << 4),
|
||||
EST_XY_VALID = (1 << 5),
|
||||
EST_Z_VALID = (1 << 6),
|
||||
} navPositionEstimationFlags_e;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t baroGroundTimeout;
|
||||
float baroGroundAlt;
|
||||
bool isBaroGroundValid;
|
||||
} navPositionEstimatorSTATE_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
uint32_t flags;
|
||||
|
||||
// Data sources
|
||||
navPositionEstimatorGPS_t gps;
|
||||
navPositionEstimatorBARO_t baro;
|
||||
navPositionEstimatorSURFACE_t surface;
|
||||
navPositionEstimatorPITOT_t pitot;
|
||||
navPositionEstimatorFLOW_t flow;
|
||||
|
||||
// IMU data
|
||||
navPosisitonEstimatorIMU_t imu;
|
||||
|
||||
// Estimate
|
||||
navPositionEstimatorESTIMATE_t est;
|
||||
|
||||
// Extra state variables
|
||||
navPositionEstimatorSTATE_t state;
|
||||
} navigationPosEstimator_t;
|
||||
|
||||
typedef struct {
|
||||
float dt;
|
||||
uint32_t newFlags;
|
||||
float newEPV;
|
||||
float newEPH;
|
||||
fpVector3_t estPosCorr;
|
||||
fpVector3_t estVelCorr;
|
||||
fpVector3_t accBiasCorr;
|
||||
} estimationContext_t;
|
||||
|
||||
extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
|
||||
extern void estimationCalculateAGL(estimationContext_t * ctx);
|
||||
extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);
|
||||
|
||||
|
||||
|
|
@ -21,6 +21,8 @@
|
|||
|
||||
#if defined(USE_NAV)
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
|
|
@ -80,6 +82,7 @@ typedef struct navigationFlags_s {
|
|||
|
||||
navigationEstimateStatus_e estAltStatus; // Indicates that we have a working altitude sensor (got at least one valid reading from it)
|
||||
navigationEstimateStatus_e estPosStatus; // Indicates that GPS is working (or not)
|
||||
navigationEstimateStatus_e estVelStatus; // Indicates that GPS is working (or not)
|
||||
navigationEstimateStatus_e estAglStatus;
|
||||
navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane
|
||||
|
||||
|
|
@ -385,7 +388,7 @@ bool isApproachingLastWaypoint(void);
|
|||
float getActiveWaypointSpeed(void);
|
||||
|
||||
void updateActualHeading(bool headingValid, int32_t newHeading);
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY);
|
||||
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);
|
||||
|
||||
bool checkForPositionSensorTimeout(void);
|
||||
|
|
|
|||
|
|
@ -73,8 +73,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLO
|
|||
|
||||
PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig,
|
||||
.opflow_hardware = OPFLOW_NONE,
|
||||
.opflow_scale = 1.0f,
|
||||
.opflow_align = CW0_DEG_FLIP,
|
||||
.opflow_scale = 1.0f,
|
||||
);
|
||||
|
||||
static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse)
|
||||
|
|
@ -178,19 +178,19 @@ void opflowUpdate(timeUs_t currentTimeUs)
|
|||
|
||||
if ((opflow.flowQuality == OPFLOW_QUALITY_VALID) && (opflow.gyroBodyRateTimeUs > 0)) {
|
||||
const float integralToRateScaler = (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale;
|
||||
int32_t flowRateRaw[3] = { opflow.dev.rawData.flowRateRaw[X], opflow.dev.rawData.flowRateRaw[Y], 0 };
|
||||
|
||||
applySensorAlignment(flowRateRaw, flowRateRaw, opticalFlowConfig()->opflow_align);
|
||||
//applyBoardAlignment(flowRateRaw);
|
||||
// Apply sensor alignment
|
||||
applySensorAlignment(opflow.dev.rawData.flowRateRaw, opflow.dev.rawData.flowRateRaw, opticalFlowConfig()->opflow_align);
|
||||
//applyBoardAlignment(opflow.dev.rawData.flowRateRaw);
|
||||
|
||||
opflow.flowRate[X] = DEGREES_TO_RADIANS(flowRateRaw[X] * integralToRateScaler);
|
||||
opflow.flowRate[Y] = DEGREES_TO_RADIANS(flowRateRaw[Y] * integralToRateScaler);
|
||||
opflow.flowRate[X] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler);
|
||||
opflow.flowRate[Y] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[Y] * integralToRateScaler);
|
||||
|
||||
opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs);
|
||||
opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs);
|
||||
|
||||
DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.flowRate[X]));
|
||||
DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.flowRate[Y]));
|
||||
DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.dev.rawData.flowRateRaw[X]));
|
||||
DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.dev.rawData.flowRateRaw[Y]));
|
||||
DEBUG_SET(DEBUG_FLOW_RAW, 2, RADIANS_TO_DEGREES(opflow.bodyRate[X]));
|
||||
DEBUG_SET(DEBUG_FLOW_RAW, 3, RADIANS_TO_DEGREES(opflow.bodyRate[Y]));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,8 +43,8 @@ typedef enum {
|
|||
|
||||
typedef struct opticalFlowConfig_s {
|
||||
uint8_t opflow_hardware;
|
||||
float opflow_scale; // Scaler value to convert between raw sensor units to [deg/s]
|
||||
uint8_t opflow_align;
|
||||
float opflow_scale; // Scaler value to convert between raw sensor units to [deg/s]
|
||||
} opticalFlowConfig_t;
|
||||
|
||||
PG_DECLARE(opticalFlowConfig_t, opticalFlowConfig);
|
||||
|
|
|
|||
|
|
@ -105,6 +105,11 @@
|
|||
#define USE_BARO_BMP280
|
||||
#define BMP280_SPI_BUS BUS_SPI3
|
||||
#define BMP280_CS_PIN PB3 // v1
|
||||
|
||||
// Support external barometers
|
||||
#define BARO_I2C_BUS BUS_I2C2
|
||||
#define USE_BARO_BMP085
|
||||
#define USE_BARO_MS5611
|
||||
#else
|
||||
#define BARO_I2C_BUS BUS_I2C2
|
||||
#define USE_BARO_BMP085
|
||||
|
|
@ -118,6 +123,10 @@
|
|||
#define USE_RANGEFINDER
|
||||
#define RANGEFINDER_I2C_BUS BUS_I2C2
|
||||
#define USE_RANGEFINDER_HCSR04_I2C
|
||||
#define USE_RANGEFINDER_VL53L0X
|
||||
|
||||
#define USE_OPTICAL_FLOW
|
||||
#define USE_OPFLOW_CXOF
|
||||
|
||||
#define USE_VCP
|
||||
#define VBUS_SENSING_PIN PC5
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue