Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: stop relying on the presence of a RangeFinder

Browse Source
This commit is contained in:
Peter Barker 2019-11-26 17:43:24 +11:00 committed by Peter Barker
parent 22d6fd5e1f
commit 369292f7f3
5 changed files with 25 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -585,9 +585,8 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
AP_GROUPEND
};
NavEKF2::NavEKF2(const AP_AHRS *ahrs, const RangeFinder &rng) :
_ahrs(ahrs),
_rng(rng)
NavEKF2::NavEKF2(const AP_AHRS *ahrs) :
_ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
}

4
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -28,7 +28,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 NavEKF2_core;
@ -38,7 +37,7 @@ class NavEKF2 {
friend class NavEKF2_core;
public:
NavEKF2(const AP_AHRS *ahrs, const RangeFinder &rng);
NavEKF2(const AP_AHRS *ahrs);
/* Do not allow copies */
NavEKF2(const NavEKF2 &other) = delete;
@ -361,7 +360,6 @@ private:
NavEKF2_core *core = nullptr;
bool core_malloc_failed;
const AP_AHRS *_ahrs;
const RangeFinder &_rng;
uint32_t _frameTimeUsec; // time per IMU frame
uint8_t _framesPerPrediction; // expected number of IMU frames per prediction

12
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -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>
@ -28,11 +29,16 @@ void NavEKF2_core::readRangeFinder(void)
// 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);
// read range finder at 20Hz
// TODO better way of knowing if it has new data
if ((imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
if (_rng && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
// reset the timer used to control the measurement rate
lastRngMeasTime_ms = imuSampleTime_ms;
@ -41,7 +47,7 @@ void NavEKF2_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;
}

8
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -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>
#include <stdio.h>
@ -103,7 +104,12 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
// only ask for limiting if we are doing optical flow only navigation
if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
// 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;

10
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -4,6 +4,7 @@
#include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.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>
@ -813,8 +814,9 @@ void NavEKF2_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) {
const AP_RangeFinder_Backend *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
if (sensor != nullptr) {
Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
if (!posOffsetBody.is_zero()) {
@ -832,13 +834,13 @@ void NavEKF2_core::selectHeightForFusion()
if (extNavUsedForPos) {
// always use external vision as the height source if using for position.
activeHgtSource = HGT_SOURCE_EV;
} else if (((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
} else 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;