主要看NavEKF2_core下面的selectHeightForFusion()函数。
首先从应用层读取高度，再进行角度补偿，分别有三种高度源：baro rangefinder GPS （可以在地面站中配置选择怎么开启和关闭）
(frontend->_useRngSwHgt > 0) 用户设置的rangefinder最大高度的百分比大于0。
(frontend->_altSource == 1) 其中_altSource 这个值是在地面站可以修改的。
_altSource 值代表用什么做主高度来源。0为气压计，1位测距仪，2为gps。
rangefinder_state.alt_healthy =((rangefinder.status() ==RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count()>= RANGEFINDER_HEALTH_MAX))；
//这就是判断状态标志的地方，根据在rangefinder驱动里的状态和采集到的有效值数量。我们可以直接把这个值强制为0，就是不管rangefinder怎么采集，都认为数据不健康，不去参与运算。下面的代码就不说了，和我没关系了。
然后就是一些逻辑的切换，如注释：
// select the height measurement to be fused from the available baro, range finder and GPS sources
//EKF2选择高度进行融合
void NavEKF2_core::selectHeightForFusion()
{
// Read range finder data and check for new data in the buffer
// This data is used by both height and optical flow fusion processing
//NRA24数据
readRangeFinder();
rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms); //若是检测到了有数据更新则给出一个信号
// 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) { //校正 range 的数据
AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(rangeDataDelayed.sensor_idx);
if (sensor != nullptr) {
Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
}
}
}
// read baro height data from the sensor and check for new data in the buffer
//获得MS5611气压计数据
readBaroData();
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms); //检查气压buff，得到一个标志
// select height source
//选择高度源
if (extNavUsedForPos) {
// always use external vision as the hight source if using for position.
//外部视觉来得到高度
activeHgtSource = HGT_SOURCE_EV;
}
//由于_useRngSwHgt>0 和 _altSource=1 只要满足一个就行，故之前设置为-1也没什么影响，只不过没加入这个减小最大range使用值。
else if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
if (frontend->_altSource == 1) { //此时在地面站上设置EK2_ALT_SOURCE =1 标记活跃的高度源为：rangefinder
// always use range finder
activeHgtSource = HGT_SOURCE_RNG;
} else { //若是选择了0，为气压
// determine if we are above or below the height switch region
// 确定我们是否在高度开关区域的上方或下方
// 得到按比例运算后的rangefinder高度 0.0001*5000*4%=200cm
float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
// What???
//terrainState 为 terrain position state (m)
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; //若是rangefinder的高度大于200cm则令此标记为1
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; //若是rangefinder的高度小于140cm则令此标记为1
//若是地形高度是连续的并且缓慢移动，接着地形高度能和rangefinder结合
// If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder
// apply a hysteresis to the speed check to prevent rapid switching
// 设置滞后
// 速度
//计算水平速度的模
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
//若是水平速度大于设置的最大速度(此时设置的是2m/s)并且速度是可用，或者高度发散则此标记记为1
bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
//判断标志，信任rangefinder
bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
/*
* Switch between range finder and primary height source using height above ground and speed thresholds with
* hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
* which cannot be assumed if the vehicle is moving horizontally.
*/
//切换高度源 （暂时只考虑高度），若是已经超过最大高度200cm并且选择的是Rangefinder则，若是已经超过最大水平速度2m/s并且选择的是Rangefinder则
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) { //当选择了NRA24后
// cannot trust terrain or range finder so stop using range finder height
if (frontend->_altSource == 0) {
activeHgtSource = HGT_SOURCE_BARO;
} else if (frontend->_altSource == 2) {
activeHgtSource = HGT_SOURCE_GPS;
}
} else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG)) {
// reliable terrain and range finder so start using range finder height
activeHgtSource = HGT_SOURCE_RNG;
}
}
//高度源为2 ，GPS高度
} else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
activeHgtSource = HGT_SOURCE_GPS;
// 不然选择3 Range Beacon.
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = HGT_SOURCE_BCN;
} else {
//否则为0，则选择气压计定高
activeHgtSource = HGT_SOURCE_BARO;
}
// Use Baro alt as a fallback if we lose range finder, GPS or external nav
//用气压高度作为一个fallback 来告诉飞控是否丢失了range finder的数据
bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500)); //若是超过500ms没收到则令此标志为1
bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); //若是超过2s没收到gps数据则令此标志为1
bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000)); //外部视觉定高
//若是当前选择的定高源丢失了则用气压定高
if (lostRngHgt || lostGpsHgt || lostExtNavHgt) {
activeHgtSource = HGT_SOURCE_BARO;
}
// if there is new baro data to fuse, calculate filtered baro data required by other processes
//若是有新的气压计数据，经过滤波
if (baroDataToFuse) {
// calculate offset to baro data that enables us to switch to Baro height use during operation
if (activeHgtSource != HGT_SOURCE_BARO) {
calcFiltBaroOffset();
}
// filtered baro data used to provide a reference for takeoff
// it is is reset to last height measurement on disarming in performArmingChecks()
//留着
if (!getTakeoffExpected()) {
const float gndHgtFiltTC = 0.5f;
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
}
}
//
// If we are not using GPS as the primary height sensor, correct EKF origin height so that
// combined local NED position height and origin height remains consistent with the GPS altitude
// This also enables the GPS height to be used as a backup height source
//若是不把GPS高度作为主要高度则作为备用高度
if (gpsDataToFuse &&
(((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == HGT_SOURCE_BARO)) ||
((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == HGT_SOURCE_RNG)))
) {
correctEkfOriginHeight();
}
// Select the height measurement source
//选择高度测量源
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EV)) { //视频定高
hgtMea = -extNavDataDelayed.pos.z;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
} else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
// using range finder data
// correct for tilt using a flat earth model
//rangefinder作为高度源
if (prevTnb.c.z >= 0.7) {
// calculate height above ground
// 计算地表高度
hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
// correct for terrain position relative to datum
hgtMea -= terrainState;
// enable fusion
// 使能融合
fuseHgtData = true;
velPosObs[5] = -hgtMea;
// set the observation noise
// 设置观测噪声
posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
// add uncertainty created by terrain gradient and vehicle tilt
// 添加不确定因素
posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
} else {
// disable fusion if tilted too far
// 若是倾斜的厉害则不进行融合
fuseHgtData = false;
}
} else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) { //gps源
// using GPS data
hgtMea = gpsDataDelayed.hgt;
// enable fusion
velPosObs[5] = -hgtMea;
fuseHgtData = true;
// set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
if (gpsHgtAccuracy > 0.0f) {
posDownObsNoise = sq(constrain_float(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
} else {
posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
}
} else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) { //若是设置为气压模式
// using Baro data
//用气压数据
hgtMea = baroDataDelayed.hgt - baroHgtOffset;
// enable fusion
//使能融合
velPosObs[5] = -hgtMea;
fuseHgtData = true;
// set the observation noise
//观测噪声，观测噪声从哪里输入？通过地面站输入的 AP_GROUPINFO("RNG_M_NSE", 18, NavEKF2, _rngNoise, 0.5f),
posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
if (getTakeoffExpected() || getTouchdownExpected()) {
posDownObsNoise *= frontend->gndEffectBaroScaler;
}
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
if (motorsArmed && getTakeoffExpected()) {
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
}
} else {
fuseHgtData = false;
}
// If we haven't fused height data for a while, then declare the height data as being timed out
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
hgtTimeout = true;
} else {
hgtTimeout = false;
}
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
