diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-13 16:04:02 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-16 14:00:58 +0200 |
commit | fbb3adde06e5ecf88a4c39e332a539fa12d173b3 (patch) | |
tree | 4871f0791f891ec2d9a5385579af61fe1fd1bec2 /src/modules/mavlink/mavlink_messages.cpp | |
parent | a72015c260dbb4e70d23c35120269cef61a439cc (diff) | |
download | px4-firmware-fbb3adde06e5ecf88a4c39e332a539fa12d173b3.tar.gz px4-firmware-fbb3adde06e5ecf88a4c39e332a539fa12d173b3.tar.bz2 px4-firmware-fbb3adde06e5ecf88a4c39e332a539fa12d173b3.zip |
mavlink app: Clean up allocations
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 1928 |
1 files changed, 987 insertions, 941 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 79dd88657..4bb827116 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -74,6 +74,8 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_range_finder.h> +#include <systemlib/err.h> + #include "mavlink_messages.h" @@ -189,42 +191,51 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set class MavlinkStreamHeartbeat : public MavlinkStream { public: - const char *get_name() + + ~MavlinkStreamHeartbeat() {}; + + const char *get_name() const + { + return MavlinkStreamHeartbeat::get_name_static(); + } + + static const char *get_name_static() { return "HEARTBEAT"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); } private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; + protected: + + explicit MavlinkStreamHeartbeat() {}; + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { - (void)status_sub->update(t); - (void)pos_sp_triplet_sub->update(t); + struct vehicle_status_s status; + struct position_setpoint_triplet_s pos_sp_triplet; + + (void)status_sub->update(t, &status); + (void)pos_sp_triplet_sub->update(t, &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); + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); mavlink_msg_heartbeat_send(_channel, mavlink_system.type, @@ -240,12 +251,19 @@ protected: class MavlinkStreamSysStatus : public MavlinkStream { public: - const char *get_name() + ~MavlinkStreamSysStatus() {}; + + const char *get_name() const + { + return MavlinkStreamSysStatus::get_name_static(); + } + + static const char *get_name_static () { return "SYS_STATUS"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); } @@ -255,29 +273,31 @@ private: struct vehicle_status_s *status; protected: + explicit MavlinkStreamSysStatus() {}; + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); } void send(const hrt_abstime t) { - status_sub->update(t); + struct vehicle_status_s status; + (void)status_sub->update(t, &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 * 1000.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); + status.onboard_control_sensors_present, + status.onboard_control_sensors_enabled, + status.onboard_control_sensors_health, + status.load * 1000.0f, + status.battery_voltage * 1000.0f, + status.battery_current * 1000.0f, + status.battery_remaining * 100.0f, + status.drop_rate_comm, + status.errors_comm, + status.errors_count1, + status.errors_count2, + status.errors_count3, + status.errors_count4); } }; @@ -285,23 +305,25 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) + ~MavlinkStreamHighresIMU(); + + const char *get_name() const { + return MavlinkStreamHighresIMU::get_name_static(); } - const char *get_name() + static const char *get_name_static() { return "HIGHRES_IMU"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); } private: MavlinkOrbSubscription *sensor_sub; - struct sensor_combined_s *sensor; uint64_t accel_timestamp; uint64_t gyro_timestamp; @@ -309,48 +331,52 @@ private: uint64_t baro_timestamp; protected: + explicit MavlinkStreamHighresIMU() : MavlinkStream(), 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)); - sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } void send(const hrt_abstime t) { - if (sensor_sub->update(t)) { + struct sensor_combined_s sensor; + if (sensor_sub->update(t, &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, + 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); } } @@ -360,12 +386,17 @@ protected: class MavlinkStreamAttitude : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamAttitude::get_name_static(); + } + + static const char *get_name_static() { return "ATTITUDE"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); } @@ -378,16 +409,17 @@ protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); } void send(const hrt_abstime t) { - if (att_sub->update(t)) { + struct vehicle_attitude_s att; + + if (att_sub->update(t, &att)) { mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + att.timestamp / 1000, + att.roll, att.pitch, att.yaw, + att.rollspeed, att.pitchspeed, att.yawspeed); } } }; @@ -396,39 +428,44 @@ protected: class MavlinkStreamAttitudeQuaternion : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamAttitudeQuaternion::get_name_static(); + } + + static const char *get_name_static() { return "ATTITUDE_QUATERNION"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); } private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); } void send(const hrt_abstime t) { - if (att_sub->update(t)) { + struct vehicle_attitude_s att; + + if (att_sub->update(t, &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); + att.timestamp / 1000, + att.q[0], + att.q[1], + att.q[2], + att.q[3], + att.rollspeed, + att.pitchspeed, + att.yawspeed); } } }; @@ -437,12 +474,18 @@ protected: class MavlinkStreamVFRHUD : public MavlinkStream { public: - const char *get_name() + + const char *get_name() const + { + return MavlinkStreamVFRHUD::get_name_static(); + } + + static const char *get_name_static() { return "VFR_HUD"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); } @@ -467,41 +510,38 @@ protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - armed = (struct actuator_armed_s *)armed_sub->get_data(); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - act = (struct actuator_controls_s *)act_sub->get_data(); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } void send(const hrt_abstime t) { - bool updated = att_sub->update(t); - updated |= pos_sub->update(t); - updated |= armed_sub->update(t); - updated |= act_sub->update(t); - updated |= airspeed_sub->update(t); + 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(t, &att); + updated |= pos_sub->update(t, &pos); + updated |= armed_sub->update(t, &armed); + updated |= act_sub->update(t, &act); + updated |= airspeed_sub->update(t, &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; + 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, + airspeed.true_airspeed_m_s, groundspeed, heading, throttle, - pos->alt, - -pos->vel_d); + pos.alt, + pos.vel_d); } } }; @@ -510,12 +550,17 @@ protected: class MavlinkStreamGPSRawInt : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamGPSRawInt::get_name_static(); + } + + static const char *get_name_static() { return "GPS_RAW_INT"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); } @@ -528,864 +573,865 @@ protected: void subscribe(Mavlink *mavlink) { gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (gps_sub->update(t)) { - mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); - } - } -}; - - -class MavlinkStreamGlobalPositionInt : public MavlinkStream -{ -public: - const char *get_name() - { - return "GLOBAL_POSITION_INT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionInt(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_global_position_s *pos; - - MavlinkOrbSubscription *home_sub; - struct home_position_s *home; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - home = (struct home_position_s *)home_sub->get_data(); - } - - void send(const hrt_abstime t) - { - bool updated = pos_sub->update(t); - updated |= home_sub->update(t); - - if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); - } - } -}; - - -class MavlinkStreamLocalPositionNED : public MavlinkStream -{ -public: - const char *get_name() - { - return "LOCAL_POSITION_NED"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionNED(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_local_position_s *pos; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - pos = (struct vehicle_local_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); - } - } -}; - - - -class MavlinkStreamViconPositionEstimate : public MavlinkStream -{ -public: - const char *get_name() - { - return "VICON_POSITION_ESTIMATE"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_vicon_position_s *pos; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); - } - } -}; - - -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -{ -public: - const char *get_name() - { - return "GPS_GLOBAL_ORIGIN"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGPSGlobalOrigin(); - } - -private: - MavlinkOrbSubscription *home_sub; - struct home_position_s *home; - -protected: - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - home = (struct home_position_s *)home_sub->get_data(); - } - - void send(const hrt_abstime t) - { - - /* we're sending the GPS home periodically to ensure the - * the GCS does pick it up at one point */ - if (home_sub->is_published()) { - home_sub->update(t); - - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home->lat * 1e7), - (int32_t)(home->lon * 1e7), - (int32_t)(home->alt) * 1000.0f); - } - } -}; - - -class MavlinkStreamServoOutputRaw : public MavlinkStream -{ -public: - MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) - { - sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); - } - - const char *get_name() - { - return _name; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamServoOutputRaw(_n); - } - -private: - MavlinkOrbSubscription *act_sub; - struct actuator_outputs_s *act; - - char _name[20]; - unsigned int _n; - -protected: - void subscribe(Mavlink *mavlink) - { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - act_sub = mavlink->add_orb_subscription(act_topics[_n]); - act = (struct actuator_outputs_s *)act_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (act_sub->update(t)) { - mavlink_msg_servo_output_raw_send(_channel, - act->timestamp / 1000, - _n, - act->output[0], - act->output[1], - act->output[2], - act->output[3], - act->output[4], - act->output[5], - act->output[6], - act->output[7]); - } - } -}; - - -class MavlinkStreamHILControls : public MavlinkStream -{ -public: - const char *get_name() - { - return "HIL_CONTROLS"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamHILControls(); - } - -private: - MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; - - MavlinkOrbSubscription *act_sub; - struct actuator_outputs_s *act; - -protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); - - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - act = (struct actuator_outputs_s *)act_sub->get_data(); - } - - void send(const hrt_abstime t) - { - bool updated = act_sub->update(t); - (void)pos_sp_triplet_sub->update(t); - (void)status_sub->update(t); - - 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); - - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* set number of valid outputs depending on vehicle type */ - unsigned n; - - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_HEXAROTOR: - n = 6; - break; - - default: - n = 8; - break; - } - - /* scale / assign outputs depending on system type */ - float out[8]; - - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ - out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - - } else { - /* send 0 when disarmed */ - out[i] = 0.0f; - } - - } else { - out[i] = -1.0f; - } - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } else { - - /* fixed wing: scale all channels except throttle -1 .. 1 - * because we know that we set the mixers up this way - */ - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; - - for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ - out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - - } else { - - /* scale fake PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - } - - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } - } - } -}; - - -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -{ -public: - const char *get_name() - { - return "GLOBAL_POSITION_SETPOINT_INT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionSetpointInt(); - } - -private: - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { - if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet->current.lat * 1e7), - (int32_t)(pos_sp_triplet->current.lon * 1e7), - (int32_t)(pos_sp_triplet->current.alt * 1000), - (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); - } - } -}; - - -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "LOCAL_POSITION_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionSetpoint(); - } - -private: - MavlinkOrbSubscription *pos_sp_sub; - struct vehicle_local_position_setpoint_s *pos_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sp_sub->update(t)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp->x, - pos_sp->y, - pos_sp->z, - pos_sp->yaw); - } - } -}; - - -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_sp_sub; - struct vehicle_attitude_setpoint_s *att_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp->timestamp / 1000, - att_sp->roll_body, - att_sp->pitch_body, - att_sp->yaw_body, - att_sp->thrust); - } - } -}; - - -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_rates_sp_sub; - struct vehicle_rates_setpoint_s *att_rates_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp->timestamp / 1000, - att_rates_sp->roll, - att_rates_sp->pitch, - att_rates_sp->yaw, - att_rates_sp->thrust); - } - } -}; - - -class MavlinkStreamRCChannelsRaw : public MavlinkStream -{ -public: - const char *get_name() - { - return "RC_CHANNELS_RAW"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRCChannelsRaw(); - } - -private: - MavlinkOrbSubscription *rc_sub; - struct rc_input_values *rc; - -protected: - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - rc = (struct rc_input_values *)rc_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (rc_sub->update(t)) { - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc->timestamp_publication / 1000, - i, - (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, - rc->rssi); - } - } - } -}; - - -class MavlinkStreamManualControl : public MavlinkStream -{ -public: - const char *get_name() - { - return "MANUAL_CONTROL"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamManualControl(); - } - -private: - MavlinkOrbSubscription *manual_sub; - struct manual_control_setpoint_s *manual; - -protected: - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual->x * 1000, - manual->y * 1000, - manual->z * 1000, - manual->r * 1000, - 0); - } - } -}; + struct vehicle_gps_position_s gps; - -class MavlinkStreamOpticalFlow : public MavlinkStream -{ -public: - const char *get_name() - { - return "OPTICAL_FLOW"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamOpticalFlow(); - } - -private: - MavlinkOrbSubscription *flow_sub; - struct optical_flow_s *flow; - -protected: - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - flow = (struct optical_flow_s *)flow_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); - } - } -}; - -class MavlinkStreamAttitudeControls : public MavlinkStream -{ -public: - const char *get_name() - { - return "ATTITUDE_CONTROLS"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeControls(); - } - -private: - MavlinkOrbSubscription *att_ctrl_sub; - struct actuator_controls_s *att_ctrl; - -protected: - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_ctrl_sub->update(t)) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "rll ctrl ", - att_ctrl->control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "ptch ctrl ", - att_ctrl->control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "yaw ctrl ", - att_ctrl->control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "thr ctrl ", - att_ctrl->control[3]); - } - } -}; - -class MavlinkStreamNamedValueFloat : public MavlinkStream -{ -public: - const char *get_name() - { - return "NAMED_VALUE_FLOAT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamNamedValueFloat(); - } - -private: - MavlinkOrbSubscription *debug_sub; - struct debug_key_value_s *debug; - -protected: - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - debug = (struct debug_key_value_s *)debug_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (debug_sub->update(t)) { - /* enforce null termination */ - debug->key[sizeof(debug->key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(_channel, - debug->timestamp_ms, - debug->key, - debug->value); - } - } -}; - -class MavlinkStreamCameraCapture : public MavlinkStream -{ -public: - const char *get_name() - { - return "CAMERA_CAPTURE"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamCameraCapture(); - } - -private: - MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - -protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - } - - void send(const hrt_abstime t) - { - (void)status_sub->update(t); - - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); - - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + if (gps_sub->update(t, &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_m), + cm_uint16_from_m_float(gps.epv_m), + gps.vel_m_s * 100.0f, + _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps.satellites_visible); } } }; -class MavlinkStreamDistanceSensor : public MavlinkStream -{ -public: - const char *get_name() - { - return "DISTANCE_SENSOR"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamDistanceSensor(); - } - -private: - MavlinkOrbSubscription *range_sub; - struct range_finder_report *range; - -protected: - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - range = (struct range_finder_report *)range_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (range_sub->update(t)) { - - uint8_t type; - - switch (range->type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; - } - - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; - - mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, - range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); - } - } -}; -MavlinkStream *streams_list[] = { - new MavlinkStreamHeartbeat(), - new MavlinkStreamSysStatus(), - new MavlinkStreamHighresIMU(), - new MavlinkStreamAttitude(), - new MavlinkStreamAttitudeQuaternion(), - new MavlinkStreamVFRHUD(), - new MavlinkStreamGPSRawInt(), - new MavlinkStreamGlobalPositionInt(), - new MavlinkStreamLocalPositionNED(), - new MavlinkStreamGPSGlobalOrigin(), - new MavlinkStreamServoOutputRaw(0), - new MavlinkStreamServoOutputRaw(1), - new MavlinkStreamServoOutputRaw(2), - new MavlinkStreamServoOutputRaw(3), - new MavlinkStreamHILControls(), - new MavlinkStreamGlobalPositionSetpointInt(), - new MavlinkStreamLocalPositionSetpoint(), - new MavlinkStreamRollPitchYawThrustSetpoint(), - new MavlinkStreamRollPitchYawRatesThrustSetpoint(), - new MavlinkStreamRCChannelsRaw(), - new MavlinkStreamManualControl(), - new MavlinkStreamOpticalFlow(), - new MavlinkStreamAttitudeControls(), - new MavlinkStreamNamedValueFloat(), - new MavlinkStreamCameraCapture(), - new MavlinkStreamDistanceSensor(), - new MavlinkStreamViconPositionEstimate(), +// class MavlinkStreamGlobalPositionInt : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GLOBAL_POSITION_INT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionInt(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_global_position_s *pos; + +// MavlinkOrbSubscription *home_sub; +// struct home_position_s *home; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home = (struct home_position_s *)home_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// bool updated = pos_sub->update(t); +// updated |= home_sub->update(t); + +// if (updated) { +// mavlink_msg_global_position_int_send(_channel, +// pos->timestamp / 1000, +// pos->lat * 1e7, +// pos->lon * 1e7, +// pos->alt * 1000.0f, +// (pos->alt - home->alt) * 1000.0f, +// pos->vel_n * 100.0f, +// pos->vel_e * 100.0f, +// pos->vel_d * 100.0f, +// _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); +// } +// } +// }; + + +// class MavlinkStreamLocalPositionNED : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "LOCAL_POSITION_NED"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionNED(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_local_position_s *pos; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// pos = (struct vehicle_local_position_s *)pos_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sub->update(t)) { +// mavlink_msg_local_position_ned_send(_channel, +// pos->timestamp / 1000, +// pos->x, +// pos->y, +// pos->z, +// pos->vx, +// pos->vy, +// pos->vz); +// } +// } +// }; + + + +// class MavlinkStreamViconPositionEstimate : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "VICON_POSITION_ESTIMATE"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamViconPositionEstimate(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_vicon_position_s *pos; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sub->update(t)) { +// mavlink_msg_vicon_position_estimate_send(_channel, +// pos->timestamp / 1000, +// pos->x, +// pos->y, +// pos->z, +// pos->roll, +// pos->pitch, +// pos->yaw); +// } +// } +// }; + + +// class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GPS_GLOBAL_ORIGIN"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSGlobalOrigin(); +// } + +// private: +// MavlinkOrbSubscription *home_sub; +// struct home_position_s *home; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home = (struct home_position_s *)home_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { + +// /* we're sending the GPS home periodically to ensure the +// * the GCS does pick it up at one point */ +// if (home_sub->is_published()) { +// home_sub->update(t); + +// mavlink_msg_gps_global_origin_send(_channel, +// (int32_t)(home->lat * 1e7), +// (int32_t)(home->lon * 1e7), +// (int32_t)(home->alt) * 1000.0f); +// } +// } +// }; + + +// class MavlinkStreamServoOutputRaw : public MavlinkStream +// { +// public: +// MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) +// { +// sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); +// } + +// const char *get_name() +// { +// return _name; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamServoOutputRaw(_n); +// } + +// private: +// MavlinkOrbSubscription *act_sub; +// struct actuator_outputs_s *act; + +// char _name[20]; +// unsigned int _n; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// orb_id_t act_topics[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; + +// act_sub = mavlink->add_orb_subscription(act_topics[_n]); +// act = (struct actuator_outputs_s *)act_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (act_sub->update(t)) { +// mavlink_msg_servo_output_raw_send(_channel, +// act->timestamp / 1000, +// _n, +// act->output[0], +// act->output[1], +// act->output[2], +// act->output[3], +// act->output[4], +// act->output[5], +// act->output[6], +// act->output[7]); +// } +// } +// }; + + +// class MavlinkStreamHILControls : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "HIL_CONTROLS"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamHILControls(); +// } + +// private: +// MavlinkOrbSubscription *status_sub; +// struct vehicle_status_s *status; + +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// struct position_setpoint_triplet_s *pos_sp_triplet; + +// MavlinkOrbSubscription *act_sub; +// struct actuator_outputs_s *act; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// status = (struct vehicle_status_s *)status_sub->get_data(); + +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// act = (struct actuator_outputs_s *)act_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// bool updated = act_sub->update(t); +// (void)pos_sp_triplet_sub->update(t); +// (void)status_sub->update(t); + +// 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); + +// if (mavlink_system.type == MAV_TYPE_QUADROTOR || +// mavlink_system.type == MAV_TYPE_HEXAROTOR || +// mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// /* set number of valid outputs depending on vehicle type */ +// unsigned n; + +// switch (mavlink_system.type) { +// case MAV_TYPE_QUADROTOR: +// n = 4; +// break; + +// case MAV_TYPE_HEXAROTOR: +// n = 6; +// break; + +// default: +// n = 8; +// break; +// } + +// /* scale / assign outputs depending on system type */ +// float out[8]; + +// for (unsigned i = 0; i < 8; i++) { +// if (i < n) { +// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { +// /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ +// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + +// } else { +// /* send 0 when disarmed */ +// out[i] = 0.0f; +// } + +// } else { +// out[i] = -1.0f; +// } +// } + +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } else { + +// /* fixed wing: scale all channels except throttle -1 .. 1 +// * because we know that we set the mixers up this way +// */ + +// float out[8]; + +// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + +// for (unsigned i = 0; i < 8; i++) { +// if (i != 3) { +// /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ +// out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + +// } else { + +// /* scale fake PWM out 900..2100 us to 0..1 for throttle */ +// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// } + +// } + +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } +// } +// } +// }; + + +// class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GLOBAL_POSITION_SETPOINT_INT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionSetpointInt(); +// } + +// private: +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// struct position_setpoint_triplet_s *pos_sp_triplet; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sp_triplet_sub->update(t)) { +// mavlink_msg_global_position_setpoint_int_send(_channel, +// MAV_FRAME_GLOBAL, +// (int32_t)(pos_sp_triplet->current.lat * 1e7), +// (int32_t)(pos_sp_triplet->current.lon * 1e7), +// (int32_t)(pos_sp_triplet->current.alt * 1000), +// (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); +// } +// } +// }; + + +// class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "LOCAL_POSITION_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *pos_sp_sub; +// struct vehicle_local_position_setpoint_s *pos_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sp_sub->update(t)) { +// mavlink_msg_local_position_setpoint_send(_channel, +// MAV_FRAME_LOCAL_NED, +// pos_sp->x, +// pos_sp->y, +// pos_sp->z, +// pos_sp->yaw); +// } +// } +// }; + + +// class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawThrustSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *att_sp_sub; +// struct vehicle_attitude_setpoint_s *att_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_sp_sub->update(t)) { +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, +// att_sp->timestamp / 1000, +// att_sp->roll_body, +// att_sp->pitch_body, +// att_sp->yaw_body, +// att_sp->thrust); +// } +// } +// }; + + +// class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *att_rates_sp_sub; +// struct vehicle_rates_setpoint_s *att_rates_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_rates_sp_sub->update(t)) { +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, +// att_rates_sp->timestamp / 1000, +// att_rates_sp->roll, +// att_rates_sp->pitch, +// att_rates_sp->yaw, +// att_rates_sp->thrust); +// } +// } +// }; + + +// class MavlinkStreamRCChannelsRaw : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "RC_CHANNELS_RAW"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRCChannelsRaw(); +// } + +// private: +// MavlinkOrbSubscription *rc_sub; +// struct rc_input_values *rc; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// rc = (struct rc_input_values *)rc_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (rc_sub->update(t)) { +// const unsigned port_width = 8; + +// for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(_channel, +// rc->timestamp_publication / 1000, +// i, +// (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, +// rc->rssi); +// } +// } +// } +// }; + + +// class MavlinkStreamManualControl : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "MANUAL_CONTROL"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamManualControl(); +// } + +// private: +// MavlinkOrbSubscription *manual_sub; +// struct manual_control_setpoint_s *manual; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (manual_sub->update(t)) { +// mavlink_msg_manual_control_send(_channel, +// mavlink_system.sysid, +// manual->x * 1000, +// manual->y * 1000, +// manual->z * 1000, +// manual->r * 1000, +// 0); +// } +// } +// }; + + +// class MavlinkStreamOpticalFlow : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "OPTICAL_FLOW"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamOpticalFlow(); +// } + +// private: +// MavlinkOrbSubscription *flow_sub; +// struct optical_flow_s *flow; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// flow = (struct optical_flow_s *)flow_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (flow_sub->update(t)) { +// mavlink_msg_optical_flow_send(_channel, +// flow->timestamp, +// flow->sensor_id, +// flow->flow_raw_x, flow->flow_raw_y, +// flow->flow_comp_x_m, flow->flow_comp_y_m, +// flow->quality, +// flow->ground_distance_m); +// } +// } +// }; + +// class MavlinkStreamAttitudeControls : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ATTITUDE_CONTROLS"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeControls(); +// } + +// private: +// MavlinkOrbSubscription *att_ctrl_sub; +// struct actuator_controls_s *att_ctrl; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_ctrl_sub->update(t)) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "rll ctrl ", +// att_ctrl->control[0]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "ptch ctrl ", +// att_ctrl->control[1]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "yaw ctrl ", +// att_ctrl->control[2]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "thr ctrl ", +// att_ctrl->control[3]); +// } +// } +// }; + +// class MavlinkStreamNamedValueFloat : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "NAMED_VALUE_FLOAT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamNamedValueFloat(); +// } + +// private: +// MavlinkOrbSubscription *debug_sub; +// struct debug_key_value_s *debug; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// debug = (struct debug_key_value_s *)debug_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (debug_sub->update(t)) { +// /* enforce null termination */ +// debug->key[sizeof(debug->key) - 1] = '\0'; + +// mavlink_msg_named_value_float_send(_channel, +// debug->timestamp_ms, +// debug->key, +// debug->value); +// } +// } +// }; + +// class MavlinkStreamCameraCapture : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "CAMERA_CAPTURE"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamCameraCapture(); +// } + +// private: +// MavlinkOrbSubscription *status_sub; +// struct vehicle_status_s *status; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// status = (struct vehicle_status_s *)status_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// (void)status_sub->update(t); + +// if (status->arming_state == ARMING_STATE_ARMED +// || status->arming_state == ARMING_STATE_ARMED_ERROR) { + +// /* send camera capture on */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + +// } else { +// /* send camera capture off */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); +// } +// } +// }; + +// class MavlinkStreamDistanceSensor : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "DISTANCE_SENSOR"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamDistanceSensor(); +// } + +// private: +// MavlinkOrbSubscription *range_sub; +// struct range_finder_report *range; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// range = (struct range_finder_report *)range_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (range_sub->update(t)) { + +// uint8_t type; + +// switch (range->type) { +// case RANGE_FINDER_TYPE_LASER: +// type = MAV_DISTANCE_SENSOR_LASER; +// break; +// } + +// uint8_t id = 0; +// uint8_t orientation = 0; +// uint8_t covariance = 20; + +// mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, +// range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); +// } +// } +// }; + +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 MavlinkStreamGlobalPositionInt(), + // new MavlinkStreamLocalPositionNED(), + // new MavlinkStreamGPSGlobalOrigin(), + // new MavlinkStreamServoOutputRaw(0), + // new MavlinkStreamServoOutputRaw(1), + // new MavlinkStreamServoOutputRaw(2), + // new MavlinkStreamServoOutputRaw(3), + // new MavlinkStreamHILControls(), + // new MavlinkStreamGlobalPositionSetpointInt(), + // new MavlinkStreamLocalPositionSetpoint(), + // new MavlinkStreamRollPitchYawThrustSetpoint(), + // new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + // new MavlinkStreamRCChannelsRaw(), + // new MavlinkStreamManualControl(), + // new MavlinkStreamOpticalFlow(), + // new MavlinkStreamAttitudeControls(), + // new MavlinkStreamNamedValueFloat(), + // new MavlinkStreamCameraCapture(), + // new MavlinkStreamDistanceSensor(), + // new MavlinkStreamViconPositionEstimate(), nullptr }; |