mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: stop relying on the presence of a RangeFinder
This commit is contained in:
parent
369292f7f3
commit
4431d01230
|
@ -607,9 +607,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
|
|||
AP_GROUPEND
|
||||
};
|
||||
|
||||
NavEKF3::NavEKF3(const AP_AHRS *ahrs, const RangeFinder &rng) :
|
||||
_ahrs(ahrs),
|
||||
_rng(rng)
|
||||
NavEKF3::NavEKF3(const AP_AHRS *ahrs) :
|
||||
_ahrs(ahrs)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
|
|
@ -25,7 +25,6 @@
|
|||
#include <AP_NavEKF/AP_Nav_Common.h>
|
||||
#include <AP_Airspeed/AP_Airspeed.h>
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
#include <AP_Logger/LogStructure.h>
|
||||
|
||||
class NavEKF3_core;
|
||||
|
@ -35,7 +34,7 @@ class NavEKF3 {
|
|||
friend class NavEKF3_core;
|
||||
|
||||
public:
|
||||
NavEKF3(const AP_AHRS *ahrs, const RangeFinder &rng);
|
||||
NavEKF3(const AP_AHRS *ahrs);
|
||||
|
||||
/* Do not allow copies */
|
||||
NavEKF3(const NavEKF3 &other) = delete;
|
||||
|
@ -390,7 +389,6 @@ private:
|
|||
uint8_t primary; // current primary core
|
||||
NavEKF3_core *core = nullptr;
|
||||
const AP_AHRS *_ahrs;
|
||||
const RangeFinder &_rng;
|
||||
|
||||
uint32_t _frameTimeUsec; // time per IMU frame
|
||||
uint8_t _framesPerPrediction; // expected number of IMU frames per prediction
|
||||
|
|
|
@ -5,6 +5,7 @@
|
|||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
|
@ -25,7 +26,11 @@ void NavEKF3_core::readRangeFinder(void)
|
|||
uint8_t minIndex;
|
||||
// get theoretical correct range when the vehicle is on the ground
|
||||
// don't allow range to go below 5cm because this can cause problems with optical flow processing
|
||||
rngOnGnd = MAX(frontend->_rng.ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f);
|
||||
const RangeFinder *_rng = AP::rangefinder();
|
||||
if (_rng == nullptr) {
|
||||
return;
|
||||
}
|
||||
rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f);
|
||||
|
||||
// limit update rate to maximum allowed by data buffers
|
||||
if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) {
|
||||
|
@ -37,7 +42,7 @@ void NavEKF3_core::readRangeFinder(void)
|
|||
// use data from two range finders if available
|
||||
|
||||
for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) {
|
||||
AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(sensorIndex);
|
||||
AP_RangeFinder_Backend *sensor = _rng->get_backend(sensorIndex);
|
||||
if (sensor == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
|
|
@ -5,6 +5,7 @@
|
|||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -112,7 +113,12 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const
|
|||
// only ask for limiting if we are doing optical flow navigation
|
||||
if (frontend->_fusionModeGPS == 3) {
|
||||
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
|
||||
height = MAX(float(frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f);
|
||||
const RangeFinder *_rng = AP::rangefinder();
|
||||
if (_rng == nullptr) {
|
||||
// we really, really shouldn't be here.
|
||||
return false;
|
||||
}
|
||||
height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f);
|
||||
// If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin
|
||||
if (frontend->_altSource != 1) {
|
||||
height -= terrainState;
|
||||
|
|
|
@ -5,6 +5,7 @@
|
|||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
|
@ -780,8 +781,9 @@ void NavEKF3_core::selectHeightForFusion()
|
|||
// correct range data for the body frame position offset relative to the IMU
|
||||
// the corrected reading is the reading that would have been taken if the sensor was
|
||||
// co-located with the IMU
|
||||
if (rangeDataToFuse) {
|
||||
AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(rangeDataDelayed.sensor_idx);
|
||||
const RangeFinder *_rng = AP::rangefinder();
|
||||
if (_rng && rangeDataToFuse) {
|
||||
AP_RangeFinder_Backend *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
|
||||
if (sensor != nullptr) {
|
||||
Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
|
||||
if (!posOffsetBody.is_zero()) {
|
||||
|
@ -796,13 +798,13 @@ void NavEKF3_core::selectHeightForFusion()
|
|||
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
||||
|
||||
// select height source
|
||||
if (((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
|
||||
if (_rng && ((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
|
||||
if (frontend->_altSource == 1) {
|
||||
// always use range finder
|
||||
activeHgtSource = HGT_SOURCE_RNG;
|
||||
} else {
|
||||
// determine if we are above or below the height switch region
|
||||
float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
|
||||
float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
|
||||
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
|
||||
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
|
||||
|
||||
|
|
Loading…
Reference in New Issue