diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 1796 |
1 files changed, 1796 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp new file mode 100644 index 000000000..7c864f127 --- /dev/null +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -0,0 +1,1796 @@ +/**************************************************************************** + * + * 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_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; + + 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() + { + return new MavlinkStreamHeartbeat(); + } + +private: + MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *pos_sp_triplet_sub; + +protected: + void subscribe(Mavlink *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)); + } + + 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() 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() + { + return new MavlinkStreamSysStatus(); + } + +private: + MavlinkOrbSubscription *status_sub; + +protected: + void subscribe(Mavlink *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_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 * 100.0f, + status.battery_remaining * 100.0f, + status.drop_rate_comm, + status.errors_comm, + status.errors_count1, + status.errors_count2, + status.errors_count3, + status.errors_count4); + } + } +}; + + +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() + { + return new MavlinkStreamHighresIMU(); + } + +private: + MavlinkOrbSubscription *sensor_sub; + uint64_t sensor_time; + + uint64_t accel_timestamp; + uint64_t gyro_timestamp; + uint64_t mag_timestamp; + uint64_t baro_timestamp; + +protected: + explicit MavlinkStreamHighresIMU() : MavlinkStream(), + sensor_time(0), + accel_timestamp(0), + gyro_timestamp(0), + mag_timestamp(0), + baro_timestamp(0) + {} + + void subscribe(Mavlink *mavlink) + { + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); + } + + 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_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() 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() + { + return new MavlinkStreamAttitude(); + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + +protected: + explicit MavlinkStreamAttitude() : MavlinkStream(), + att_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_s att; + + if (att_sub->update(&att_time, &att)) { + 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() 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() + { + return new MavlinkStreamAttitudeQuaternion(); + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + +protected: + explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), + att_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_s att; + + if (att_sub->update(&att_time, &att)) { + 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() 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() + { + return new MavlinkStreamVFRHUD(); + } + +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; + +protected: + explicit MavlinkStreamVFRHUD() : MavlinkStream(), + att_time(0), + pos_time(0), + armed_time(0), + act_time(0), + airspeed_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); + armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); + } + + 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) { + 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() 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() + { + return new MavlinkStreamGPSRawInt(); + } + +private: + MavlinkOrbSubscription *gps_sub; + uint64_t gps_time; + +protected: + explicit MavlinkStreamGPSRawInt() : MavlinkStream(), + gps_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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_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), + cm_uint16_from_m_float(gps.epv), + gps.vel_m_s * 100.0f, + _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps.satellites_used); + } + } +}; + + +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() + { + return new MavlinkStreamGlobalPositionInt(); + } + +private: + MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; + + MavlinkOrbSubscription *home_sub; + uint64_t home_time; + +protected: + explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), + pos_time(0), + home_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamLocalPositionNED(); + } + +private: + MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; + +protected: + explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), + pos_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; + +protected: + explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), + pos_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamGPSGlobalOrigin(); + } + +private: + MavlinkOrbSubscription *home_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamServoOutputRaw<N>(); + } + +private: + MavlinkOrbSubscription *act_sub; + uint64_t act_time; + +protected: + explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), + act_time(0) + {} + + 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]); + } + + 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() + { + 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; + +protected: + explicit MavlinkStreamHILControls() : MavlinkStream(), + status_time(0), + pos_sp_triplet_time(0), + act_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamGlobalPositionSetpointInt(); + } + +private: + MavlinkOrbSubscription *pos_sp_triplet_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamLocalPositionSetpoint(); + } + +private: + MavlinkOrbSubscription *pos_sp_sub; + uint64_t pos_sp_time; + +protected: + explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), + pos_sp_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamRollPitchYawThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_sp_sub; + uint64_t att_sp_time; + +protected: + explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), + att_sp_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_rates_sp_sub; + uint64_t att_rates_sp_time; + +protected: + explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), + att_rates_sp_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamRCChannelsRaw(); + } + +private: + MavlinkOrbSubscription *rc_sub; + uint64_t rc_time; + +protected: + explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), + rc_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamManualControl(); + } + +private: + MavlinkOrbSubscription *manual_sub; + uint64_t manual_time; + +protected: + explicit MavlinkStreamManualControl() : MavlinkStream(), + manual_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamOpticalFlow(); + } + +private: + MavlinkOrbSubscription *flow_sub; + uint64_t flow_time; + +protected: + explicit MavlinkStreamOpticalFlow() : MavlinkStream(), + flow_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamAttitudeControls(); + } + +private: + MavlinkOrbSubscription *att_ctrl_sub; + uint64_t att_ctrl_time; + +protected: + explicit MavlinkStreamAttitudeControls() : MavlinkStream(), + att_ctrl_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamNamedValueFloat(); + } + +private: + MavlinkOrbSubscription *debug_sub; + uint64_t debug_time; + +protected: + explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), + debug_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamCameraCapture(); + } + +private: + MavlinkOrbSubscription *status_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + 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() + { + return new MavlinkStreamDistanceSensor(); + } + +private: + MavlinkOrbSubscription *range_sub; + uint64_t range_time; + +protected: + explicit MavlinkStreamDistanceSensor() : MavlinkStream(), + range_time(0) + {} + + void subscribe(Mavlink *mavlink) + { + 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(&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 +}; |