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);
}
}
void send(const hrt_abstime t)
{
struct vehicle_gps_position_s gps;
if (gps_sub->update(&gps_time, &gps)) {
mavlink_msg_gps_raw_int_send(_channel,
gps.timestamp_position,
gps.fix_type + (gps.fix_quality << 4),
gps.lat,
gps.lon,
gps.alt,
cm_uint16_from_m_float(gps.eph),
cm_uint16_from_m_float(gps.epv),
gps.vel_m_s * 100.0f,
_wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps.satellites_used);
}
}
void
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), mavlink_subs.gps_sub, &gps);
/* GPS COG is 0..2PI in degrees * 1e2 */
float cog_deg = gps.cog_rad;
if (cog_deg > M_PI_F)
cog_deg -= 2.0f * M_PI_F;
cog_deg *= M_RAD_TO_DEG_F;
/* GPS position */
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
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 * 1e2f, // from m/s to cm/s
cog_deg * 1e2f, // from deg to deg * 100
gps.satellites_visible);
/* update SAT info every 10 seconds */
if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
mavlink_msg_gps_status_send(MAVLINK_COMM_0,
gps.satellites_visible,
gps.satellite_prn,
gps.satellite_used,
gps.satellite_elevation,
gps.satellite_azimuth,
gps.satellite_snr);
}
gps_counter++;
}
