support force setpoints

5 files changed, 39 insertions, 4 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index cc68c4e36..fc9560e18 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1951,6 +1951,15 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_force_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
 		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
 		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
 		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index e5d380cfb..2cc2d6162 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -105,6 +105,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
+	_force_sp_pub(-1),
 	_vicon_position_pub(-1),
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
@@ -444,6 +445,13 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 		offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy;
 		offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz;
 		offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9));
+
+		/* If we are in force control mode, for now set offboard mode to force control */
+		if (offboard_control_sp.isForceSetpoint) {
+			offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
+		}
+
+		/* set ignore flags */
 		for (int i = 0; i < 9; i++) {
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (local_ned_position_setpoint_external.type_mask & (1 << i));
@@ -467,9 +475,23 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 			if (updated) {
 				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 			}
-
 			if (_control_mode.flag_control_offboard_enabled) {
+				if (offboard_control_sp.isForceSetpoint) {
+					struct vehicle_force_setpoint_s	force_sp;
+					force_sp.x = offboard_control_sp.acceleration[0];
+					force_sp.y = offboard_control_sp.acceleration[1];
+					force_sp.z = offboard_control_sp.acceleration[2];
+					//XXX: yaw
+					if (_force_sp_pub < 0) {
+						_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
+					}
+				} else {
+
 				//XXX: copy to and publish setpoint triplet here
+				}
+
 			}
 
 		}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 0f12cf32c..0044b42cb 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -70,6 +70,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_force_setpoint.h>
 
 #include "mavlink_ftp.h"
 
@@ -146,6 +147,7 @@ private:
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;
+	orb_advert_t _force_sp_pub;
 	orb_advert_t _vicon_position_pub;
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 809a2ace4..19f11ba92 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -58,9 +58,10 @@ enum OFFBOARD_CONTROL_MODE {
 	OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
 	OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
 	OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+	OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
+	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
 };
 
 enum OFFBOARD_CONTROL_FRAME {
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 49e2ba4b5..ca7705456 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -77,6 +77,7 @@ struct vehicle_control_mode_s {
 	bool flag_control_offboard_enabled;		/**< true if offboard control should be used */
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
+	bool flag_control_force_enabled;		/**< true if force control is mixed in */
 	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */