Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

mc_pos_control: for terrain following use range sensor climbrate for velocity estimate

Browse Source
This commit is contained in:
Dennis Mannhart
2018-06-25 10:15:35 +02:00
committed by Lorenz Meier
parent eb7139bc56
commit bf4ac7a9d6
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+8 -2
View File
@@ -130,7 +130,8 @@ private:
(ParamFloat<px4::params::MPC_LAND_SPEED>) _land_speed,
(ParamFloat<px4::params::MPC_TKO_SPEED>) _tko_speed,
(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed
(ParamInt<px4::params::MPC_POS_MODE>) MPC_POS_MODE
(ParamInt<px4::params::MPC_POS_MODE>) MPC_POS_MODE,
(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE
);
control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -432,7 +433,12 @@ MulticopterPositionControl::check_vehicle_states(const float &vel_sp_z)
_vel_y_deriv.update(0.0f);
}
if (PX4_ISFINITE(_local_pos.vz)) {
if (MPC_ALT_MODE.get() && _local_pos.dist_bottom_valid && PX4_ISFINITE(_local_pos.dist_bottom_rate)) {
// terrain following
_states.velocity(2) = -_local_pos.dist_bottom_rate;
_states.acceleration(2) = _vel_z_deriv.update(-_states.velocity(2));
} else if (PX4_ISFINITE(_local_pos.vz)) {
if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {
// A change in velocity is demanded. Set velocity to the derivative of position