Update offboard control uorb topic

Work towards supporting the new external setpoint mavlink messages

5 files changed, 102 insertions, 46 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 38297745d..7185eebc4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -946,7 +946,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* Perform system checks (again) once params are loaded and MAVLink is up. */
-		if (!system_checked && mavlink_fd && 
+		if (!system_checked && mavlink_fd &&
 			(telemetry.heartbeat_time > 0) &&
 			(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
 
@@ -1847,21 +1847,17 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
-		case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true; /* XXX: hack for now */
-			control_mode.flag_control_velocity_enabled = true;
-			break;
-		case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
 			control_mode.flag_control_rates_enabled = true;
 			control_mode.flag_control_attitude_enabled = true;
 			control_mode.flag_control_altitude_enabled = true;
 			control_mode.flag_control_climb_rate_enabled = true;
 			control_mode.flag_control_position_enabled = true;
 			control_mode.flag_control_velocity_enabled = true;
+			//XXX: the flags could depend on sp_offboard.ignore
 			break;
 		default:
 			control_mode.flag_control_rates_enabled = false;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3747ad3ba..73222637f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -154,6 +154,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
 		break;
 
+	case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL:
+		handle_message_local_ned_position_setpoint_external(msg);
+		break;
+
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -394,6 +398,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 }
 
 void
+MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg)
+{
+	mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external;
+	mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external);
+
+	struct offboard_control_setpoint_s offboard_control_sp;
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
+
+	//XXX check if target system/component is correct
+
+	/* convert mavlink type (local, NED) to uORB offboard control struct */
+	//XXX do the conversion
+	//
+	offboard_control_sp.timestamp = hrt_absolute_time();
+
+	if (_offboard_control_sp_pub < 0) {
+		_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+	} else {
+		orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+	}
+}
+
+void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
 	mavlink_radio_status_t rstatus;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 65ef884a2..658f51562 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -115,6 +115,7 @@ private:
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
+	void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp
index ef7d11a03..27ce46a1d 100644
--- a/src/modules/navigator/offboard.cpp
+++ b/src/modules/navigator/offboard.cpp
@@ -79,33 +79,46 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	bool changed = false;
 
 	/* copy offboard setpoints to the corresponding topics */
-	if (_navigator->get_control_mode()->flag_control_position_enabled
-	    && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
-		/* position control */
-		pos_sp_triplet->current.x = _offboard_control_sp.p1;
-		pos_sp_triplet->current.y = _offboard_control_sp.p2;
-		pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
-		pos_sp_triplet->current.z = -_offboard_control_sp.p4;
-
-		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->current.position_valid = true;
-
-		changed = true;
-
-	} else if (_navigator->get_control_mode()->flag_control_velocity_enabled
-	           && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) {
-		/* velocity control */
-		pos_sp_triplet->current.vx = _offboard_control_sp.p2;
-		pos_sp_triplet->current.vy = _offboard_control_sp.p1;
-		pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
-		pos_sp_triplet->current.vz = _offboard_control_sp.p4;
-
-		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->current.velocity_valid = true;
-
-		changed = true;
+	if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) {
+		/* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not
+		 * support deactivation of individual directions) */
+		if (_navigator->get_control_mode()->flag_control_position_enabled &&
+				(!_offboard_control_sp.ignore[0] &&
+				 !_offboard_control_sp.ignore[1] &&
+				 !_offboard_control_sp.ignore[2])) {
+			/* position control */
+			pos_sp_triplet->current.x = _offboard_control_sp.p1;
+			pos_sp_triplet->current.y = _offboard_control_sp.p2;
+			pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
+			pos_sp_triplet->current.z = -_offboard_control_sp.p4;
+
+			pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+			pos_sp_triplet->current.valid = true;
+			pos_sp_triplet->current.position_valid = true;
+
+			changed = true;
+
+		}
+		/* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not
+		 * support deactivation of individual directions) */
+		if (_navigator->get_control_mode()->flag_control_velocity_enabled &&
+				(!_offboard_control_sp.ignore[3] &&
+				 !_offboard_control_sp.ignore[4] &&
+				 !_offboard_control_sp.ignore[5])) {
+			/* velocity control */
+			pos_sp_triplet->current.vx = _offboard_control_sp.p2;
+			pos_sp_triplet->current.vy = _offboard_control_sp.p1;
+			pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
+			pos_sp_triplet->current.vz = _offboard_control_sp.p4;
+
+			pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+			pos_sp_triplet->current.valid = true;
+			pos_sp_triplet->current.velocity_valid = true;
+
+			changed = true;
+		}
+
+		//XXX: map acceleration setpoint once supported in setpoint triplet
 	}
 
 	return changed;
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index d7b131e3c..427ccc6c8 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -53,11 +53,22 @@ enum OFFBOARD_CONTROL_MODE {
 	OFFBOARD_CONTROL_MODE_DIRECT = 0,
 	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
 	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
-	OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3,
-	OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3,
+	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4,
+	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 */
+};
+
+enum OFFBOARD_CONTROL_FRAME {
+	OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0,
+	OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1,
+	OFFBOARD_CONTROL_FRAME_BODY_NED = 2,
+	OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3,
+	OFFBOARD_CONTROL_FRAME_GLOBAL = 4
 };
 
 /**
@@ -70,10 +81,17 @@ struct offboard_control_setpoint_s {
 
 	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
 
-	float p1;	/**< ailerons roll / roll rate input */
-	float p2;	/**< elevator / pitch / pitch rate */
-	float p3;	/**< rudder / yaw rate / yaw */
-	float p4;	/**< throttle / collective thrust / altitude */
+	double p1;	/**< ailerons / roll / x pos / lat */
+	double p2;	/**< elevator / pitch / y pos / lon */
+	float p3;	/**< rudder / yaw / z pos / alt */
+	float p4;	/**< throttle / x vel */
+	float p5;       /**< roll rate / y vel */
+	float p6;       /**< pitch rate / z vel */
+	float p7;	/**< yaw rate / x acc */
+	float p8;	/**< y acc */
+	float p9;	/**< z acc */
+
+	bool ignore[9]; /**< if field i is set to true, pi should be ignored */
 
 	float override_mode_switch;