diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-02-28 23:44:51 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-02-28 23:44:51 +0400 |
commit | 323b90bfd9f3a1524f36c089d532ed3836cc2ea3 (patch) | |
tree | 79ae96cd6bdf76e9bd658a0b66fdf7051ec4a6ea /src/modules/mavlink | |
parent | 967f81bfabbab0773fa29c079e36678670bcf67a (diff) | |
download | px4-firmware-323b90bfd9f3a1524f36c089d532ed3836cc2ea3.tar.gz px4-firmware-323b90bfd9f3a1524f36c089d532ed3836cc2ea3.tar.bz2 px4-firmware-323b90bfd9f3a1524f36c089d532ed3836cc2ea3.zip |
mavlink: uORB topics includes moved to mavlink_messages.cpp, more messages implemented
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 28 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 433 |
2 files changed, 300 insertions, 161 deletions
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 049f5fedd..41e781ee8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -42,44 +42,18 @@ #include <stdbool.h> #include <nuttx/fs/fs.h> -#include <drivers/drv_rc_input.h> #include <systemlib/param/param.h> #include <systemlib/perf_counter.h> #include <pthread.h> #include <mavlink/mavlink_log.h> #include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/rc_channels.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/home_position.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/vehicle_command.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/position_setpoint_triplet.h> -#include <uORB/topics/optical_flow.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls_effective.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_armed.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/telemetry_status.h> -#include <uORB/topics/debug_key_value.h> -#include <uORB/topics/airspeed.h> -#include <uORB/topics/battery_status.h> -#include <uORB/topics/navigation_capabilities.h> #include <uORB/topics/mission.h> #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" +#include "mavlink_messages.h" // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 967606841..3634acefa 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -10,12 +10,33 @@ #include <lib/geo/geo.h> #include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/position_setpoint_triplet.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/telemetry_status.h> +#include <uORB/topics/debug_key_value.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/navigation_capabilities.h> +#include <drivers/drv_rc_input.h> #include "mavlink_messages.h" @@ -128,9 +149,9 @@ public: private: MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct vehicle_status_s *status; + + MavlinkOrbSubscription *pos_sp_triplet_sub; struct position_setpoint_triplet_s *pos_sp_triplet; protected: @@ -177,7 +198,6 @@ public: private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; protected: @@ -210,6 +230,10 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: + MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) + { + } + const char *get_name() { return "HIGHRES_IMU"; @@ -222,13 +246,12 @@ public: private: MavlinkOrbSubscription *sensor_sub; - struct sensor_combined_s *sensor; - uint32_t accel_counter = 0; - uint32_t gyro_counter = 0; - uint32_t mag_counter = 0; - uint32_t baro_counter = 0; + uint32_t accel_counter; + uint32_t gyro_counter; + uint32_t mag_counter; + uint32_t baro_counter; protected: void subscribe(Mavlink *mavlink) @@ -292,7 +315,6 @@ public: private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; protected: @@ -313,6 +335,75 @@ protected: }; +class MavlinkStreamVFRHUD : public MavlinkStream { +public: + const char *get_name() + { + return "VFR_HUD"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamVFRHUD(); + } + +private: + MavlinkOrbSubscription *att_sub; + struct vehicle_attitude_s *att; + + MavlinkOrbSubscription *pos_sub; + struct vehicle_global_position_s *pos; + + MavlinkOrbSubscription *armed_sub; + struct actuator_armed_s *armed; + + MavlinkOrbSubscription *act_sub; + struct actuator_controls_s *act; + + MavlinkOrbSubscription *airspeed_sub; + struct airspeed_s *airspeed; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed), sizeof(struct actuator_armed_s)); + armed = (struct actuator_armed_s *)armed_sub->get_data(); + + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0), sizeof(struct actuator_controls_s)); + act = (struct actuator_controls_s *)act_sub->get_data(); + + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed), sizeof(struct airspeed_s)); + airspeed = (struct airspeed_s *)airspeed_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sub->update(t); + pos_sub->update(t); + armed_sub->update(t); + act_sub->update(t); + airspeed_sub->update(t); + + 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() @@ -638,11 +729,203 @@ protected: }; +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), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + } + + void send(const hrt_abstime t) { + 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), sizeof(vehicle_local_position_setpoint_s)); + pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + 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), sizeof(vehicle_attitude_setpoint_s)); + att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + 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), sizeof(vehicle_rates_setpoint_s)); + att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + 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), sizeof(struct rc_input_values)); + rc = (struct rc_input_values *)rc_sub->get_data(); + } + + void send(const hrt_abstime t) { + 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); + } + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamVFRHUD(), new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), @@ -652,6 +935,11 @@ MavlinkStream *streams_list[] = { new MavlinkStreamServoOutputRaw(2), new MavlinkStreamServoOutputRaw(3), new MavlinkStreamHILControls(), + new MavlinkStreamGlobalPositionSetpointInt(), + new MavlinkStreamLocalPositionSetpoint(), + new MavlinkStreamRollPitchYawThrustSetpoint(), + new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new MavlinkStreamRCChannelsRaw(), nullptr }; @@ -705,132 +993,9 @@ MavlinkStream *streams_list[] = { // l->listener->attitude_counter++; //} // -//void -//MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) -//{ -// struct vehicle_gps_position_s gps; -// -// /* copy gps data into local buffer */ -// orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); -// -// /* update SAT info every 10 seconds */ -// if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { -// mavlink_msg_gps_status_send(l->mavlink->get_chan(), -// gps.satellites_visible, -// gps.satellite_prn, -// gps.satellite_used, -// gps.satellite_elevation, -// gps.satellite_azimuth, -// gps.satellite_snr); -// } // -// l->listener->gps_counter++; -//} // // -//void -//MavlinkOrbListener::l_input_rc(const struct listener *l) -//{ -// /* copy rc _mavlink->get_chan()nels into local buffer */ -// orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// -// const unsigned port_width = 8; -// -// for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { -// /* Channels are sent in MAVLink main loop at a fixed interval */ -// mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), -// l->listener->rc_raw.timestamp_publication / 1000, -// i, -// (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, -// l->listener->rc_raw.rssi); -// } -// } -//} -// -// -//void -//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) -//{ -// struct position_setpoint_triplet_s triplet; -// orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); -// -// if (!triplet.current.valid) -// return; -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), -// MAV_FRAME_GLOBAL, -// (int32_t)(triplet.current.lat * 1e7d), -// (int32_t)(triplet.current.lon * 1e7d), -// (int32_t)(triplet.current.alt * 1e3f), -// (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); -//} -// -//void -//MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) -//{ -// struct vehicle_local_position_setpoint_s local_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), -// MAV_FRAME_LOCAL_NED, -// local_sp.x, -// local_sp.y, -// local_sp.z, -// local_sp.yaw); -//} -// -//void -//MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) -//{ -// struct vehicle_attitude_setpoint_s att_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), -// att_sp.timestamp / 1000, -// att_sp.roll_body, -// att_sp.pitch_body, -// att_sp.yaw_body, -// att_sp.thrust); -//} -// -//void -//MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) -//{ -// struct vehicle_rates_setpoint_s rates_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), -// rates_sp.timestamp / 1000, -// rates_sp.roll, -// rates_sp.pitch, -// rates_sp.yaw, -// rates_sp.thrust); -//} -// -//void -//MavlinkOrbListener::l_actuator_armed(const struct listener *l) -//{ -// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); -//} // //void //MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) |