dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: optical flow control limits constrain speed using HAGL max

This commit is contained in:
Daniel Agar 2022-06-20 18:10:02 -04:00
parent 15747239c1
commit dc8ed97809
2 changed files with 23 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -838,43 +838,42 @@ hagl_max : Maximum height above ground (meters). NaN when limiting is not needed
*/
void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
{
// Calculate range finder limits
const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
// Allow use of 75% of rangefinder maximum range to allow for angular motion
const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal();
// Calculate optical flow limits
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f);
const float flow_hagl_min = _flow_min_distance;
const float flow_hagl_max = _flow_max_distance;
// TODO : calculate visual odometry limits
const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
// Do not require limiting by default
*vxy_max = NAN;
*vz_max = NAN;
*hagl_min = NAN;
*hagl_max = NAN;
// Calculate range finder limits
const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
// Allow use of 75% of rangefinder maximum range to allow for angular motion
const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal();
// TODO : calculate visual odometry limits
const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
// Keep within range sensor limit when using rangefinder as primary height source
if (relying_on_rangefinder) {
*vxy_max = NAN;
*vz_max = NAN;
*hagl_min = rangefinder_hagl_min;
*hagl_max = rangefinder_hagl_max;
}
// Keep within flow AND range sensor limits when exclusively using optical flow
if (relying_on_optical_flow) {
// Calculate optical flow limits
const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance);
const float flow_hagl_max = fminf(rangefinder_hagl_max, _flow_max_distance);
const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), flow_hagl_min, flow_hagl_max);
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
*vxy_max = flow_vxy_max;
*vz_max = NAN;
*hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min);
*hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max);
*hagl_min = flow_hagl_min;
*hagl_max = flow_hagl_max;
}
}

4
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -303,9 +303,9 @@ protected:
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
// Sensor limitations
float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m)
float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m)
float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m)
// Output Predictor
outputSample _output_new{}; // filter output on the non-delayed time horizon