Merge pull request #1821 from PX4/mcposcontrolmultiplatform1703

port #1703 to multiplatform

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(); }