path: root/src/modules/mavlink/mavlink_messages.cpp

/****************************************************************************
 *
 *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file mavlink_messages.cpp
 * MAVLink 1.0 message formatters implementation.
 *
 * @author Lorenz Meier <lm@inf.ethz.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 */

#include <stdio.h>
#include <commander/px4_custom_mode.h>
#include <lib/geo/geo.h>

#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/navigation_capabilities.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>

#include "mavlink_messages.h"


static uint16_t cm_uint16_from_m_float(float m);
static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
				   uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);

uint16_t
cm_uint16_from_m_float(float m)
{
	if (m < 0.0f) {
		return 0;

	} else if (m > 655.35f) {
		return 65535;
	}

	return (uint16_t)(m * 100.0f);
}

void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
			    uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
{
	*mavlink_state = 0;
	*mavlink_base_mode = 0;
	*mavlink_custom_mode = 0;

	/* HIL */
	if (status->hil_state == HIL_STATE_ON) {
		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
	}

	/* arming state */
	if (status->arming_state == ARMING_STATE_ARMED
	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
	}

	/* main state */
	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

	union px4_custom_mode custom_mode;
	custom_mode.data = 0;

	if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
		/* use main state when navigator is not active */
		if (status->main_state == MAIN_STATE_MANUAL) {
			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;

		} else if (status->main_state == MAIN_STATE_SEATBELT) {
			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;

		} else if (status->main_state == MAIN_STATE_EASY) {
			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;

		} else if (status->main_state == MAIN_STATE_AUTO) {
			*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_READY;
		}

	} else {
		/* use navigation state when navigator is active */
		*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;

		if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;

		} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;

		} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;

		} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;

		} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
		}
	}

	*mavlink_custom_mode = custom_mode.data;

	/* set system state */
	if (status->arming_state == ARMING_STATE_INIT
	    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
	    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
		*mavlink_state = MAV_STATE_UNINIT;

	} else if (status->arming_state == ARMING_STATE_ARMED) {
		*mavlink_state = MAV_STATE_ACTIVE;

	} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
		*mavlink_state = MAV_STATE_CRITICAL;

	} else if (status->arming_state == ARMING_STATE_STANDBY) {
		*mavlink_state = MAV_STATE_STANDBY;

	} else if (status->arming_state == ARMING_STATE_REBOOT) {
		*mavlink_state = MAV_STATE_POWEROFF;

	} else {
		*mavlink_state = MAV_STATE_CRITICAL;
	}
}


class MavlinkStreamHeartbeat : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "HEARTBEAT";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamHeartbeat();
	}

private:
	MavlinkOrbSubscription *status_sub;
	struct vehicle_status_s *status;

	MavlinkOrbSubscription *pos_sp_triplet_sub;
	struct position_setpoint_triplet_s *pos_sp_triplet;

protected:
	void subscribe(Mavlink *mavlink)
	{
		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
		status = (struct vehicle_status_s *)status_sub->get_data();

		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		(void)status_sub->update(t);
		(void)pos_sp_triplet_sub->update(t);

		uint8_t mavlink_state = 0;
		uint8_t mavlink_base_mode = 0;
		uint32_t mavlink_custom_mode = 0;
		get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);

		mavlink_msg_heartbeat_send(_channel,
					   mavlink_system.type,
					   MAV_AUTOPILOT_PX4,
					   mavlink_base_mode,
					   mavlink_custom_mode,
					   mavlink_state);

	}
};


class MavlinkStreamSysStatus : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "SYS_STATUS";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamSysStatus();
	}

private:
	MavlinkOrbSubscription *status_sub;
	struct vehicle_status_s *status;

protected:
	void subscribe(Mavlink *mavlink)
	{
		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
		status = (struct vehicle_status_s *)status_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		status_sub->update(t);
		mavlink_msg_sys_status_send(_channel,
						status->onboard_control_sensors_present,
						status->onboard_control_sensors_enabled,
						status->onboard_control_sensors_health,
						status->load * 1000.0f,
						status->battery_voltage * 1000.0f,
						status->battery_current * 1000.0f,
						status->battery_remaining,
						status->drop_rate_comm,
						status->errors_comm,
						status->errors_count1,
						status->errors_count2,
						status->errors_count3,
						status->errors_count4);
	}
};


class MavlinkStreamHighresIMU : public MavlinkStream
{
public:
	MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0)
	{
	}

	const char *get_name()
	{
		return "HIGHRES_IMU";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamHighresIMU();
	}

private:
	MavlinkOrbSubscription *sensor_sub;
	struct sensor_combined_s *sensor;

	uint64_t accel_timestamp;
	uint64_t gyro_timestamp;
	uint64_t mag_timestamp;
	uint64_t baro_timestamp;

protected:
	void subscribe(Mavlink *mavlink)
	{
		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
		sensor = (struct sensor_combined_s *)sensor_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (sensor_sub->update(t)) {
			uint16_t fields_updated = 0;

			if (accel_timestamp != sensor->accelerometer_timestamp) {
				/* mark first three dimensions as changed */
				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
				accel_timestamp = sensor->accelerometer_timestamp;
			}

			if (gyro_timestamp != sensor->timestamp) {
				/* mark second group dimensions as changed */
				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
				gyro_timestamp = sensor->timestamp;
			}

			if (mag_timestamp != sensor->magnetometer_timestamp) {
				/* mark third group dimensions as changed */
				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
				mag_timestamp = sensor->magnetometer_timestamp;
			}

			if (baro_timestamp != sensor->baro_timestamp) {
				/* mark last group dimensions as changed */
				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
				baro_timestamp = sensor->baro_timestamp;
			}

			mavlink_msg_highres_imu_send(_channel,
						     sensor->timestamp,
						     sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
						     sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
						     sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
						     sensor->baro_pres_mbar, sensor->differential_pressure_pa,
						     sensor->baro_alt_meter, sensor->baro_temp_celcius,
						     fields_updated);
		}
	}
};


class MavlinkStreamAttitude : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "ATTITUDE";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamAttitude();
	}

private:
	MavlinkOrbSubscription *att_sub;
	struct vehicle_attitude_s *att;

protected:
	void subscribe(Mavlink *mavlink)
	{
		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
		att = (struct vehicle_attitude_s *)att_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (att_sub->update(t)) {
			mavlink_msg_attitude_send(_channel,
						  att->timestamp / 1000,
						  att->roll, att->pitch, att->yaw,
						  att->rollspeed, att->pitchspeed, att->yawspeed);
		}
	}
};


class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "ATTITUDE_QUATERNION";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamAttitudeQuaternion();
	}

private:
	MavlinkOrbSubscription *att_sub;
	struct vehicle_attitude_s *att;

protected:
	void subscribe(Mavlink *mavlink)
	{
		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
		att = (struct vehicle_attitude_s *)att_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (att_sub->update(t)) {
			mavlink_msg_attitude_quaternion_send(_channel,
							     att->timestamp / 1000,
							     att->q[0],
							     att->q[1],
							     att->q[2],
							     att->q[3],
							     att->rollspeed,
							     att->pitchspeed,
							     att->yawspeed);
		}
	}
};


class MavlinkStreamVFRHUD : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "VFR_HUD";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamVFRHUD();
	}

private:
	MavlinkOrbSubscription *att_sub;
	struct vehicle_attitude_s *att;

	MavlinkOrbSubscription *pos_sub;
	struct vehicle_global_position_s *pos;

	MavlinkOrbSubscription *armed_sub;
	struct actuator_armed_s *armed;

	MavlinkOrbSubscription *act_sub;
	struct actuator_controls_s *act;

	MavlinkOrbSubscription *airspeed_sub;
	struct airspeed_s *airspeed;

protected:
	void subscribe(Mavlink *mavlink)
	{
		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
		att = (struct vehicle_attitude_s *)att_sub->get_data();

		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
		pos = (struct vehicle_global_position_s *)pos_sub->get_data();

		armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
		armed = (struct actuator_armed_s *)armed_sub->get_data();

		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
		act = (struct actuator_controls_s *)act_sub->get_data();

		airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
		airspeed = (struct airspeed_s *)airspeed_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		bool updated = att_sub->update(t);
		updated |= pos_sub->update(t);
		updated |= armed_sub->update(t);
		updated |= act_sub->update(t);
		updated |= airspeed_sub->update(t);

		if (updated) {
			float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
			uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
			float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;

			mavlink_msg_vfr_hud_send(_channel,
						 airspeed->true_airspeed_m_s,
						 groundspeed,
						 heading,
						 throttle,
						 pos->alt,
						 -pos->vel_d);
		}
	}
};


class MavlinkStreamGPSRawInt : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "GPS_RAW_INT";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamGPSRawInt();
	}

private:
	MavlinkOrbSubscription *gps_sub;
	struct vehicle_gps_position_s *gps;

protected:
	void subscribe(Mavlink *mavlink)
	{
		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
		gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (gps_sub->update(t)) {
			mavlink_msg_gps_raw_int_send(_channel,
						     gps->timestamp_position,
						     gps->fix_type,
						     gps->lat,
						     gps->lon,
						     gps->alt,
						     cm_uint16_from_m_float(gps->eph_m),
						     cm_uint16_from_m_float(gps->epv_m),
						     gps->vel_m_s * 100.0f,
						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
						     gps->satellites_visible);
		}
	}
};


class MavlinkStreamGlobalPositionInt : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "GLOBAL_POSITION_INT";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamGlobalPositionInt();
	}

private:
	MavlinkOrbSubscription *pos_sub;
	struct vehicle_global_position_s *pos;

	MavlinkOrbSubscription *home_sub;
	struct home_position_s *home;

protected:
	void subscribe(Mavlink *mavlink)
	{
		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
		pos = (struct vehicle_global_position_s *)pos_sub->get_data();

		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
		home = (struct home_position_s *)home_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		bool updated = pos_sub->update(t);
		updated |= home_sub->update(t);

		if (updated) {
			mavlink_msg_global_position_int_send(_channel,
							     pos->timestamp / 1000,
							     pos->lat * 1e7,
							     pos->lon * 1e7,
							     pos->alt * 1000.0f,
							     (pos->alt - home->alt) * 1000.0f,
							     pos->vel_n * 100.0f,
							     pos->vel_e * 100.0f,
							     pos->vel_d * 100.0f,
							     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
		}
	}
};


class MavlinkStreamLocalPositionNED : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "LOCAL_POSITION_NED";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamLocalPositionNED();
	}

private:
	MavlinkOrbSubscription *pos_sub;
	struct vehicle_local_position_s *pos;

protected:
	void subscribe(Mavlink *mavlink)
	{
		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
		pos = (struct vehicle_local_position_s *)pos_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (pos_sub->update(t)) {
			mavlink_msg_local_position_ned_send(_channel,
							    pos->timestamp / 1000,
							    pos->x,
							    pos->y,
							    pos->z,
							    pos->vx,
							    pos->vy,
							    pos->vz);
		}
	}
};



class MavlinkStreamViconPositionEstimate : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "VICON_POSITION_ESTIMATE";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamViconPositionEstimate();
	}

private:
	MavlinkOrbSubscription *pos_sub;
	struct vehicle_vicon_position_s *pos;

protected:
	void subscribe(Mavlink *mavlink)
	{
		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
		pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (pos_sub->update(t)) {
			mavlink_msg_vicon_position_estimate_send(_channel,
								pos->timestamp / 1000,
								pos->x,
								pos->y,
								pos->z,
								pos->roll,
								pos->pitch,
								pos->yaw);
		}
	}
};


class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "GPS_GLOBAL_ORIGIN";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamGPSGlobalOrigin();
	}

private:
	MavlinkOrbSubscription *home_sub;
	struct home_position_s *home;

protected:
	void subscribe(Mavlink *mavlink)
	{
		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
		home = (struct home_position_s *)home_sub->get_data();
	}

	void send(const hrt_abstime t)
	{

		/* we're sending the GPS home periodically to ensure the
		 * the GCS does pick it up at one point */
		if (home_sub->is_published()) {
			home_sub->update(t);

			mavlink_msg_gps_global_origin_send(_channel,
							   (int32_t)(home->lat * 1e7),
							   (int32_t)(home->lon * 1e7),
							   (int32_t)(home->alt) * 1000.0f);
		}
	}
};


class MavlinkStreamServoOutputRaw : public MavlinkStream
{
public:
	MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
	{
		sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
	}

	const char *get_name()
	{
		return _name;
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamServoOutputRaw(_n);
	}

private:
	MavlinkOrbSubscription *act_sub;
	struct actuator_outputs_s *act;

	char _name[20];
	unsigned int _n;

protected:
	void subscribe(Mavlink *mavlink)
	{
		orb_id_t act_topics[] = {
			ORB_ID(actuator_outputs_0),
			ORB_ID(actuator_outputs_1),
			ORB_ID(actuator_outputs_2),
			ORB_ID(actuator_outputs_3)
		};

		act_sub = mavlink->add_orb_subscription(act_topics[_n]);
		act = (struct actuator_outputs_s *)act_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (act_sub->update(t)) {
			mavlink_msg_servo_output_raw_send(_channel,
							  act->timestamp / 1000,
							  _n,
							  act->output[0],
							  act->output[1],
							  act->output[2],
							  act->output[3],
							  act->output[4],
							  act->output[5],
							  act->output[6],
							  act->output[7]);
		}
	}
};


class MavlinkStreamHILControls : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "HIL_CONTROLS";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamHILControls();
	}

private:
	MavlinkOrbSubscription *status_sub;
	struct vehicle_status_s *status;

	MavlinkOrbSubscription *pos_sp_triplet_sub;
	struct position_setpoint_triplet_s *pos_sp_triplet;

	MavlinkOrbSubscription *act_sub;
	struct actuator_outputs_s *act;

protected:
	void subscribe(Mavlink *mavlink)
	{
		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
		status = (struct vehicle_status_s *)status_sub->get_data();

		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();

		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
		act = (struct actuator_outputs_s *)act_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		bool updated = status_sub->update(t);
		updated |= pos_sp_triplet_sub->update(t);
		updated |= act_sub->update(t);

		if (updated) {
			/* translate the current syste state to mavlink state and mode */
			uint8_t mavlink_state;
			uint8_t mavlink_base_mode;
			uint32_t mavlink_custom_mode;
			get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);

			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
				/* set number of valid outputs depending on vehicle type */
				unsigned n;

				switch (mavlink_system.type) {
				case MAV_TYPE_QUADROTOR:
					n = 4;
					break;

				case MAV_TYPE_HEXAROTOR:
					n = 6;
					break;

				default:
					n = 8;
					break;
				}

				/* scale / assign outputs depending on system type */
				float out[8];

				for (unsigned i = 0; i < 8; i++) {
					if (i < n) {
						if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
							/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
							out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);

						} else {
							/* send 0 when disarmed */
							out[i] = 0.0f;
						}

					} else {
						out[i] = -1.0f;
					}
				}

				mavlink_msg_hil_controls_send(_channel,
							      hrt_absolute_time(),
							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
							      mavlink_base_mode,
							      0);
			} else {

				/* fixed wing: scale all channels except throttle -1 .. 1
				 * because we know that we set the mixers up this way
				 */

				float out[8];

				const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;

				for (unsigned i = 0; i < 8; i++) {
					if (i != 3) {
						/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
						out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);

					} else {

						/* scale fake PWM out 900..2100 us to 0..1 for throttle */
						out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
					}

				}

				mavlink_msg_hil_controls_send(_channel,
							      hrt_absolute_time(),
							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
							      mavlink_base_mode,
							      0);
			}
		}
	}
};


class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "GLOBAL_POSITION_SETPOINT_INT";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamGlobalPositionSetpointInt();
	}

private:
	MavlinkOrbSubscription *pos_sp_triplet_sub;
	struct position_setpoint_triplet_s *pos_sp_triplet;

protected:
	void subscribe(Mavlink *mavlink)
	{
		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (pos_sp_triplet_sub->update(t)) {
			mavlink_msg_global_position_setpoint_int_send(_channel,
					MAV_FRAME_GLOBAL,
					(int32_t)(pos_sp_triplet->current.lat * 1e7),
					(int32_t)(pos_sp_triplet->current.lon * 1e7),
					(int32_t)(pos_sp_triplet->current.alt * 1000),
					(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
		}
	}
};


class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "LOCAL_POSITION_SETPOINT";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamLocalPositionSetpoint();
	}

private:
	MavlinkOrbSubscription *pos_sp_sub;
	struct vehicle_local_position_setpoint_s *pos_sp;

protected:
	void subscribe(Mavlink *mavlink)
	{
		pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
		pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (pos_sp_sub->update(t)) {
			mavlink_msg_local_position_setpoint_send(_channel,
					MAV_FRAME_LOCAL_NED,
					pos_sp->x,
					pos_sp->y,
					pos_sp->z,
					pos_sp->yaw);
		}
	}
};


class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamRollPitchYawThrustSetpoint();
	}

private:
	MavlinkOrbSubscription *att_sp_sub;
	struct vehicle_attitude_setpoint_s *att_sp;

protected:
	void subscribe(Mavlink *mavlink)
	{
		att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
		att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (att_sp_sub->update(t)) {
			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
					att_sp->timestamp / 1000,
					att_sp->roll_body,
					att_sp->pitch_body,
					att_sp->yaw_body,
					att_sp->thrust);
		}
	}
};


class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
	}

private:
	MavlinkOrbSubscription *att_rates_sp_sub;
	struct vehicle_rates_setpoint_s *att_rates_sp;

protected:
	void subscribe(Mavlink *mavlink)
	{
		att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
		att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (att_rates_sp_sub->update(t)) {
			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
					att_rates_sp->timestamp / 1000,
					att_rates_sp->roll,
					att_rates_sp->pitch,
					att_rates_sp->yaw,
					att_rates_sp->thrust);
		}
	}
};


class MavlinkStreamRCChannelsRaw : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "RC_CHANNELS_RAW";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamRCChannelsRaw();
	}

private:
	MavlinkOrbSubscription *rc_sub;
	struct rc_input_values *rc;

protected:
	void subscribe(Mavlink *mavlink)
	{
		rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
		rc = (struct rc_input_values *)rc_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (rc_sub->update(t)) {
			const unsigned port_width = 8;

			for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
				/* Channels are sent in MAVLink main loop at a fixed interval */
				mavlink_msg_rc_channels_raw_send(_channel,
								 rc->timestamp_publication / 1000,
								 i,
								 (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
								 rc->rssi);
			}
		}
	}
};


class MavlinkStreamManualControl : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "MANUAL_CONTROL";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamManualControl();
	}

private:
	MavlinkOrbSubscription *manual_sub;
	struct manual_control_setpoint_s *manual;

protected:
	void subscribe(Mavlink *mavlink)
	{
		manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
		manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (manual_sub->update(t)) {
			mavlink_msg_manual_control_send(_channel,
							mavlink_system.sysid,
							manual->roll * 1000,
							manual->pitch * 1000,
							manual->yaw * 1000,
							manual->throttle * 1000,
							0);
		}
	}
};


class MavlinkStreamOpticalFlow : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "OPTICAL_FLOW";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamOpticalFlow();
	}

private:
	MavlinkOrbSubscription *flow_sub;
	struct optical_flow_s *flow;

protected:
	void subscribe(Mavlink *mavlink)
	{
		flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
		flow = (struct optical_flow_s *)flow_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (flow_sub->update(t)) {
			mavlink_msg_optical_flow_send(_channel,
						      flow->timestamp,
						      flow->sensor_id,
						      flow->flow_raw_x, flow->flow_raw_y,
						      flow->flow_comp_x_m, flow->flow_comp_y_m,
						      flow->quality,
						      flow->ground_distance_m);
		}
	}
};

class MavlinkStreamAttitudeControls : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "ATTITUDE_CONTROLS";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamAttitudeControls();
	}

private:
	MavlinkOrbSubscription *att_ctrl_sub;
	struct actuator_controls_s *att_ctrl;

protected:
	void subscribe(Mavlink *mavlink)
	{
		att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
		att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (att_ctrl_sub->update(t)) {
			/* send, add spaces so that string buffer is at least 10 chars long */
			mavlink_msg_named_value_float_send(_channel,
							   att_ctrl->timestamp / 1000,
							   "rll ctrl    ",
							   att_ctrl->control[0]);
			mavlink_msg_named_value_float_send(_channel,
							   att_ctrl->timestamp / 1000,
							   "ptch ctrl    ",
							   att_ctrl->control[1]);
			mavlink_msg_named_value_float_send(_channel,
							   att_ctrl->timestamp / 1000,
							   "yaw ctrl     ",
							   att_ctrl->control[2]);
			mavlink_msg_named_value_float_send(_channel,
							   att_ctrl->timestamp / 1000,
							   "thr ctrl     ",
							   att_ctrl->control[3]);
		}
	}
};

class MavlinkStreamNamedValueFloat : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "NAMED_VALUE_FLOAT";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamNamedValueFloat();
	}

private:
	MavlinkOrbSubscription *debug_sub;
	struct debug_key_value_s *debug;

protected:
	void subscribe(Mavlink *mavlink)
	{
		debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
		debug = (struct debug_key_value_s *)debug_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		if (debug_sub->update(t)) {
			/* enforce null termination */
			debug->key[sizeof(debug->key) - 1] = '\0';

			mavlink_msg_named_value_float_send(_channel,
							   debug->timestamp_ms,
							   debug->key,
							   debug->value);
		}
	}
};

class MavlinkStreamCameraCapture : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "CAMERA_CAPTURE";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamCameraCapture();
	}

private:
	MavlinkOrbSubscription *status_sub;
	struct vehicle_status_s *status;

protected:
	void subscribe(Mavlink *mavlink)
	{
		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
		status = (struct vehicle_status_s *)status_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		(void)status_sub->update(t);

		if (status->arming_state == ARMING_STATE_ARMED
		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {

			/* send camera capture on */
			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);

		} else {
			/* send camera capture off */
			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
		}
	}
};

class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
	const char *get_name()
	{
		return "DISTANCE_SENSOR";
	}

	MavlinkStream *new_instance()
	{
		return new MavlinkStreamDistanceSensor();
	}

private:
	MavlinkOrbSubscription *range_sub;
	struct range_finder_report *range;

protected:
	void subscribe(Mavlink *mavlink)
	{
		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
		range = (struct range_finder_report *)range_sub->get_data();
	}

	void send(const hrt_abstime t)
	{
		(void)range_sub->update(t);

		uint8_t type;

		switch (range->type) {
			case RANGE_FINDER_TYPE_LASER:
			type = MAV_DISTANCE_SENSOR_LASER;
			break;
		}

		uint8_t id = 0;
		uint8_t orientation = 0;
		uint8_t covariance = 20;

		mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, 
			range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
	}
};

MavlinkStream *streams_list[] = {
	new MavlinkStreamHeartbeat(),
	new MavlinkStreamSysStatus(),
	new MavlinkStreamHighresIMU(),
	new MavlinkStreamAttitude(),
	new MavlinkStreamAttitudeQuaternion(),
	new MavlinkStreamVFRHUD(),
	new MavlinkStreamGPSRawInt(),
	new MavlinkStreamGlobalPositionInt(),
	new MavlinkStreamLocalPositionNED(),
	new MavlinkStreamGPSGlobalOrigin(),
	new MavlinkStreamServoOutputRaw(0),
	new MavlinkStreamServoOutputRaw(1),
	new MavlinkStreamServoOutputRaw(2),
	new MavlinkStreamServoOutputRaw(3),
	new MavlinkStreamHILControls(),
	new MavlinkStreamGlobalPositionSetpointInt(),
	new MavlinkStreamLocalPositionSetpoint(),
	new MavlinkStreamRollPitchYawThrustSetpoint(),
	new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
	new MavlinkStreamRCChannelsRaw(),
	new MavlinkStreamManualControl(),
	new MavlinkStreamOpticalFlow(),
	new MavlinkStreamAttitudeControls(),
	new MavlinkStreamNamedValueFloat(),
	new MavlinkStreamCameraCapture(),
	new MavlinkStreamDistanceSensor(),
	new MavlinkStreamViconPositionEstimate(),
	nullptr
};