diff options
| author | t0ni0 <azntonio789@gmail.com> | 2014-06-18 16:45:38 -0400 |
|---|---|---|
| committer | t0ni0 <azntonio789@gmail.com> | 2014-06-18 16:45:38 -0400 |
| commit | 9d18da4433773cfa02bec1d33fdb34e4d03d1442 (patch) | |
| tree | 12ad5dcbb9b86590e0fb5930ac9a0c1e0c3a3146 /src/modules/mc_pos_control | |
| parent | e078ef992fc21d86cbd09db89c25332273840b22 (diff) | |
| download | px4-firmware-9d18da4433773cfa02bec1d33fdb34e4d03d1442.tar.gz px4-firmware-9d18da4433773cfa02bec1d33fdb34e4d03d1442.tar.bz2 px4-firmware-9d18da4433773cfa02bec1d33fdb34e4d03d1442.zip | |
Adds NaN checks and setpoint resets for offboard posctl
Diffstat (limited to 'src/modules/mc_pos_control')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 31 |
1 files changed, 20 insertions, 11 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 41fb20108..a4c871d95 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -675,21 +675,30 @@ MulticopterPositionControl::task_main() orb_check(_local_pos_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, - &_local_pos_sp); + orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); } - /* Make sure position control is selected i.e. not only velocity control */ - if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _local_pos_sp.x; - _pos_sp(1) = _local_pos_sp.y; - } + if (isfinite(_local_pos_sp.x) && isfinite(_local_pos_sp.y) && isfinite(_local_pos_sp.z) && isfinite(_local_pos_sp.yaw)) { + /* If manual control overides offboard, cancel the offboard setpoint and stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; - if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _local_pos_sp.z; - } + /* Make sure position control is selected i.e. not only velocity control */ + 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 { /* AUTO */ |