mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: use rangefinder backend accessors
This commit is contained in:
parent
1e83ef3c44
commit
63440800fc
|
@ -6,6 +6,7 @@
|
|||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_RangeFinder/RangeFinder_Backend.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -35,13 +36,17 @@ void NavEKF3_core::readRangeFinder(void)
|
|||
// use data from two range finders if available
|
||||
|
||||
for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) {
|
||||
if ((frontend->_rng.get_orientation(sensorIndex) == ROTATION_PITCH_270) && (frontend->_rng.status(sensorIndex) == RangeFinder::RangeFinder_Good)) {
|
||||
AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(sensorIndex);
|
||||
if (sensor == nullptr) {
|
||||
continue;
|
||||
}
|
||||
if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == RangeFinder::RangeFinder_Good)) {
|
||||
rngMeasIndex[sensorIndex] ++;
|
||||
if (rngMeasIndex[sensorIndex] > 2) {
|
||||
rngMeasIndex[sensorIndex] = 0;
|
||||
}
|
||||
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
|
||||
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = frontend->_rng.distance_cm(sensorIndex) * 0.01f;
|
||||
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f;
|
||||
}
|
||||
|
||||
// check for three fresh samples
|
||||
|
|
|
@ -7,6 +7,7 @@
|
|||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_RangeFinder/RangeFinder_Backend.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -753,10 +754,13 @@ void NavEKF3_core::selectHeightForFusion()
|
|||
// the corrected reading is the reading that would have been taken if the sensor was
|
||||
// co-located with the IMU
|
||||
if (rangeDataToFuse) {
|
||||
Vector3f posOffsetBody = frontend->_rng.get_pos_offset(rangeDataDelayed.sensor_idx) - accelPosOffset;
|
||||
if (!posOffsetBody.is_zero()) {
|
||||
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
||||
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue