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: copy triplet in one place only

Browse Source
This commit is contained in:
Julian Oes
2016-11-15 08:01:55 +01:00
committed by Lorenz Meier
parent 432824d603
commit 61fff54cbe
1 changed files with 14 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -659,6 +659,7 @@ MulticopterPositionControl::parameters_update(bool force)
void
MulticopterPositionControl::poll_subscriptions()
{
bool updated;
orb_check(_vehicle_status_sub, &updated);
@@ -771,6 +772,19 @@ MulticopterPositionControl::poll_subscriptions()
_vz_reset_counter = _local_pos.vz_reset_counter;
_vxy_reset_counter = _local_pos.vxy_reset_counter;
}
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
//Make sure that the position setpoint is valid
if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false;
}
}
}
float
@@ -1003,12 +1017,6 @@ MulticopterPositionControl::control_manual(float dt)
void
MulticopterPositionControl::control_offboard(float dt)
{
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
@@ -1121,21 +1129,6 @@ void MulticopterPositionControl::control_auto(float dt)
reset_pos_sp();
reset_alt_sp();
//Poll position setpoint
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
//Make sure that the position setpoint is valid
if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false;
}
}
bool current_setpoint_valid = false;
bool previous_setpoint_valid = false;