diff options
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 21 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 53 |
2 files changed, 48 insertions, 26 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5ac76e1cf..3102c937d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1531,18 +1531,19 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* add default streams, intervals depend on baud rate */ -// if (_baudrate >= 230400) { -// } else if (_baudrate >= 115200) { -// } else if (_baudrate >= 57600) { -// } + float rate_mult = _baudrate / 57600.0f; + if (rate_mult > 4.0f) { + rate_mult = 4.0f; + } add_stream("HEARTBEAT", 1.0f); - add_stream("SYS_STATUS", 1.0f); - add_stream("HIGHRES_IMU", 1.0f); - add_stream("ATTITUDE", 10.0f); - add_stream("GPS_RAW_INT", 1.0f); - add_stream("GLOBAL_POSITION_INT", 5.0f); - add_stream("LOCAL_POSITION_NED", 5.0f); + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); while (!_task_should_exit) { /* main loop */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 27ae197a6..48443fbdc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -430,6 +430,42 @@ protected: }; +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), sizeof(struct home_position_s)); + home = (struct home_position_s *)home_sub->get_data(); + } + + void send(const hrt_abstime t) { + 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); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -438,6 +474,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), + new MavlinkStreamGPSGlobalOrigin(), nullptr }; @@ -517,14 +554,6 @@ MavlinkStream *streams_list[] = { // // //void -//MavlinkOrbListener::l_rc_channels(const struct listener *l) -//{ -// /* copy rc channels into local buffer */ -// orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); -// // XXX Add RC channels scaled message here -//} -// -//void //MavlinkOrbListener::l_input_rc(const struct listener *l) //{ // /* copy rc _mavlink->get_chan()nels into local buffer */ @@ -801,14 +830,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_home(const struct listener *l) -//{ -// orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); -// -// mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); -//} -// -//void //MavlinkOrbListener::l_airspeed(const struct listener *l) //{ // orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); |