fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 22:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adds NaN checks and setpoint resets for offboard posctl

Browse Source
This commit is contained in:
t0ni0
2014-06-18 16:45:38 -04:00
parent e078ef992f
commit 9d18da4433
1 changed files with 20 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+20 -11
View File
@@ -675,21 +675,30 @@ MulticopterPositionControl::task_main()
orb_check(_local_pos_sp_sub, &updated); orb_check(_local_pos_sp_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp);
&_local_pos_sp);
} }
/* Make sure position control is selected i.e. not only velocity control */ if (isfinite(_local_pos_sp.x) && isfinite(_local_pos_sp.y) && isfinite(_local_pos_sp.z) && isfinite(_local_pos_sp.yaw)) {
if (_control_mode.flag_control_position_enabled) { /* If manual control overides offboard, cancel the offboard setpoint and stay at current position */
_pos_sp(0) = _local_pos_sp.x; _reset_pos_sp = true;
_pos_sp(1) = _local_pos_sp.y; _reset_alt_sp = true;
}
if (_control_mode.flag_control_altitude_enabled) { /* Make sure position control is selected i.e. not only velocity control */
_pos_sp(2) = _local_pos_sp.z; if (_control_mode.flag_control_position_enabled) {
} _pos_sp(0) = _local_pos_sp.x;
_pos_sp(1) = _local_pos_sp.y;
}
_att_sp.yaw_body = _local_pos_sp.yaw; if (_control_mode.flag_control_altitude_enabled) {
_pos_sp(2) = _local_pos_sp.z;
}
_att_sp.yaw_body = _local_pos_sp.yaw;
} else {
reset_pos_sp();
reset_alt_sp();
}
} else { } else {
/* AUTO */ /* AUTO */