Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Add support for fixed height optical flow

This commit is contained in:
Stephen Dade 2022-10-24 14:01:04 +11:00 committed by Randy Mackay
parent b15cb46d25
commit c0df999094
5 changed files with 20 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1199,13 +1199,14 @@ bool NavEKF2::use_compass(void) const
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset)
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset);
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset);
core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
}
}
}

3
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -179,7 +179,8 @@ public:
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
// Set to true if the terrain underneath is stable enough to be used as a height reference
// in combination with a range finder. Set to false if the terrain underneath the vehicle

4
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -113,7 +113,7 @@ void NavEKF2_core::readRangeFinder(void)
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset)
void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
@ -158,6 +158,8 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
ofDataNew.flowRadXY = (-rawFlowRates).toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write the flow sensor position in body frame
ofDataNew.body_offset = posOffset.toftype();
// write the flow sensor height override
ofDataNew.heightOverride = heightOverride;
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;

8
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -1,4 +1,5 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include "AP_NavEKF2.h"
#include "AP_NavEKF2_core.h"
@ -315,6 +316,13 @@ void NavEKF2_core::FuseOptFlow()
range -= posOffsetEarth.z / prevTnb.c.z;
}
// override with user specified height (if given, for rover)
#if APM_BUILD_TYPE(APM_BUILD_Rover)
if (ofDataDelayed.heightOverride > 0) {
range = ofDataDelayed.heightOverride;
}
#endif
// calculate relative velocity in sensor frame including the relative motion due to rotation
relVelSensor = prevTnb*stateStruct.velocity + ofDataDelayed.bodyRadXYZ % posOffsetBody;

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -199,7 +199,8 @@ public:
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
/*
Returns the following data for debugging range beacon fusion
@ -461,6 +462,7 @@ private:
Vector2F flowRadXYcomp;
Vector3F bodyRadXYZ;
Vector3F body_offset;
float heightOverride;
};
struct ext_nav_elements : EKF_obs_element_t {