diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 1466 |
1 files changed, 917 insertions, 549 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7c864f127..683f0f1e8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -40,9 +40,9 @@ */ #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> @@ -72,11 +72,11 @@ #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, @@ -249,21 +249,33 @@ public: return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHeartbeat(); + 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; + MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; + + /* do not allow top copying this class */ + MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); 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)); - } + 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) { @@ -271,30 +283,150 @@ protected: 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 (!_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 (!_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_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; - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + msg.severity = logmsg.severity; + 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 { @@ -304,7 +436,7 @@ public: return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static () + static const char *get_name_static() { return "SYS_STATUS"; } @@ -314,39 +446,50 @@ public: return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamSysStatus(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamSysStatus(); + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamSysStatus(MavlinkStreamSysStatus &); + MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } + 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_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); + 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); } } }; @@ -370,73 +513,89 @@ public: return MAVLINK_MSG_ID_HIGHRES_IMU; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHighresIMU(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamHighresIMU(); + return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; + 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() : MavlinkStream(), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) + 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 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)) { + if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor.accelerometer_timestamp) { + 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; + _accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor.magnetometer_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; + _mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor.baro_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; + _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); + 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); } } }; @@ -460,34 +619,47 @@ public: return MAVLINK_MSG_ID_ATTITUDE; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitude(); + 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; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitude(MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + protected: - explicit MavlinkStreamAttitude() : MavlinkStream(), - att_time(0) + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _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); + 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); } } }; @@ -511,39 +683,47 @@ public: return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamAttitudeQuaternion(); + 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; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); protected: - explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), - att_time(0) + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _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); + 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); } } }; @@ -568,45 +748,50 @@ public: return MAVLINK_MSG_ID_VFR_HUD; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamVFRHUD(); + 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 *_att_sub; + uint64_t _att_time; + + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_armed_sub; + uint64_t _armed_time; - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_airspeed_sub; + uint64_t _airspeed_time; - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; + /* do not allow top copying this class */ + MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); protected: - explicit MavlinkStreamVFRHUD() : MavlinkStream(), - att_time(0), - pos_time(0), - armed_time(0), - act_time(0), - airspeed_time(0) + 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 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; @@ -615,24 +800,23 @@ protected: 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); + 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); + 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); } } }; @@ -656,41 +840,49 @@ public: return MAVLINK_MSG_ID_GPS_RAW_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGPSRawInt(); + 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; + MavlinkOrbSubscription *_gps_sub; + uint64_t _gps_time; + + /* do not allow top copying this class */ + MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); protected: - explicit MavlinkStreamGPSRawInt() : MavlinkStream(), - gps_time(0) + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), + _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); + 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); } } }; @@ -714,49 +906,57 @@ public: return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGlobalPositionInt(); + return new MavlinkStreamGlobalPositionInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + MavlinkOrbSubscription *_home_sub; + uint64_t _home_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_time(0), - home_time(0) + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _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); + 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); + mavlink_global_position_int_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.lat = pos.lat * 1e7; + msg.lon = pos.lon * 1e7; + msg.alt = pos.alt * 1000.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } } }; @@ -780,44 +980,51 @@ public: return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNED(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamLocalPositionNED(); + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + 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_time(0) + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _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); + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_local_position_ned_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } } }; - class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: @@ -836,38 +1043,46 @@ public: return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamViconPositionEstimate(); + return new MavlinkStreamViconPositionEstimate(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + 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_time(0) + explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), + _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); + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_vicon_position_estimate_t msg; + + msg.usec = pos.timestamp; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } } }; @@ -891,37 +1106,49 @@ public: return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGPSGlobalOrigin(); + return new MavlinkStreamGPSGlobalOrigin(mavlink); + } + + unsigned get_size() + { + return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *home_sub; + MavlinkOrbSubscription *_home_sub; + + /* do not allow top copying this class */ + MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); protected: - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } + explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(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()) { + 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); + if (_home_sub->update(&home)) { + mavlink_gps_global_origin_t msg; + + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); } } } }; + template <int N> class MavlinkStreamServoOutputRaw : public MavlinkStream { @@ -953,21 +1180,28 @@ public: } } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamServoOutputRaw<N>(); + return new MavlinkStreamServoOutputRaw<N>(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; -protected: - explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), - act_time(0) - {} + /* do not allow top copying this class */ + MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); - void subscribe(Mavlink *mavlink) +protected: + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _act_sub(nullptr), + _act_time(0) { orb_id_t act_topics[] = { ORB_ID(actuator_outputs_0), @@ -976,25 +1210,28 @@ protected: ORB_ID(actuator_outputs_3) }; - act_sub = mavlink->add_orb_subscription(act_topics[N]); + _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]); + if (_act_sub->update(&_act_time, &act)) { + mavlink_servo_output_raw_t msg; + + msg.time_usec = act.timestamp; + msg.port = N; + msg.servo1_raw = act.output[0]; + msg.servo2_raw = act.output[1]; + msg.servo3_raw = act.output[2]; + msg.servo4_raw = act.output[3]; + msg.servo5_raw = act.output[4]; + msg.servo6_raw = act.output[5]; + msg.servo7_raw = act.output[6]; + msg.servo8_raw = act.output[7]; + + _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); } } }; @@ -1018,44 +1255,49 @@ public: return MAVLINK_MSG_ID_HIL_CONTROLS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHILControls(); + return new MavlinkStreamHILControls(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; - uint64_t status_time; + MavlinkOrbSubscription *_status_sub; + uint64_t _status_time; + + MavlinkOrbSubscription *_pos_sp_triplet_sub; + uint64_t _pos_sp_triplet_time; - MavlinkOrbSubscription *pos_sp_triplet_sub; - uint64_t pos_sp_triplet_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_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_time(0), - pos_sp_triplet_time(0), - act_time(0) + explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _status_time(0), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _pos_sp_triplet_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_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); + 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 */ @@ -1123,11 +1365,21 @@ protected: } } - 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); + mavlink_hil_controls_t msg; + + msg.time_usec = hrt_absolute_time(); + msg.roll_ailerons = out[0]; + msg.pitch_elevator = out[1]; + msg.yaw_rudder = out[2]; + msg.throttle = out[3]; + msg.aux1 = out[4]; + msg.aux2 = out[5]; + msg.aux3 = out[6]; + msg.aux4 = out[7]; + msg.mode = mavlink_base_mode; + msg.nav_mode = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); } } }; @@ -1151,31 +1403,42 @@ public: return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGlobalPositionSetpointInt(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamGlobalPositionSetpointInt(); + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; + + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); protected: - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } + explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(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)); + if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { + mavlink_global_position_setpoint_int_t msg; + + msg.coordinate_frame = MAV_FRAME_GLOBAL; + msg.latitude = pos_sp_triplet.current.lat * 1e7; + msg.longitude = pos_sp_triplet.current.lon * 1e7; + msg.altitude = pos_sp_triplet.current.alt * 1000; + msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg); } } }; @@ -1199,36 +1462,44 @@ public: return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionSetpoint(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamLocalPositionSetpoint(); + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *pos_sp_sub; - uint64_t pos_sp_time; + 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_time(0) + explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), + _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); + if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { + mavlink_local_position_setpoint_t msg; + + msg.coordinate_frame = MAV_FRAME_LOCAL_NED; + msg.x = pos_sp.x; + msg.y = pos_sp.y; + msg.z = pos_sp.z; + msg.yaw = pos_sp.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg); } } }; @@ -1252,36 +1523,44 @@ public: return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamRollPitchYawThrustSetpoint(); + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } 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(), + explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), 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); + mavlink_roll_pitch_yaw_thrust_setpoint_t msg; + + msg.time_boot_ms = att_sp.timestamp / 1000; + msg.roll = att_sp.roll_body; + msg.pitch = att_sp.pitch_body; + msg.yaw = att_sp.yaw_body; + msg.thrust = att_sp.thrust; + + _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg); } } }; @@ -1305,36 +1584,44 @@ public: return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_rates_sp_sub; - uint64_t att_rates_sp_time; + 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_time(0) + explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _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); + if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) { + mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg; + + msg.time_boot_ms = att_rates_sp.timestamp / 1000; + msg.roll_rate = att_rates_sp.roll; + msg.pitch_rate = att_rates_sp.pitch; + msg.yaw_rate = att_rates_sp.yaw; + msg.thrust = att_rates_sp.thrust; + + _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg); } } }; @@ -1358,72 +1645,84 @@ public: return MAVLINK_MSG_ID_RC_CHANNELS_RAW; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamRCChannelsRaw(); + return new MavlinkStreamRCChannelsRaw(mavlink); + } + + unsigned get_size() + { + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *rc_sub; - uint64_t rc_time; + 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_time(0) + explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), + _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)) { + if (_rc_sub->update(&_rc_time, &rc)) { const unsigned port_width = 8; - // Deprecated message (but still needed for compatibility!) + /* 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); + /* channels are sent in MAVLink main loop at a fixed interval */ + mavlink_rc_channels_raw_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.port = i; + msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); } - // 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); + /* new message */ + mavlink_rc_channels_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.chancount = rc.channel_count; + msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX; + msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX; + msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX; + msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX; + msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX; + msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX; + msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX; + msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX; + msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; + msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; + msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } } }; @@ -1447,37 +1746,45 @@ public: return MAVLINK_MSG_ID_MANUAL_CONTROL; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamManualControl(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamManualControl(); + return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *manual_sub; - uint64_t manual_time; + 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_time(0) + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), + _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))), + _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); + if (_manual_sub->update(&_manual_time, &manual)) { + mavlink_manual_control_t msg; + + msg.target = mavlink_system.sysid; + msg.x = manual.x * 1000; + msg.y = manual.y * 1000; + msg.z = manual.z * 1000; + msg.r = manual.r * 1000; + msg.buttons = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); } } }; @@ -1501,37 +1808,47 @@ public: return MAVLINK_MSG_ID_OPTICAL_FLOW; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamOpticalFlow(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamOpticalFlow(); + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *flow_sub; - uint64_t flow_time; + 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_time(0) + explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), + _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); + if (_flow_sub->update(&_flow_time, &flow)) { + mavlink_optical_flow_t msg; + + msg.time_usec = flow.timestamp; + msg.sensor_id = flow.sensor_id; + msg.flow_x = flow.flow_raw_x; + msg.flow_y = flow.flow_raw_y; + msg.flow_comp_m_x = flow.flow_comp_x_m; + msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.quality = flow.quality; + msg.ground_distance = flow.ground_distance_m; + + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); } } }; @@ -1554,51 +1871,64 @@ public: return 0; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeControls(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamAttitudeControls(); + return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } private: - MavlinkOrbSubscription *att_ctrl_sub; - uint64_t att_ctrl_time; + 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_time(0) + explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)), + _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)) { + 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]); + mavlink_named_value_float_t msg; + + msg.time_boot_ms = att_ctrl.timestamp / 1000; + + snprintf(msg.name, sizeof(msg.name), "rll ctrl"); + msg.value = att_ctrl.control[0]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "ptch ctrl"); + msg.value = att_ctrl.control[1]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "yaw ctrl"); + msg.value = att_ctrl.control[2]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "thr ctrl"); + msg.value = att_ctrl.control[3]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); } } }; + class MavlinkStreamNamedValueFloat : public MavlinkStream { public: @@ -1617,41 +1947,49 @@ public: return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamNamedValueFloat(); + return new MavlinkStreamNamedValueFloat(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *debug_sub; - uint64_t debug_time; + 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_time(0) + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), + _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))), + _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)) { + if (_debug_sub->update(&_debug_time, &debug)) { + mavlink_named_value_float_t msg; + + msg.time_boot_ms = debug.timestamp_ms; + memcpy(msg.name, debug.key, sizeof(msg.name)); /* enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; + msg.name[sizeof(msg.name) - 1] = '\0'; + msg.value = debug.value; - mavlink_msg_named_value_float_send(_channel, - debug.timestamp_ms, - debug.key, - debug.value); + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); } } }; + class MavlinkStreamCameraCapture : public MavlinkStream { public: @@ -1670,38 +2008,53 @@ public: return 0; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamCameraCapture(); + return new MavlinkStreamCameraCapture(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } + explicit MavlinkStreamCameraCapture(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; - (void)status_sub->update(&status); + (void)_status_sub->update(&status); - if (status.arming_state == ARMING_STATE_ARMED - || status.arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_command_long_t msg; - /* 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); + msg.target_system = mavlink_system.sysid; + msg.target_component = MAV_COMP_ID_ALL; + msg.command = MAV_CMD_DO_CONTROL_VIDEO; + msg.confirmation = 0; + msg.param1 = 0; + msg.param2 = 0; + msg.param3 = 0; + /* set camera capture ON/OFF depending on arming state */ + msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param5 = 0; + msg.param6 = 0; + msg.param7 = 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); - } + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); } }; + class MavlinkStreamDistanceSensor : public MavlinkStream { public: @@ -1720,45 +2073,58 @@ public: return MAVLINK_MSG_ID_DISTANCE_SENSOR; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamDistanceSensor(); + return new MavlinkStreamDistanceSensor(mavlink); + } + + unsigned get_size() + { + return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *range_sub; - uint64_t range_time; + 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_time(0) + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), + _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), + _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)) { + if (_range_sub->update(&_range_time, &range)) { + + mavlink_distance_sensor_t msg; - uint8_t type; + msg.time_boot_ms = range.timestamp / 1000; switch (range.type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; + case RANGE_FINDER_TYPE_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + default: + msg.type = MAV_DISTANCE_SENSOR_LASER; break; } - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; + msg.id = 0; + msg.orientation = 0; + msg.min_distance = range.minimum_distance * 100; + msg.max_distance = range.minimum_distance * 100; + msg.current_distance = range.distance * 100; + msg.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); + _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); } } }; @@ -1766,6 +2132,8 @@ protected: 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), @@ -1774,6 +2142,7 @@ StreamListItem *streams_list[] = { 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(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::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), @@ -1791,6 +2160,5 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; |