diff options
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 12 | ||||
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.h | 1 |
2 files changed, 12 insertions, 1 deletions
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 391558dcc..e6a7ee8a6 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -106,7 +106,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0); _armed = _n.subscribe<px4_actuator_armed>(0); _local_pos = _n.subscribe<px4_vehicle_local_position>(0); - _pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(0); + _pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(&MulticopterPositionControl::handle_position_setpoint_triplet, this, 0); _local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0); _global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0); @@ -554,6 +554,16 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd parameters_update(); } +void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg) +{ + /* Make sure that the position setpoint is valid */ + if (!isfinite(_pos_sp_triplet->data().current.lat) || + !isfinite(_pos_sp_triplet->data().current.lon) || + !isfinite(_pos_sp_triplet->data().current.alt)) { + _pos_sp_triplet->data().current.valid = false; + } +} + void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { static bool reset_int_z = true; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 245b5f5a9..b754ccf01 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -75,6 +75,7 @@ public: /* Callbacks for topics */ void handle_vehicle_attitude(const px4_vehicle_attitude &msg); void handle_parameter_update(const px4_parameter_update &msg); + void handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg); void spin() { _n.spin(); } |