diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 3186 |
1 files changed, 1602 insertions, 1584 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fe33985d2..7dc3ebac1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -76,7 +76,7 @@ #include <systemlib/err.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, @@ -254,6 +254,15 @@ public: 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; @@ -291,6 +300,7 @@ protected: } 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); @@ -298,7 +308,7 @@ protected: msg.autopilot = mavlink_system.type; msg.mavlink_version = 3; - _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 1); + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); } }; @@ -311,7 +321,7 @@ public: return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static () + static const char *get_name_static() { return "SYS_STATUS"; } @@ -321,1618 +331,1626 @@ public: return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance() - { - return new MavlinkStreamSysStatus(); - } - -private: - MavlinkOrbSubscription *status_sub; - - /* do not allow top copying this class */ - MavlinkStreamSysStatus(MavlinkStreamSysStatus &); - MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); - -protected: - explicit MavlinkStreamSysStatus() : MavlinkStream(), - status_sub(nullptr) - {} - - 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; - - /* do not allow top copying this class */ - MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); - MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); - -protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), - sensor_sub(nullptr), - 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; - - /* do not allow top copying this class */ - MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); - - -protected: - explicit MavlinkStreamAttitude() : MavlinkStream(), - att_sub(nullptr), - 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; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); - MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); - -protected: - explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), - att_sub(nullptr), - 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; - - /* do not allow top copying this class */ - MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); - MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); - -protected: - explicit MavlinkStreamVFRHUD() : MavlinkStream(), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - 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; - - /* do not allow top copying this class */ - MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); - MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); - -protected: - explicit MavlinkStreamGPSRawInt() : MavlinkStream(), - gps_sub(nullptr), - 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; - - /* do not allow top copying this class */ - MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); - MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); - -protected: - explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0), - home_sub(nullptr), - home_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); - MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); - -protected: - explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); - -protected: - explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); - -protected: - explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), - home_sub(nullptr) - {} - - 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; - - /* do not allow top copying this class */ - MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); - MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); - -protected: - explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); - -protected: - explicit MavlinkStreamHILControls() : MavlinkStream(), - status_sub(nullptr), - status_time(0), - pos_sp_triplet_sub(nullptr), - pos_sp_triplet_time(0), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); - -protected: - explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), - pos_sp_triplet_sub(nullptr) - {} - - 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; - - /* do not allow top copying this class */ - MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); - MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); - -protected: - explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), - pos_sp_sub(nullptr), - pos_sp_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), - att_sp_sub(nullptr), - att_sp_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); - MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), - att_rates_sp_sub(nullptr), - att_rates_sp_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); - -protected: - explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), - rc_sub(nullptr), - rc_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamManualControl(MavlinkStreamManualControl &); - MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); - -protected: - explicit MavlinkStreamManualControl() : MavlinkStream(), - manual_sub(nullptr), - manual_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); - -protected: - explicit MavlinkStreamOpticalFlow() : MavlinkStream(), - flow_sub(nullptr), - flow_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); - MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); - -protected: - explicit MavlinkStreamAttitudeControls() : MavlinkStream(), - att_ctrl_sub(nullptr), - att_ctrl_time(0) - {} - - void subscribe(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; - - /* do not allow top copying this class */ - MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); - MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); - -protected: - explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), - debug_sub(nullptr), - debug_time(0) - {} - - void subscribe(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() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return 0; + return new MavlinkStreamSysStatus(mavlink); } - static MavlinkStream *new_instance() + unsigned get_size() { - return new MavlinkStreamCameraCapture(); + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: MavlinkOrbSubscription *status_sub; /* do not allow top copying this class */ - MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); - MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + MavlinkStreamSysStatus(MavlinkStreamSysStatus &); + MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: - explicit MavlinkStreamCameraCapture() : MavlinkStream(), + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), status_sub(nullptr) {} - void subscribe(Mavlink *mavlink) + void subscribe() { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + 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); + if (status_sub->update(&status)) { + mavlink_sys_status_t msg; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.load = status.load * 1000.0f; + msg.voltage_battery = status.battery_voltage * 1000.0f; + msg.current_battery = status.battery_current * 100.0f; + msg.drop_rate_comm = status.drop_rate_comm; + msg.errors_comm = status.errors_comm; + msg.errors_count1 = status.errors_count1; + msg.errors_count2 = status.errors_count2; + msg.errors_count3 = status.errors_count3; + msg.errors_count4 = status.errors_count4; + msg.battery_remaining = status.battery_remaining * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); } } }; -class 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; - - /* do not allow top copying this class */ - MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); - MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); - -protected: - explicit MavlinkStreamDistanceSensor() : MavlinkStream(), - range_sub(nullptr), - range_time(0) - {} - - void subscribe(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); - } - } -}; +//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; +// +// /* do not allow top copying this class */ +// MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); +// MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); +// +//protected: +// explicit MavlinkStreamHighresIMU() : MavlinkStream(), +// sensor_sub(nullptr), +// 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; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitude(MavlinkStreamAttitude &); +// MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); +// +// +//protected: +// explicit MavlinkStreamAttitude() : MavlinkStream(), +// att_sub(nullptr), +// 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; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); +// MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); +// +//protected: +// explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), +// att_sub(nullptr), +// 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; +// +// /* do not allow top copying this class */ +// MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); +// MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); +// +//protected: +// explicit MavlinkStreamVFRHUD() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0), +// pos_sub(nullptr), +// pos_time(0), +// armed_sub(nullptr), +// armed_time(0), +// act_sub(nullptr), +// act_time(0), +// airspeed_sub(nullptr), +// 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; +// +// /* do not allow top copying this class */ +// MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); +// MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); +// +//protected: +// explicit MavlinkStreamGPSRawInt() : MavlinkStream(), +// gps_sub(nullptr), +// 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; +// +// /* do not allow top copying this class */ +// MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); +// MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); +// +//protected: +// explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0), +// home_sub(nullptr), +// home_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); +// MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); +// +//protected: +// explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); +// MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); +// +//protected: +// explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); +// MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); +// +//protected: +// explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), +// home_sub(nullptr) +// {} +// +// 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; +// +// /* do not allow top copying this class */ +// MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); +// MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); +// +//protected: +// explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), +// act_sub(nullptr), +// act_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamHILControls(MavlinkStreamHILControls &); +// MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); +// +//protected: +// explicit MavlinkStreamHILControls() : MavlinkStream(), +// status_sub(nullptr), +// status_time(0), +// pos_sp_triplet_sub(nullptr), +// pos_sp_triplet_time(0), +// act_sub(nullptr), +// act_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); +// MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); +// +//protected: +// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), +// pos_sp_triplet_sub(nullptr) +// {} +// +// 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; +// +// /* do not allow top copying this class */ +// MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); +// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); +// +//protected: +// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), +// pos_sp_sub(nullptr), +// pos_sp_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); +// MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); +// +//protected: +// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), +// att_sp_sub(nullptr), +// att_sp_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); +// MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); +// +//protected: +// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), +// att_rates_sp_sub(nullptr), +// att_rates_sp_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); +// MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); +// +//protected: +// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), +// rc_sub(nullptr), +// rc_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamManualControl(MavlinkStreamManualControl &); +// MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); +// +//protected: +// explicit MavlinkStreamManualControl() : MavlinkStream(), +// manual_sub(nullptr), +// manual_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); +// MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); +// +//protected: +// explicit MavlinkStreamOpticalFlow() : MavlinkStream(), +// flow_sub(nullptr), +// flow_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); +// MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); +// +//protected: +// explicit MavlinkStreamAttitudeControls() : MavlinkStream(), +// att_ctrl_sub(nullptr), +// att_ctrl_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); +// MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); +// +//protected: +// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), +// debug_sub(nullptr), +// debug_time(0) +// {} +// +// void subscribe(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; +// +// /* do not allow top copying this class */ +// MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); +// MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); +// +//protected: +// explicit MavlinkStreamCameraCapture() : MavlinkStream(), +// status_sub(nullptr) +// {} +// +// 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; +// +// /* do not allow top copying this class */ +// MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); +// MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); +// +//protected: +// explicit MavlinkStreamDistanceSensor() : MavlinkStream(), +// range_sub(nullptr), +// range_time(0) +// {} +// +// void subscribe(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), +// 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 }; |