Merged master into vision_estimate

1 files changed, 66 insertions, 85 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 77226cd20..a0a953f72 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -102,6 +102,10 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_cmd_pub(-1),
 	_flow_pub(-1),
 	_offboard_control_sp_pub(-1),
+	_local_pos_sp_pub(-1),
+	_global_vel_sp_pub(-1),
+	_att_sp_pub(-1),
+	_rates_sp_pub(-1),
 	_vicon_position_pub(-1),
 	_vision_position_pub(-1),
 	_telemetry_status_pub(-1),
@@ -109,12 +113,15 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_manual_pub(-1),
 	_telemetry_heartbeat_time(0),
 	_radio_status_available(false),
+	_control_mode_sub(-1),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0)
 {
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
+	memset(&_control_mode, 0, sizeof(_control_mode));
 
 	// make sure the FTP server is started
 	(void)MavlinkFTP::getServer();
@@ -403,53 +410,21 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
 {
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
+	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
+	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
 
-	if (mavlink_system.sysid < 4) {
+	/* Only accept system IDs from 1 to 4 */
+	if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
 		struct offboard_control_setpoint_s offboard_control_sp;
 		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
 
-		uint8_t ml_mode = 0;
-		bool ml_armed = false;
+		/* 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;
 
-		switch (quad_motors_setpoint.mode) {
-		case 0:
-			ml_armed = false;
-			break;
-
-		case 1:
-			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-			ml_armed = true;
-
-			break;
-
-		case 2:
-			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-			ml_armed = true;
-
-			break;
-
-		case 3:
-			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-			break;
-
-		case 4:
-			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-			break;
-		}
-
-		offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-		offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-		offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-		offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
-
-		if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
-			ml_armed = false;
-		}
-
-		offboard_control_sp.armed = ml_armed;
-		offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
+		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
@@ -465,32 +440,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
-	mavlink_radio_status_t rstatus;
-	mavlink_msg_radio_status_decode(msg, &rstatus);
-
-	struct telemetry_status_s tstatus;
-	memset(&tstatus, 0, sizeof(tstatus));
-
-	tstatus.timestamp = hrt_absolute_time();
-	tstatus.heartbeat_time = _telemetry_heartbeat_time;
-	tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
-	tstatus.rssi = rstatus.rssi;
-	tstatus.remote_rssi = rstatus.remrssi;
-	tstatus.txbuf = rstatus.txbuf;
-	tstatus.noise = rstatus.noise;
-	tstatus.remote_noise = rstatus.remnoise;
-	tstatus.rxerrors = rstatus.rxerrors;
-	tstatus.fixed = rstatus.fixed;
-
-	if (_telemetry_status_pub < 0) {
-		_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
+	if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
+		mavlink_radio_status_t rstatus;
+		mavlink_msg_radio_status_decode(msg, &rstatus);
+
+		struct telemetry_status_s tstatus;
+		memset(&tstatus, 0, sizeof(tstatus));
+
+		tstatus.timestamp = hrt_absolute_time();
+		tstatus.heartbeat_time = _telemetry_heartbeat_time;
+		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
+		tstatus.rssi = rstatus.rssi;
+		tstatus.remote_rssi = rstatus.remrssi;
+		tstatus.txbuf = rstatus.txbuf;
+		tstatus.noise = rstatus.noise;
+		tstatus.remote_noise = rstatus.remnoise;
+		tstatus.rxerrors = rstatus.rxerrors;
+		tstatus.fixed = rstatus.fixed;
+
+		if (_telemetry_status_pub < 0) {
+			_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-	} else {
-		orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
-	}
+		} else {
+			orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
+		}
 
-	/* this means that heartbeats alone won't be published to the radio status no more */
-	_radio_status_available = true;
+		/* this means that heartbeats alone won't be published to the radio status no more */
+		_radio_status_available = true;
+	}
 }
 
 void
@@ -508,6 +486,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	manual.r = man.r / 1000.0f;
 	manual.z = man.z / 1000.0f;
 
+	warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
+
 	if (_manual_pub < 0) {
 		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
 
@@ -519,28 +499,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 {
-	mavlink_heartbeat_t hb;
-	mavlink_msg_heartbeat_decode(msg, &hb);
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
+	if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
+		mavlink_heartbeat_t hb;
+		mavlink_msg_heartbeat_decode(msg, &hb);
 
-	/* ignore own heartbeats, accept only heartbeats from GCS */
-	if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
-		_telemetry_heartbeat_time = hrt_absolute_time();
+		/* ignore own heartbeats, accept only heartbeats from GCS */
+		if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+			_telemetry_heartbeat_time = hrt_absolute_time();
 
-		/* if no radio status messages arrive, lets at least publish that heartbeats were received */
-		if (!_radio_status_available) {
+			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+			if (!_radio_status_available) {
 
-			struct telemetry_status_s tstatus;
-			memset(&tstatus, 0, sizeof(tstatus));
+				struct telemetry_status_s tstatus;
+				memset(&tstatus, 0, sizeof(tstatus));
 
-			tstatus.timestamp = _telemetry_heartbeat_time;
-			tstatus.heartbeat_time = _telemetry_heartbeat_time;
-			tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+				tstatus.timestamp = _telemetry_heartbeat_time;
+				tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
-			if (_telemetry_status_pub < 0) {
-				_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+				if (_telemetry_status_pub < 0) {
+					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-			} else {
-				orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+				} else {
+					orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
+				}
 			}
 		}
 	}
@@ -785,7 +768,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 
 	hil_gps.timestamp_variance = timestamp;
 	hil_gps.s_variance_m_s = 5.0f;
-	hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
 
 	hil_gps.timestamp_velocity = timestamp;
 	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
@@ -795,9 +777,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.vel_ned_valid = true;
 	hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
 
-	hil_gps.timestamp_satellites = timestamp;
 	hil_gps.fix_type = gps.fix_type;
-	hil_gps.satellites_visible = gps.satellites_visible;
+	hil_gps.satellites_used = gps.satellites_visible;  //TODO: rename mavlink_hil_gps_t sats visible to used?
 
 	if (_gps_pub < 0) {
 		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);