offboard setpoints: correctly check for force setpoint

2 files changed, 35 insertions, 1 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index dfc5ddc91..418b81ee1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -441,7 +441,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 			}
 			if (_control_mode.flag_control_offboard_enabled) {
-				if (offboard_control_sp.isForceSetpoint) {
+				if (offboard_control_sp.isForceSetpoint &&
+						offboard_control_sp_ignore_position_all(offboard_control_sp) &&
+						offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+					/* The offboard setpoint is a force setpoint only, directly writing to the force
+					 * setpoint topic and not publishing the setpoint triplet topic */
 					struct vehicle_force_setpoint_s	force_sp;
 					force_sp.x = offboard_control_sp.acceleration[0];
 					force_sp.y = offboard_control_sp.acceleration[1];
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 19f11ba92..4f45ac14f 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -104,14 +104,44 @@ struct offboard_control_setpoint_s {
  * @}
  */
 
+/**
+ * Returns true if the position setpoint at index should be ignored
+ */
 inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
 	return (bool)(offboard_control_sp.ignore & (1 << index));
 }
 
+/**
+ * Returns true if all position setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_position(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * Returns true if the velocity setpoint at index should be ignored
+ */
 inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
 	return (bool)(offboard_control_sp.ignore & (1 << (3 + index)));
 }
 
+/**
+ * Returns true if all velocity setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
 inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
 	return (bool)(offboard_control_sp.ignore & (1 << (6 + index)));
 }