コード例 #1
0
ファイル: AP_GPS.cpp プロジェクト: hughhugh/ardupilot-rov
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));
}
コード例 #2
0
ファイル: orb_listener.c プロジェクト: youngklee/Firmware
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++;
}
コード例 #3
0
	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);
		}
	}
コード例 #4
0
	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);
		}
	}
コード例 #5
0
ファイル: orb_listener.c プロジェクト: akistanov/Firmware
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++;
}
コード例 #6
0
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);
	}

}
コード例 #7
0
ファイル: mavlink.c プロジェクト: enacuavlab/paparazzi
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
}
コード例 #8
0
ファイル: AP_GPS.cpp プロジェクト: AdiKulkarni/ardupilot
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));
    }
}