fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 15:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Release flow speed limit with altitude gained

Browse Source 
When GPS use is gained whilst flying using optical flow data, the sudden release of the speed limit is unannounced to  the operator and can cause unexpected acceleration.
This patch releases the speed limit as height is gained, but does not reduce it when the vehicle descends, unless GPS use is lost.
This commit is contained in:
Paul Riseborough
2017-12-12 08:39:26 +11:00
committed by Lorenz Meier
parent dea0c8bb6a
commit e70206f74b
2 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+1
View File
@@ -348,6 +348,7 @@ private:
Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
float _flow_gnd_spd_max{0.0f}; ///< maximum ground speed that the flow sensor can reliably measure (m/s)
float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad)
EKF/ekf_helper.cpp
+11 -6
View File
@@ -1037,25 +1037,30 @@ limit_hagl : Boolean true when height above ground needs to be controlled to rem
*/
void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl)
{
float flow_gnd_spd_max;
bool flow_limit_hagl;
// If relying on optical flow for navigation we need to keep within flow and range sensor limits
bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos);
if (relying_on_optical_flow) {
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2));
flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f);
_flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2));
_flow_gnd_spd_max = fmaxf(_flow_gnd_spd_max , 0.0f);
flow_limit_hagl = true;
} else if (_control_status.flags.opt_flow) {
// We are using optical flow but not reliant on it
// Release the speed limit as the vehicle climbs, but do not reduce it when it descends
float lower_limit = fmaxf(0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)) , 0.0f);
_flow_gnd_spd_max = fmaxf(_flow_gnd_spd_max , lower_limit);
flow_limit_hagl = false;
} else {
flow_gnd_spd_max = NAN;
_flow_gnd_spd_max = NAN;
flow_limit_hagl = false;
}
memcpy(vxy_max, &flow_gnd_spd_max, sizeof(float));
memcpy(vxy_max, &_flow_gnd_spd_max, sizeof(float));
memcpy(limit_hagl, &flow_limit_hagl, sizeof(bool));
}