fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 14:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "EKF: Release flow speed limit with altitude gained" (#382)

Browse Source 
* Revert "EKF: Release flow speed limit with altitude gained"

This reverts commit e70206f74b.

* Revert "fix code style"

This reverts commit 76bf70121c.

* Revert "Reverse the linked list of data_validator_group and maintain a first node"

This reverts commit 32482e7644.
This commit is contained in:
Paul Riseborough
2018-01-24 20:46:48 +11:00
committed by GitHub
parent 76bf70121c
commit f016e66ff8
6 changed files with 18 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
-1
View File
@@ -348,7 +348,6 @@ 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
+6 -11
View File
@@ -1039,30 +1039,25 @@ 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));
}