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

mc_pos_control: refactor, move landed thrust reduction into function

Browse Source 
and make it use matrix library for flight task compatibility

# Conflicts:
#	src/modules/mc_pos_control/mc_pos_control_main.cpp
This commit is contained in:
Matthias Grob 2018-02-27 11:15:59 +01:00 committed by Beat Küng
parent fabf214bca
commit 5ee136fe10
1 changed files with 35 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -394,6 +394,8 @@ private:
void set_takeoff_velocity(float &vel_sp_z);
void landdetection_thrust_limit(matrix::Vector3f &thrust_sp);
/**
* Shim for calling task_main from task_create.
*/
@ -2587,32 +2589,8 @@ MulticopterPositionControl::calculate_thrust_setpoint()
thrust_sp(1) = 0.0f;
}
if (!in_auto_takeoff() && !manual_wants_takeoff()) {
if (_vehicle_land_detected.ground_contact) {
/* if still or already on ground command zero xy thrust_sp in body
* frame to consider uneven ground */
/* thrust setpoint in body frame*/
matrix::Vector3f thrust_sp_body = _R.transpose() * thrust_sp;
/* we dont want to make any correction in body x and y*/
thrust_sp_body(0) = 0.0f;
thrust_sp_body(1) = 0.0f;
/* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */
thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
/* convert back to local frame (NED) */
thrust_sp = _R * thrust_sp_body;
}
if (_vehicle_land_detected.maybe_landed) {
/* we set thrust to zero
* this will help to decide if we are actually landed or not
*/
thrust_sp.zero();
}
}
// reduce thrust in early landing detection states to check if the vehicle still moves
landdetection_thrust_limit(thrust_sp);
if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) {
thrust_sp(2) = 0.0f;
@ -3246,6 +3224,37 @@ MulticopterPositionControl::set_takeoff_velocity(float &vel_sp_z)
vel_sp_z = math::max(vel_sp_z, -_takeoff_vel_limit);
}
void
MulticopterPositionControl::landdetection_thrust_limit(matrix::Vector3f &thrust_sp)
{
if (!in_auto_takeoff() && !manual_wants_takeoff()) {
if (_vehicle_land_detected.ground_contact) {
/* if still or already on ground command zero xy thrust_sp in body
* frame to consider uneven ground */
/* thrust setpoint in body frame*/
matrix::Vector3f thrust_sp_body = _R.transpose() * thrust_sp;
/* we dont want to make any correction in body x and y*/
thrust_sp_body(0) = 0.0f;
thrust_sp_body(1) = 0.0f;
/* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */
thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
/* convert back to local frame (NED) */
thrust_sp = _R * thrust_sp_body;
}
if (_vehicle_land_detected.maybe_landed) {
/* we set thrust to zero
* this will help to decide if we are actually landed or not
*/
thrust_sp.zero();
}
}
}
int
MulticopterPositionControl::start()
{