void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) { static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS]; if (status(0) > AP_GPS::NO_GPS) { // when we have a GPS then only send new data if (last_send_time_ms[chan] == last_message_time_ms(0)) { return; } last_send_time_ms[chan] = last_message_time_ms(0); } else { // when we don't have a GPS then send at 1Hz uint32_t now = AP_HAL::millis(); if (now - last_send_time_ms[chan] < 1000) { return; } last_send_time_ms[chan] = now; } const Location &loc = location(0); mavlink_msg_gps_raw_int_send( chan, last_fix_time_ms(0)*(uint64_t)1000, status(0), loc.lat, // in 1E7 degrees loc.lng, // in 1E7 degrees loc.alt * 10UL, // in mm get_hdop(0), get_vdop(0), ground_speed(0)*100, // cm/s ground_course_cd(0), // 1/100 degrees, num_sats(0)); }
void l_vehicle_gps_position(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 position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps.timestamp, gps.fix_type, gps.lat, gps.lon, gps.alt, gps.eph, gps.epv, gps.vel, gps.cog, gps.satellites_visible); if (gps.satellite_info_available && (gps_counter % 4 == 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++; }
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++; }
static void *gps_receiveloop(void * arg) //runs as a pthread and listens messages from GPS { while(1) { if(0 == global_data_wait(&global_data_gps.access_conf)) //only send if pthread_cond_timedwait received a con signal { mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, global_data_gps.time_usec, global_data_gps.fix_type, global_data_gps.lat, global_data_gps.lon, global_data_gps.alt, global_data_gps.eph, global_data_gps.epv, global_data_gps.vel, global_data_gps.cog, global_data_gps.satellites_visible); mavlink_msg_gps_status_send(MAVLINK_COMM_0, global_data_gps.satellites_visible, global_data_gps.satellite_prn, global_data_gps.satellite_used, global_data_gps.satellite_elevation, global_data_gps.satellite_azimuth, global_data_gps.satellite_snr); } // else // { // mavlink_msg_statustext_send(chan,0,"timeout"); // } global_data_unlock(&global_data_gps.access_conf); } }
static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev) { #if USE_GPS int32_t course = DegOfRad(gps.course) / 1e5; if (course < 0) { course += 36000; } mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, get_sys_time_usec(), gps.fix, gps.lla_pos.lat, gps.lla_pos.lon, gps.lla_pos.alt, gps.pdop, UINT16_MAX, // VDOP gps.gspeed, course, gps.num_sv); MAVLinkSendMessage(); #endif }
void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) { static uint32_t last_send_time_ms; if (last_send_time_ms == 0 || last_send_time_ms != last_message_time_ms(0)) { last_send_time_ms = last_message_time_ms(0); const Location &loc = location(0); mavlink_msg_gps_raw_int_send( chan, last_fix_time_ms(0)*(uint64_t)1000, status(0), loc.lat, // in 1E7 degrees loc.lng, // in 1E7 degrees loc.alt * 10UL, // in mm get_hdop(0), 65535, ground_speed(0)*100, // cm/s ground_course_cd(0), // 1/100 degrees, num_sats(0)); } }