Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 09:26:25 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

mavlink: GPS_GLOBAL_ORIGIN message added, set message rate depending on baudrate

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-26 23:02:53 +04:00
parent 85dc422d98
commit 18f72f8dd7
2 changed files with 48 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+11 -10
View File
@@ -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 */
src/modules/mavlink/mavlink_messages.cpp
+37 -16
View File
@@ -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);