WIP, change uorb offboard control sp topic

3 files changed, 55 insertions, 39 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b72bfa858..e1a74f599 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -379,10 +379,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
 
 		/* Convert values * 1000 back */
-		offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
+		//XXX: convert to quaternion
+		//offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
+		//offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
+		//offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
+		//offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
 
 		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
 
@@ -427,18 +428,19 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 				/* invalid setpoint, avoid publishing */
 				return;
 		}
-		offboard_control_sp.p1 = local_ned_position_setpoint_external.x;
-		offboard_control_sp.p2 = local_ned_position_setpoint_external.y;
-		offboard_control_sp.p3 = local_ned_position_setpoint_external.z;
-		offboard_control_sp.p4 = local_ned_position_setpoint_external.vx;
-		offboard_control_sp.p5 = local_ned_position_setpoint_external.vy;
-		offboard_control_sp.p6 = local_ned_position_setpoint_external.vz;
-		offboard_control_sp.p7 = local_ned_position_setpoint_external.afx;
-		offboard_control_sp.p8 = local_ned_position_setpoint_external.afy;
-		offboard_control_sp.p9 = local_ned_position_setpoint_external.afz;
+		offboard_control_sp.position[0] = local_ned_position_setpoint_external.x;
+		offboard_control_sp.position[1] = local_ned_position_setpoint_external.y;
+		offboard_control_sp.position[2] = local_ned_position_setpoint_external.z;
+		offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx;
+		offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy;
+		offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz;
+		offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx;
+		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));
 		for (int i = 0; i < 9; i++) {
-			offboard_control_sp.ignore[i] =  (bool)(local_ned_position_setpoint_external.type_mask & (1 << i));
+			offboard_control_sp.ignore &=  ~(local_ned_position_setpoint_external.type_mask & (1 << i));
+			offboard_control_sp.ignore |=  (local_ned_position_setpoint_external.type_mask & (1 << i));
 		}
 
 
diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp
index 27ce46a1d..71a14c029 100644
--- a/src/modules/navigator/offboard.cpp
+++ b/src/modules/navigator/offboard.cpp
@@ -83,14 +83,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		/* 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])) {
+				(!offboard_control_sp_ignore_position(_offboard_control_sp, 0) &&
+				 !offboard_control_sp_ignore_position(_offboard_control_sp, 1) &&
+				 !offboard_control_sp_ignore_position(_offboard_control_sp, 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.x = _offboard_control_sp.position[0];
+			pos_sp_triplet->current.y = _offboard_control_sp.position[1];
+			//pos_sp_triplet->current.yaw = _offboard_control_sp.position[2];
+			//XXX: copy yaw
+			pos_sp_triplet->current.z = -_offboard_control_sp.position[2];
 
 			pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
 			pos_sp_triplet->current.valid = true;
@@ -102,14 +103,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		/* 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])) {
+				(!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) &&
+				 !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) &&
+				 !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) {
 			/* 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.vx = _offboard_control_sp.velocity[0];
+			pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1];
+//			pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[;
+//			//XXX: copy yaw speed
+			pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2];
 
 			pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
 			pos_sp_triplet->current.valid = true;
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 2741f0c07..688135533 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -81,19 +81,15 @@ struct offboard_control_setpoint_s {
 
 	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
 
-	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 */
+	double position[3];	/**< lat, lon, alt / x, y, z */
+	float velocity[3];	/**< x vel, y vel, z vel */
+	float acceleration[3];	/**< x acc, y acc, z acc */
+	float attitude[4];	/**< attitude of vehicle (quaternion) */
+	float attitude_rate[3];	/**< body angular rates (x, y, z) */
 
 	//XXX: use a bitmask with wrapper functions instead
-	bool ignore[9]; /**< if field i is set to true, pi should be ignored */
-	bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */
+	uint16_t ignore; /**< if field i is set to true, pi should be ignored */
+	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
 
 	float override_mode_switch;
 
@@ -107,6 +103,22 @@ struct offboard_control_setpoint_s {
  * @}
  */
 
+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));
+}
+
+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)));
+}
+
+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)));
+}
+
+inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << (9 + index)));
+}
+
 /* register this as object request broker structure */
 ORB_DECLARE(offboard_control_setpoint);