yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: add OFFBOARD takeoff intent

Browse Source
This commit is contained in:
Daniel Agar
2021-03-06 23:44:30 -05:00
committed by Matthias Grob
parent 22abe1619b
commit 35488337d3
1 changed files with 22 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@@ -331,7 +331,28 @@ void MulticopterPositionControl::Run()
} }
if (_control_mode.flag_control_offboard_enabled) { if (_control_mode.flag_control_offboard_enabled) {
_vehicle_constraints.want_takeoff = _control_mode.flag_armed && hrt_elapsed_time(&_setpoint.timestamp) < 1_s;
bool want_takeoff = _control_mode.flag_armed && _vehicle_land_detected.landed
&& hrt_elapsed_time(&_setpoint.timestamp) < 1_s;
if (want_takeoff && PX4_ISFINITE(_setpoint.z)
&& (_setpoint.z < states.position(2))) {
_vehicle_constraints.want_takeoff = true;
} else if (want_takeoff && PX4_ISFINITE(_setpoint.vz)
&& (_setpoint.vz < 0.f)) {
_vehicle_constraints.want_takeoff = true;
} else if (want_takeoff && PX4_ISFINITE(_setpoint.acceleration[2])
&& (_setpoint.acceleration[2] < 0.f)) {
_vehicle_constraints.want_takeoff = true;
} else {
_vehicle_constraints.want_takeoff = false;
}
// override with defaults // override with defaults
_vehicle_constraints.speed_xy = _param_mpc_xy_vel_max.get(); _vehicle_constraints.speed_xy = _param_mpc_xy_vel_max.get();