Mavlink processor changes

6 files changed, 210 insertions, 16 deletions

diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 0cd08769e..4fa2fd3ee 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
-#include <v1.0/common/mavlink.h>
+#include <mavlink/custom/headers/airdog/mavlink.h>
 
 __END_DECLS
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 0d932d20a..6ec04b0b8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -387,7 +387,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	Mavlink *inst;
 	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
-
+			// TODO! Consider if this code should run on targeter.
 			/* if not in normal mode, we are an onboard link
 			 * onboard links should only pass on messages from the same system ID */
 			if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
@@ -1241,6 +1241,9 @@ Mavlink::task_main(int argc, char *argv[])
 			} else if (strcmp(optarg, "onboard") == 0) {
 				_mode = MAVLINK_MODE_ONBOARD;
 			}
+			else if (strcmp(optarg, "targeter") == 0) {
+				_mode = MAVLINK_MODE_TARGETER;
+			}
 
 			break;
 
@@ -1302,6 +1305,9 @@ Mavlink::task_main(int argc, char *argv[])
 	case MAVLINK_MODE_ONBOARD:
 		warnx("mode: ONBOARD");
 		break;
+	case MAVLINK_MODE_TARGETER:
+		warnx("mode: TARGETER");
+		break;
 
 	default:
 		warnx("ERROR: Unknown mode");
@@ -1370,6 +1376,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* HEARTBEAT is constant rate stream, rate never adjusted */
 	configure_stream("HEARTBEAT", 1.0f);
 
+	// TODO! Consider if any streams except Heartbeat are required on Target
 	/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
 	configure_stream("STATUSTEXT", 20.0f);
 
@@ -1390,6 +1397,7 @@ Mavlink::task_main(int argc, char *argv[])
 	LL_APPEND(_streams, _mission_manager);
 
 	switch (_mode) {
+	// TODO! Consider limiting copter's topics
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
@@ -1416,6 +1424,14 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("VFR_HUD", 10.0f);
 		break;
 
+	// Targeter requires different streams
+	case MAVLINK_MODE_TARGETER:
+		// TODO! Check which rates fit best
+		// TODO! Change to custom message, when it is available
+		configure_stream("GLOBAL_POSITION_INT", 10.0f);
+		configure_stream("TRAJECTORY", 10.0f);
+		break;
+
 	default:
 		break;
 	}
@@ -1669,8 +1685,6 @@ Mavlink::display_status()
 		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
 	}
 
-	printf("\tmavlink chan: #%u\n", _channel);
-
 	if (_rstatus.timestamp > 0) {
 
 		printf("\ttype:\t\t");
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1f96e586b..4a080af58 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -127,7 +127,8 @@ public:
 	enum MAVLINK_MODE {
 		MAVLINK_MODE_NORMAL = 0,
 		MAVLINK_MODE_CUSTOM,
-		MAVLINK_MODE_ONBOARD
+		MAVLINK_MODE_ONBOARD,
+		MAVLINK_MODE_TARGETER // Special mode for airleash
 	};
 
 	void			set_mode(enum MAVLINK_MODE);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 90090653b..dacb401f1 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/trajectory.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/offboard_control_setpoint.h>
@@ -169,8 +170,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			break;
 
 		case NAVIGATION_STATE_AUTO_RTL:
-			/* fallthrough */
-		case NAVIGATION_STATE_AUTO_RCRECOVER:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -179,8 +178,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			break;
 
 		case NAVIGATION_STATE_LAND:
-		case NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 			/* fallthrough */
 		case NAVIGATION_STATE_DESCEND:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
@@ -198,6 +195,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
 			break;
 
+		case NAVIGATION_STATE_AUTO_ABS_FOLLOW:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_ABS_FOLLOW;
+			break;
+
 		case NAVIGATION_STATE_TERMINATION:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
@@ -961,11 +966,13 @@ protected:
 			msg.lat = pos.lat * 1e7;
 			msg.lon = pos.lon * 1e7;
 			msg.alt = pos.alt * 1000.0f;
-			msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
-			msg.vx = pos.vel_n * 100.0f;
-			msg.vy = pos.vel_e * 100.0f;
-			msg.vz = pos.vel_d * 100.0f;
-			msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
+		    msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
+		    msg.vx = pos.vel_n * 100.0f;
+		    msg.vy = pos.vel_e * 100.0f;
+		    msg.vz = pos.vel_d * 100.0f;
+		    msg.eph = cm_uint16_from_m_float(gps.eph);
+		    msg.epv = cm_uint16_from_m_float(gps.epv);
+		    msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
 		}
@@ -2095,6 +2102,61 @@ protected:
 };
 
 
+class MavlinkStreamTrajectory : public MavlinkStream {
+public:
+	static const char *get_name_static() {
+		return "TRAJECTORY";
+	}
+	const char *get_name() const {
+		return MavlinkStreamTrajectory::get_name_static();
+	}
+	uint8_t get_id() {
+		return MAVLINK_MSG_ID_TRAJECTORY;
+	}
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamTrajectory(mavlink);
+	}
+	unsigned get_size() {
+		// TODO! Change to if published -> length, else -> 0 when correct topic will be available
+		return MAVLINK_MSG_ID_TRAJECTORY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+private:
+	MavlinkOrbSubscription *trajectory_sub;
+	uint64_t trajectory_time;
+	/* do not allow top copying this class */
+	MavlinkStreamTrajectory(MavlinkStreamTrajectory &);
+	MavlinkStreamTrajectory& operator = (const MavlinkStreamTrajectory &);
+protected:
+	explicit MavlinkStreamTrajectory(Mavlink *mavlink) : MavlinkStream(mavlink),
+			trajectory_sub(mavlink->add_orb_subscription(ORB_ID(trajectory))),
+			trajectory_time(0)
+	{}
+	void send(const hrt_abstime t) {
+		mavlink_trajectory_t msg;
+		trajectory_s report;
+		// TODO! Check if the messages match!
+		if (trajectory_sub->update(&trajectory_time, &report)) {
+			msg.time_boot_ms = report.timestamp / 1000; // uint32_t Timestamp (milliseconds since system boot)
+			msg.lat = report.lat * 1e7; // int32_t Latitude, expressed as * 1E7
+			msg.lon = report.lon * 1e7; // int32_t Longitude, expressed as * 1E7
+			// TODO! Check the altitude. Is it WGS84 or AMSL?!
+			msg.alt = report.alt * 1000.0f; // int32_t Altitude in meters, expressed as * 1000 (millimeters)
+			msg.relative_alt = report.relative_alt * 1000.0f; // int32_t Altitude above ground in meters, expressed as * 1000 (millimeters)
+			msg.vx = report.vel_n * 100.0f; // int16_t Ground X Speed (Latitude), expressed as m/s * 100
+			msg.vy = report.vel_e * 100.0f; // int16_t Ground Y Speed (Longitude), expressed as m/s * 100
+			msg.vz = report.vel_d * 100.0f; // int16_t Ground Z Speed (Altitude), expressed as m/s * 100
+			msg.hdg = report.heading * M_RAD_TO_DEG_F * 100.0f; // uint16_t Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
+
+			msg.point_type = report.point_type; // uint8_t Indicates type of the trajectory reference point. Currently 0 is for "still point" and 1 is for "valid point".
+			/* warnx("%d\n%d %d %d\n%d\n%d %d %d\n%d\n%d", msg.time_boot_ms, msg.lat, msg.lon, msg.alt,
+					msg.relative_alt, msg.vx, msg.vy, msg.vz, msg.hdg, msg.point_type); */
+			_mavlink->send_message(MAVLINK_MSG_ID_TRAJECTORY, &msg);
+		}
+	}
+};
+
+
 StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
 	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
@@ -2124,5 +2186,6 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
 	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
+	new StreamListItem(&MavlinkStreamTrajectory::new_instance, &MavlinkStreamTrajectory::get_name_static),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 032b23ca8..367b265cf 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -99,6 +99,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_gyro_pub(-1),
 	_accel_pub(-1),
 	_mag_pub(-1),
+	_range_finder_pub(-1),
 	_baro_pub(-1),
 	_airspeed_pub(-1),
 	_battery_pub(-1),
@@ -117,12 +118,15 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_rc_pub(-1),
 	_manual_pub(-1),
 	_target_pos_pub(-1),
+	_external_trajectory_pub(-1),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0f),
-	_hil_local_proj_ref{}
+	_hil_local_proj_ref{},
+	_airdog_status_pub(-1),
+	_airdog_status{}
 {
 
 	// make sure the FTP server is started
@@ -183,6 +187,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 
 	case MAVLINK_MSG_ID_HEARTBEAT:
 		handle_message_heartbeat(msg);
+		handle_message_drone_heartbeat(msg);
 		break;
 
 	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@@ -193,6 +198,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		MavlinkFTP::get_server()->handle_message(_mavlink, msg);
 		break;
 
+	case MAVLINK_MSG_ID_SYS_STATUS:
+		handle_message_drone_status(msg);
+		break;
+
+	case MAVLINK_MSG_ID_TRAJECTORY:
+		handle_message_trajectory(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -247,6 +260,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 {
+
 	/* command */
 	mavlink_command_long_t cmd_mavlink;
 	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
@@ -924,6 +938,78 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_drone_heartbeat(mavlink_message_t *msg)
+{
+    if(msg->sysid == 1) { //SIMIS TODO get sysid from linked airdog
+		mavlink_heartbeat_t heartbeat;
+		mavlink_msg_heartbeat_decode(msg, &heartbeat);
+
+		union px4_custom_mode custom_mode;
+		custom_mode.data = heartbeat.custom_mode;
+
+		_airdog_status.main_mode = custom_mode.main_mode;
+		_airdog_status.sub_mode = custom_mode.sub_mode;
+		_airdog_status.base_mode = heartbeat.base_mode;
+		_airdog_status.system_status = heartbeat.system_status;
+		_airdog_status.timestamp = hrt_absolute_time();
+
+		if (_airdog_status_pub < 0) {
+			_airdog_status_pub = orb_advertise(ORB_ID(airdog_status), &_airdog_status);
+
+		} else {
+			orb_publish(ORB_ID(airdog_status), _airdog_status_pub, &_airdog_status);
+		}
+	}
+}
+
+void
+MavlinkReceiver::handle_message_drone_status(mavlink_message_t *msg)
+{
+    if(msg->sysid == 1) { //SIMIS TODO get sysid from linked airdog
+		mavlink_sys_status_t drone_status;
+		mavlink_msg_sys_status_decode(msg, &drone_status);
+
+		_airdog_status.timestamp = hrt_absolute_time();
+		_airdog_status.battery_remaining = drone_status.battery_remaining;
+		// _airdog_status.discharged_mah = drone_status.battery_discharged_mah;
+
+		if (_airdog_status_pub < 0) {
+			_airdog_status_pub = orb_advertise(ORB_ID(airdog_status), &_airdog_status);
+
+		} else {
+			orb_publish(ORB_ID(airdog_status), _airdog_status_pub, &_airdog_status);
+		}
+	}
+}
+
+void
+MavlinkReceiver::handle_message_trajectory(mavlink_message_t *msg)
+{
+	mavlink_trajectory_t msg_traj;
+	mavlink_msg_trajectory_decode(msg, &msg_traj);
+
+	struct external_trajectory_s ext_traj;
+
+	ext_traj.timestamp = ((uint64_t) msg_traj.time_boot_ms) * 1000;
+	ext_traj.sysid = msg->sysid;
+	ext_traj.lat = msg_traj.lat * 1.0e-7d;
+	ext_traj.lon = msg_traj.lon * 1.0e-7d;
+	ext_traj.alt = msg_traj.alt * 1.0e-3f;
+	ext_traj.relative_alt = msg_traj.relative_alt * 1.0e-3f;
+	ext_traj.vel_n = msg_traj.vx * 1.0e-2f;
+	ext_traj.vel_e = msg_traj.vy * 1.0e-2f;
+	ext_traj.vel_d = msg_traj.vz * 1.0e-2f;
+	ext_traj.heading = msg_traj.hdg * 1.0e-2f * M_DEG_TO_RAD_F;
+
+	if (_external_trajectory_pub < 0) {
+		_external_trajectory_pub = orb_advertise(ORB_ID(external_trajectory), &ext_traj);
+
+	} else {
+		orb_publish(ORB_ID(external_trajectory), _external_trajectory_pub, &ext_traj);
+	}
+}
+
+void
 MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 {
 	mavlink_request_data_stream_t req;
@@ -1035,6 +1121,27 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		}
 	}
 
+    /* sonar */
+	{
+		struct range_finder_report sonar;
+		memset(&sonar, 0, sizeof(sonar));
+
+		sonar.timestamp = timestamp;
+		sonar.error_count = 0;
+		sonar.type = 1;
+		sonar.distance = imu.range_finder;
+		sonar.minimum_distance = 0.4f;
+		sonar.maximum_distance = 6;
+		sonar.valid = (sonar.distance <= sonar.maximum_distance && sonar.distance >= sonar.minimum_distance) ? 1 : 0;
+
+		if (_range_finder_pub < 0) {
+			_range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &sonar);
+
+		} else {
+			orb_publish(ORB_ID(sensor_range_finder), _range_finder_pub, &sonar);
+		}
+	}
+
 	/* baro */
 	{
 		struct baro_report baro;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index b358de807..8d08fa5c0 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -73,6 +73,8 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_force_setpoint.h>
+#include <uORB/topics/airdog_status.h>
+#include <uORB/topics/external_trajectory.h>
 
 #include "mavlink_ftp.h"
 
@@ -129,6 +131,10 @@ private:
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
+	void handle_message_drone_heartbeat(mavlink_message_t *msg);
+	void handle_message_drone_status(mavlink_message_t *msg);
+	void handle_message_trajectory(mavlink_message_t *msg);
+
 
 	void *receive_thread(void *arg);
 
@@ -143,12 +149,12 @@ private:
 	orb_advert_t _gyro_pub;
 	orb_advert_t _accel_pub;
 	orb_advert_t _mag_pub;
+	orb_advert_t _range_finder_pub;
 	orb_advert_t _baro_pub;
 	orb_advert_t _airspeed_pub;
 	orb_advert_t _battery_pub;
 	orb_advert_t _cmd_pub;
 	orb_advert_t _flow_pub;
-	orb_advert_t _range_pub;
 	orb_advert_t _offboard_control_sp_pub;
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
@@ -161,12 +167,15 @@ private:
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
 	orb_advert_t _target_pos_pub;
+	orb_advert_t _external_trajectory_pub;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
+	orb_advert_t _airdog_status_pub;
+	struct airdog_status_s _airdog_status;
 
 	/* do not allow copying this class */
 	MavlinkReceiver(const MavlinkReceiver&);