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: if takeoff setpoint in auto mode, then do not enter ground contact

Browse Source
This commit is contained in:
Dennis Mannhart 2017-03-30 17:01:16 +02:00 committed by Lorenz Meier
parent 08cf97c687
commit 4ff3fb4dee
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -376,6 +376,8 @@ private:
float get_cruising_speed_xy();
bool in_auto_takeoff();
/**
* limit altitude based on several conditions
*/
@ -935,6 +937,17 @@ MulticopterPositionControl::limit_vel_xy_gradually()
_vel_sp(1) = _vel_sp(1) / vel_mag_xy * vel_limit;
}
bool
MulticopterPositionControl::in_auto_takeoff()
{
/*
* in auto mode, check if we do a takeoff
*/
return (_pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) ||
_control_mode.flag_control_offboard_enabled;
}
float
MulticopterPositionControl::get_cruising_speed_xy()
{
@ -1871,7 +1884,7 @@ MulticopterPositionControl::control_position(float dt)
}
/* if still or already on ground command zero xy velcoity and zero xy thrust_sp in body frame to consider uneven ground */
if (_vehicle_land_detected.ground_contact) {
if (_vehicle_land_detected.ground_contact && !in_auto_takeoff()) {
/* thrust setpoint in body frame*/
math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
@ -1920,11 +1933,7 @@ MulticopterPositionControl::control_position(float dt)
// We can only run the control if we're already in-air, have a takeoff setpoint,
// or if we're in offboard control.
// Otherwise, we should just bail out
const bool got_takeoff_setpoint = (_pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) ||
_control_mode.flag_control_offboard_enabled;
if (_vehicle_land_detected.landed && !got_takeoff_setpoint) {
if (_vehicle_land_detected.landed && !in_auto_takeoff()) {
// Keep throttle low while still on ground.
thr_max = 0.0f;