diff options
Diffstat (limited to 'apps/mavlink')
| -rw-r--r-- | apps/mavlink/mavlink.c | 10 | ||||
| -rw-r--r-- | apps/mavlink/mavlink_receiver.c | 24 | ||||
| -rw-r--r-- | apps/mavlink/orb_listener.c | 12 |
3 files changed, 26 insertions, 20 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ceb7c2554..b958d5f96 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -625,7 +625,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 2 Hz */ + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { @@ -634,8 +636,10 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 100 ms */ + /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); @@ -651,6 +655,8 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 510d2f3e4..4d9cd964d 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -387,22 +387,22 @@ handle_message(mavlink_message_t *msg) static uint64_t old_timestamp = 0; /* gps */ - hil_gps.timestamp = gps.time_usec; - hil_gps.counter = hil_counter++; + hil_gps.timestamp_position = gps.time_usec; +// hil_gps.counter = hil_counter++; hil_gps.time_gps_usec = gps.time_usec; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; - hil_gps.counter_pos_valid = hil_counter++; - hil_gps.eph = gps.eph; - hil_gps.epv = gps.epv; - hil_gps.s_variance = 100; - hil_gps.p_variance = 100; - hil_gps.vel = gps.vel; - hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_d = 0.0f; - hil_gps.cog = gps.cog; +// hil_gps.counter_pos_valid = hil_counter++; + hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance? + hil_gps.p_variance_m = 100; // XXX 100 m variance? + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_d_m_s = 0.0f; + hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index ec5149745..9f85d5801 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -231,15 +231,15 @@ l_vehicle_gps_position(struct listener *l) /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, - gps.timestamp, + gps.timestamp_position, gps.fix_type, gps.lat, gps.lon, gps.alt, - gps.eph, - gps.epv, - gps.vel, - gps.cog, + (uint16_t)(gps.eph_m * 1e2f), // from m to cm + (uint16_t)(gps.epv_m * 1e2f), // from m to cm + (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s + (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100 gps.satellites_visible); if (gps.satellite_info_available && (gps_counter % 4 == 0)) { @@ -698,7 +698,7 @@ uorb_receive_start(void) /* --- GPS VALUE --- */ mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */ + orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ /* --- HOME POSITION --- */ mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); |