/****************************************************************************
*
* 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/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 <systemlib/err.h>
#include <mavlink/mavlink_log.h>
#include "mavlink_messages.h"
#include "mavlink_main.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;
switch (status->nav_state) {
case NAVIGATION_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;
break;
case NAVIGATION_STATE_ACRO:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case NAVIGATION_STATE_ALTCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;
case NAVIGATION_STATE_POSCTL:
*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_POSCTL;
break;
case NAVIGATION_STATE_AUTO_MISSION:
*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_MISSION;
break;
case NAVIGATION_STATE_AUTO_LOITER:
*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_LOITER;
break;
case NAVIGATION_STATE_AUTO_RTL:
*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_RTL;
break;
case NAVIGATION_STATE_LAND:
/* fallthrough */
case NAVIGATION_STATE_DESCEND:
*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_LAND;
break;
case NAVIGATION_STATE_AUTO_RTGS:
*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_RTGS;
break;
case NAVIGATION_STATE_TERMINATION:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case NAVIGATION_STATE_OFFBOARD:
*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_OFFBOARD;
break;
case NAVIGATION_STATE_MAX:
/* this is an unused case, ignore */
break;
}
*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() const
{
return MavlinkStreamHeartbeat::get_name_static();
}
static const char *get_name_static()
{
return "HEARTBEAT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_HEARTBEAT;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamHeartbeat(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
bool const_rate() {
return true;
}
private:
MavlinkOrbSubscription *_status_sub;
MavlinkOrbSubscription *_pos_sp_triplet_sub;
/* do not allow top copying this class */
MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &);
MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &);
protected:
explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink),
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
{}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
/* always send the heartbeat, independent of the update status of the topics */
if (!_status_sub->update(&status)) {
/* if topic update failed fill it with defaults */
memset(&status, 0, sizeof(status));
}
if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
/* if topic update failed fill it with defaults */
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
}
mavlink_heartbeat_t msg;
msg.base_mode = 0;
msg.custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
msg.type = mavlink_system.type;
msg.autopilot = MAV_AUTOPILOT_PX4;
msg.mavlink_version = 3;
_mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg);
}
};
class MavlinkStreamStatustext : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamStatustext::get_name_static();
}
static const char *get_name_static()
{
return "STATUSTEXT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_STATUSTEXT;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamStatustext(mavlink);
}
unsigned get_size() {
return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
}
private:
/* do not allow top copying this class */
MavlinkStreamStatustext(MavlinkStreamStatustext &);
MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
protected:
explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
void send(const hrt_abstime t)
{
if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
struct mavlink_logmessage logmsg;
int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg);
if (lb_ret == OK) {
mavlink_statustext_t msg;
/* map severity */
switch (logmsg.severity) {
case MAVLINK_IOC_SEND_TEXT_INFO:
msg.severity = MAV_SEVERITY_INFO;
break;
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
msg.severity = MAV_SEVERITY_CRITICAL;
break;
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
msg.severity = MAV_SEVERITY_EMERGENCY;
break;
default:
msg.severity = MAV_SEVERITY_INFO;
break;
}
strncpy(msg.text, logmsg.text, sizeof(msg.text));
_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
}
}
}
};
class MavlinkStreamCommandLong : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamCommandLong::get_name_static();
}
static const char *get_name_static()
{
return "COMMAND_LONG";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_COMMAND_LONG;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamCommandLong(mavlink);
}
unsigned get_size() {
return 0; // commands stream is not regular and not predictable
}
private:
MavlinkOrbSubscription *_cmd_sub;
uint64_t _cmd_time;
/* do not allow top copying this class */
MavlinkStreamCommandLong(MavlinkStreamCommandLong &);
MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &);
protected:
explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink),
_cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
_cmd_time(0)
{}
void send(const hrt_abstime t)
{
struct vehicle_command_s cmd;
if (_cmd_sub->update(&_cmd_time, &cmd)) {
/* only send commands for other systems/components */
if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
mavlink_command_long_t msg;
msg.target_system = cmd.target_system;
msg.target_component = cmd.target_component;
msg.command = cmd.command;
msg.confirmation = cmd.confirmation;
msg.param1 = cmd.param1;
msg.param2 = cmd.param2;
msg.param3 = cmd.param3;
msg.param4 = cmd.param4;
msg.param5 = cmd.param5;
msg.param6 = cmd.param6;
msg.param7 = cmd.param7;
_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
}
}
}
};
class MavlinkStreamSysStatus : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamSysStatus::get_name_static();
}
static const char *get_name_static()
{
return "SYS_STATUS";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_SYS_STATUS;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamSysStatus(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_status_sub;
/* do not allow top copying this class */
MavlinkStreamSysStatus(MavlinkStreamSysStatus &);
MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &);
protected:
explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
{}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
if (_status_sub->update(&status)) {
mavlink_sys_status_t msg;
msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
msg.load = status.load * 1000.0f;
msg.voltage_battery = status.battery_voltage * 1000.0f;
msg.current_battery = status.battery_current * 100.0f;
msg.drop_rate_comm = status.drop_rate_comm;
msg.errors_comm = status.errors_comm;
msg.errors_count1 = status.errors_count1;
msg.errors_count2 = status.errors_count2;
msg.errors_count3 = status.errors_count3;
msg.errors_count4 = status.errors_count4;
msg.battery_remaining = status.battery_remaining * 100.0f;
_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
}
}
};
class MavlinkStreamHighresIMU : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamHighresIMU::get_name_static();
}
static const char *get_name_static()
{
return "HIGHRES_IMU";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_HIGHRES_IMU;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamHighresIMU(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_sensor_sub;
uint64_t _sensor_time;
uint64_t _accel_timestamp;
uint64_t _gyro_timestamp;
uint64_t _mag_timestamp;
uint64_t _baro_timestamp;
/* do not allow top copying this class */
MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &);
MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &);
protected:
explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
_sensor_time(0),
_accel_timestamp(0),
_gyro_timestamp(0),
_mag_timestamp(0),
_baro_timestamp(0)
{}
void send(const hrt_abstime t)
{
struct sensor_combined_s sensor;
if (_sensor_sub->update(&_sensor_time, &sensor)) {
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_highres_imu_t msg;
msg.time_usec = sensor.timestamp;
msg.xacc = sensor.accelerometer_m_s2[0];
msg.yacc = sensor.accelerometer_m_s2[1];
msg.zacc = sensor.accelerometer_m_s2[2];
msg.xgyro = sensor.gyro_rad_s[0];
msg.ygyro = sensor.gyro_rad_s[1];
msg.zgyro = sensor.gyro_rad_s[2];
msg.xmag = sensor.magnetometer_ga[0];
msg.ymag = sensor.magnetometer_ga[1];
msg.zmag = sensor.magnetometer_ga[2];
msg.abs_pressure = sensor.baro_pres_mbar;
msg.diff_pressure = sensor.differential_pressure_pa;
msg.pressure_alt = sensor.baro_alt_meter;
msg.temperature = sensor.baro_temp_celcius;
msg.fields_updated = fields_updated;
_mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg);
}
}
};
class MavlinkStreamAttitude : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamAttitude::get_name_static();
}
static const char *get_name_static()
{
return "ATTITUDE";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ATTITUDE;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamAttitude(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_att_sub;
uint64_t _att_time;
/* do not allow top copying this class */
MavlinkStreamAttitude(MavlinkStreamAttitude &);
MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &);
protected:
explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
_att_time(0)
{}
void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
if (_att_sub->update(&_att_time, &att)) {
mavlink_attitude_t msg;
msg.time_boot_ms = att.timestamp / 1000;
msg.roll = att.roll;
msg.pitch = att.pitch;
msg.yaw = att.yaw;
msg.rollspeed = att.rollspeed;
msg.pitchspeed = att.pitchspeed;
msg.yawspeed = att.yawspeed;
_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg);
}
}
};
class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamAttitudeQuaternion::get_name_static();
}
static const char *get_name_static()
{
return "ATTITUDE_QUATERNION";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamAttitudeQuaternion(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_att_sub;
uint64_t _att_time;
/* do not allow top copying this class */
MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &);
MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &);
protected:
explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
_att_time(0)
{}
void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
if (_att_sub->update(&_att_time, &att)) {
mavlink_attitude_quaternion_t msg;
msg.time_boot_ms = att.timestamp / 1000;
msg.q1 = att.q[0];
msg.q2 = att.q[1];
msg.q3 = att.q[2];
msg.q4 = att.q[3];
msg.rollspeed = att.rollspeed;
msg.pitchspeed = att.pitchspeed;
msg.yawspeed = att.yawspeed;
_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg);
}
}
};
class MavlinkStreamVFRHUD : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamVFRHUD::get_name_static();
}
static const char *get_name_static()
{
return "VFR_HUD";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_VFR_HUD;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamVFRHUD(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_att_sub;
uint64_t _att_time;
MavlinkOrbSubscription *_pos_sub;
uint64_t _pos_time;
MavlinkOrbSubscription *_armed_sub;
uint64_t _armed_time;
MavlinkOrbSubscription *_act_sub;
uint64_t _act_time;
MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
protected:
explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
_att_time(0),
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
_pos_time(0),
_armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))),
_armed_time(0),
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act_time(0),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_time(0)
{}
void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
struct vehicle_global_position_s pos;
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
bool updated = _att_sub->update(&_att_time, &att);
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _act_sub->update(&_act_time, &act);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
if (updated) {
mavlink_vfr_hud_t msg;
msg.airspeed = airspeed.true_airspeed_m_s;
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
msg.alt = pos.alt;
msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
}
}
};
class MavlinkStreamGPSRawInt : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamGPSRawInt::get_name_static();
}
static const char *get_name_static()
{
return "GPS_RAW_INT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_GPS_RAW_INT;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamGPSRawInt(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_gps_sub;
uint64_t _gps_time;
/* do not allow top copying this class */
MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &);
MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &);
protected:
explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink),
_gps_sub(nullptr),
_gps_time(0)
{}
void subscribe()
{
_gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
}
void send(const hrt_abstime t)
{
struct vehicle_gps_position_s gps;
if (_gps_sub->update(&_gps_time, &gps)) {
mavlink_gps_raw_int_t msg;
msg.time_usec = gps.timestamp_position;
msg.fix_type = gps.fix_type;
msg.lat = gps.lat;
msg.lon = gps.lon;
msg.alt = gps.alt;
msg.eph = cm_uint16_from_m_float(gps.eph);
msg.epv = cm_uint16_from_m_float(gps.epv);
msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
msg.satellites_visible = gps.satellites_used;
_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
}
}
};
//class MavlinkStreamGlobalPositionInt : public MavlinkStream
//{
//public:
// const char *get_name() const
// {
// return MavlinkStreamGlobalPositionInt::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "GLOBAL_POSITION_INT";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamGlobalPositionInt();
// }
//
//private:
// MavlinkOrbSubscription *pos_sub;
// uint64_t pos_time;
//
// MavlinkOrbSubscription *home_sub;
// uint64_t home_time;
//
// /* do not allow top copying this class */
// MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &);
// MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &);
//
//protected:
// explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
// pos_sub(nullptr),
// pos_time(0),
// home_sub(nullptr),
// home_time(0)
// {}
//
// void subscribe()
// {
// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position));
// }
//
// void send(const hrt_abstime t)
// {
// struct vehicle_global_position_s pos;
// struct home_position_s home;
//
// bool updated = pos_sub->update(&pos_time, &pos);
// updated |= home_sub->update(&home_time, &home);
//
// 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() const
// {
// return MavlinkStreamLocalPositionNED::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "LOCAL_POSITION_NED";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamLocalPositionNED();
// }
//
//private:
// MavlinkOrbSubscription *pos_sub;
// uint64_t pos_time;
//
// /* do not allow top copying this class */
// MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &);
// MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &);
//
//protected:
// explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
// pos_sub(nullptr),
// pos_time(0)
// {}
//
// void subscribe()
// {
// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
// }
//
// void send(const hrt_abstime t)
// {
// struct vehicle_local_position_s pos;
//
// if (pos_sub->update(&pos_time, &pos)) {
// 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() const
// {
// return MavlinkStreamViconPositionEstimate::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "VICON_POSITION_ESTIMATE";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamViconPositionEstimate();
// }
//
//private:
// MavlinkOrbSubscription *pos_sub;
// uint64_t pos_time;
//
// /* do not allow top copying this class */
// MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &);
// MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &);
//
//protected:
// explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
// pos_sub(nullptr),
// pos_time(0)
// {}
//
// void subscribe()
// {
// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
// }
//
// void send(const hrt_abstime t)
// {
// struct vehicle_vicon_position_s pos;
//
// if (pos_sub->update(&pos_time, &pos)) {
// 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() const
// {
// return MavlinkStreamGPSGlobalOrigin::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "GPS_GLOBAL_ORIGIN";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamGPSGlobalOrigin();
// }
//
//private:
// MavlinkOrbSubscription *home_sub;
//
// /* do not allow top copying this class */
// MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &);
// MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &);
//
//protected:
// explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(),
// home_sub(nullptr)
// {}
//
// void subscribe()
// {
// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position));
// }
//
// 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()) {
// struct home_position_s home;
//
// if (home_sub->update(&home)) {
// mavlink_msg_gps_global_origin_send(_channel,
// (int32_t)(home.lat * 1e7),
// (int32_t)(home.lon * 1e7),
// (int32_t)(home.alt) * 1000.0f);
// }
// }
// }
//};
//
//template <int N>
//class MavlinkStreamServoOutputRaw : public MavlinkStream
//{
//public:
// const char *get_name() const
// {
// return MavlinkStreamServoOutputRaw<N>::get_name_static();
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
// }
//
// static const char *get_name_static()
// {
// switch (N) {
// case 0:
// return "SERVO_OUTPUT_RAW_0";
//
// case 1:
// return "SERVO_OUTPUT_RAW_1";
//
// case 2:
// return "SERVO_OUTPUT_RAW_2";
//
// case 3:
// return "SERVO_OUTPUT_RAW_3";
// }
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamServoOutputRaw<N>();
// }
//
//private:
// MavlinkOrbSubscription *act_sub;
// uint64_t act_time;
//
// /* do not allow top copying this class */
// MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &);
// MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &);
//
//protected:
// explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
// act_sub(nullptr),
// act_time(0)
// {}
//
// void subscribe()
// {
// 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]);
// }
//
// void send(const hrt_abstime t)
// {
// struct actuator_outputs_s act;
//
// if (act_sub->update(&act_time, &act)) {
// 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() const
// {
// return MavlinkStreamHILControls::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "HIL_CONTROLS";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_HIL_CONTROLS;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamHILControls();
// }
//
//private:
// MavlinkOrbSubscription *status_sub;
// uint64_t status_time;
//
// MavlinkOrbSubscription *pos_sp_triplet_sub;
// uint64_t pos_sp_triplet_time;
//
// MavlinkOrbSubscription *act_sub;
// uint64_t act_time;
//
// /* do not allow top copying this class */
// MavlinkStreamHILControls(MavlinkStreamHILControls &);
// MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &);
//
//protected:
// explicit MavlinkStreamHILControls() : MavlinkStream(),
// status_sub(nullptr),
// status_time(0),
// pos_sp_triplet_sub(nullptr),
// pos_sp_triplet_time(0),
// act_sub(nullptr),
// act_time(0)
// {}
//
// void subscribe()
// {
// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status));
// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
// act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
// }
//
// void send(const hrt_abstime t)
// {
// struct vehicle_status_s status;
// struct position_setpoint_triplet_s pos_sp_triplet;
// struct actuator_outputs_s act;
//
// bool updated = act_sub->update(&act_time, &act);
// updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
// updated |= status_sub->update(&status_time, &status);
//
// if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
// /* 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);
//
// float out[8];
//
// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
//
// /* scale outputs depending on system type */
// if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
// mavlink_system.type == MAV_TYPE_HEXAROTOR ||
// mavlink_system.type == MAV_TYPE_OCTOROTOR) {
// /* multirotors: set number of rotor outputs depending on 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;
// }
//
// for (unsigned i = 0; i < 8; i++) {
// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
// if (i < n) {
// /* scale PWM out 900..2100 us to 0..1 for rotors */
// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
//
// } else {
// /* scale PWM out 900..2100 us to -1..1 for other channels */
// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
// }
//
// } else {
// /* send 0 when disarmed */
// out[i] = 0.0f;
// }
// }
//
// } else {
// /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
//
// for (unsigned i = 0; i < 8; i++) {
// if (i != 3) {
// /* scale 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 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() const
// {
// return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "GLOBAL_POSITION_SETPOINT_INT";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamGlobalPositionSetpointInt();
// }
//
//private:
// MavlinkOrbSubscription *pos_sp_triplet_sub;
//
// /* do not allow top copying this class */
// MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
// MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
//
//protected:
// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(),
// pos_sp_triplet_sub(nullptr)
// {}
//
// void subscribe()
// {
// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
// }
//
// void send(const hrt_abstime t)
// {
// struct position_setpoint_triplet_s pos_sp_triplet;
//
// if (pos_sp_triplet_sub->update(&pos_sp_triplet)) {
// 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() const
// {
// return MavlinkStreamLocalPositionSetpoint::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "LOCAL_POSITION_SETPOINT";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamLocalPositionSetpoint();
// }
//
//private:
// MavlinkOrbSubscription *pos_sp_sub;
// uint64_t pos_sp_time;
//
// /* do not allow top copying this class */
// MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &);
// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &);
//
//protected:
// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
// pos_sp_sub(nullptr),
// pos_sp_time(0)
// {}
//
// void subscribe()
// {
// pos_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
// }
//
// void send(const hrt_abstime t)
// {
// struct vehicle_local_position_setpoint_s pos_sp;
//
// if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
// 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() const
// {
// return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "ROLL_PITCH_YAW_THRUST_SETPOINT";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamRollPitchYawThrustSetpoint();
// }
//
//private:
// MavlinkOrbSubscription *att_sp_sub;
// uint64_t att_sp_time;
//
// /* do not allow top copying this class */
// MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &);
// MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
//
//protected:
// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
// att_sp_sub(nullptr),
// att_sp_time(0)
// {}
//
// void subscribe()
// {
// att_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
// }
//
// void send(const hrt_abstime t)
// {
// struct vehicle_attitude_setpoint_s att_sp;
//
// if (att_sp_sub->update(&att_sp_time, &att_sp)) {
// 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() const
// {
// return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
// }
//
//private:
// MavlinkOrbSubscription *att_rates_sp_sub;
// uint64_t att_rates_sp_time;
//
// /* do not allow top copying this class */
// MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &);
// MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &);
//
//protected:
// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
// att_rates_sp_sub(nullptr),
// att_rates_sp_time(0)
// {}
//
// void subscribe()
// {
// att_rates_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
// }
//
// void send(const hrt_abstime t)
// {
// struct vehicle_rates_setpoint_s att_rates_sp;
//
// if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
// 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() const
// {
// return MavlinkStreamRCChannelsRaw::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "RC_CHANNELS_RAW";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamRCChannelsRaw();
// }
//
//private:
// MavlinkOrbSubscription *rc_sub;
// uint64_t rc_time;
//
// /* do not allow top copying this class */
// MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &);
// MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &);
//
//protected:
// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
// rc_sub(nullptr),
// rc_time(0)
// {}
//
// void subscribe()
// {
// rc_sub = _mavlink->add_orb_subscription(ORB_ID(input_rc));
// }
//
// void send(const hrt_abstime t)
// {
// struct rc_input_values rc;
//
// if (rc_sub->update(&rc_time, &rc)) {
// const unsigned port_width = 8;
//
// // Deprecated message (but still needed for compatibility!)
// 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);
// }
//
// // New message
// mavlink_msg_rc_channels_send(_channel,
// rc.timestamp_publication / 1000,
// rc.channel_count,
// ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
// ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
// ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
// ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
// ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
// ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
// ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
// ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
// ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
// ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
// ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
// ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
// ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
// ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
// ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
// ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
// ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
// ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
// rc.rssi);
// }
// }
//};
//
//
//class MavlinkStreamManualControl : public MavlinkStream
//{
//public:
// const char *get_name() const
// {
// return MavlinkStreamManualControl::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "MANUAL_CONTROL";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_MANUAL_CONTROL;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamManualControl();
// }
//
//private:
// MavlinkOrbSubscription *manual_sub;
// uint64_t manual_time;
//
// /* do not allow top copying this class */
// MavlinkStreamManualControl(MavlinkStreamManualControl &);
// MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &);
//
//protected:
// explicit MavlinkStreamManualControl() : MavlinkStream(),
// manual_sub(nullptr),
// manual_time(0)
// {}
//
// void subscribe()
// {
// manual_sub = _mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
// }
//
// void send(const hrt_abstime t)
// {
// struct manual_control_setpoint_s manual;
//
// if (manual_sub->update(&manual_time, &manual)) {
// mavlink_msg_manual_control_send(_channel,
// mavlink_system.sysid,
// manual.x * 1000,
// manual.y * 1000,
// manual.z * 1000,
// manual.r * 1000,
// 0);
// }
// }
//};
//
//
//class MavlinkStreamOpticalFlow : public MavlinkStream
//{
//public:
// const char *get_name() const
// {
// return MavlinkStreamOpticalFlow::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "OPTICAL_FLOW";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_OPTICAL_FLOW;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamOpticalFlow();
// }
//
//private:
// MavlinkOrbSubscription *flow_sub;
// uint64_t flow_time;
//
// /* do not allow top copying this class */
// MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
// MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
//
//protected:
// explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
// flow_sub(nullptr),
// flow_time(0)
// {}
//
// void subscribe()
// {
// flow_sub = _mavlink->add_orb_subscription(ORB_ID(optical_flow));
// }
//
// void send(const hrt_abstime t)
// {
// struct optical_flow_s flow;
//
// if (flow_sub->update(&flow_time, &flow)) {
// 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() const
// {
// return MavlinkStreamAttitudeControls::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "ATTITUDE_CONTROLS";
// }
//
// uint8_t get_id()
// {
// return 0;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamAttitudeControls();
// }
//
//private:
// MavlinkOrbSubscription *att_ctrl_sub;
// uint64_t att_ctrl_time;
//
// /* do not allow top copying this class */
// MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &);
// MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &);
//
//protected:
// explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
// att_ctrl_sub(nullptr),
// att_ctrl_time(0)
// {}
//
// void subscribe()
// {
// att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
// }
//
// void send(const hrt_abstime t)
// {
// struct actuator_controls_s att_ctrl;
//
// if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) {
// /* 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() const
// {
// return MavlinkStreamNamedValueFloat::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "NAMED_VALUE_FLOAT";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamNamedValueFloat();
// }
//
//private:
// MavlinkOrbSubscription *debug_sub;
// uint64_t debug_time;
//
// /* do not allow top copying this class */
// MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &);
// MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &);
//
//protected:
// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
// debug_sub(nullptr),
// debug_time(0)
// {}
//
// void subscribe()
// {
// debug_sub = _mavlink->add_orb_subscription(ORB_ID(debug_key_value));
// }
//
// void send(const hrt_abstime t)
// {
// struct debug_key_value_s debug;
//
// if (debug_sub->update(&debug_time, &debug)) {
// /* 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() const
// {
// return MavlinkStreamCameraCapture::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "CAMERA_CAPTURE";
// }
//
// uint8_t get_id()
// {
// return 0;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamCameraCapture();
// }
//
//private:
// MavlinkOrbSubscription *status_sub;
//
// /* do not allow top copying this class */
// MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &);
// MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &);
//
//protected:
// explicit MavlinkStreamCameraCapture() : MavlinkStream(),
// status_sub(nullptr)
// {}
//
// void subscribe()
// {
// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status));
// }
//
// void send(const hrt_abstime t)
// {
// struct vehicle_status_s status;
// (void)status_sub->update(&status);
//
// 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() const
// {
// return MavlinkStreamDistanceSensor::get_name_static();
// }
//
// static const char *get_name_static()
// {
// return "DISTANCE_SENSOR";
// }
//
// uint8_t get_id()
// {
// return MAVLINK_MSG_ID_DISTANCE_SENSOR;
// }
//
// static MavlinkStream *new_instance(Mavlink *mavlink)
// {
// return new MavlinkStreamDistanceSensor();
// }
//
//private:
// MavlinkOrbSubscription *range_sub;
// uint64_t range_time;
//
// /* do not allow top copying this class */
// MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &);
// MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &);
//
//protected:
// explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
// range_sub(nullptr),
// range_time(0)
// {}
//
// void subscribe()
// {
// range_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
// }
//
// void send(const hrt_abstime t)
// {
// struct range_finder_report range;
//
// if (range_sub->update(&range_time, &range)) {
//
// 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);
// }
// }
//};
StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
// new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
// new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
// new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
// new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
// new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
// new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
// new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
// new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
// new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
// new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
// new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
// new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
// new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
// new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
// new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
// new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
// new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
// 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(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
nullptr
};