static void send_gps_info(void) { int32_t latitude, longitude, altitude; int16_t gps_vx, gps_vy, gps_vz; float true_yaw; /* Prepare the GPS data */ read_global_data_value(GPS_LAT, DATA_POINTER_CAST(&latitude)); read_global_data_value(GPS_LON, DATA_POINTER_CAST(&longitude)); read_global_data_value(GPS_ALT, DATA_POINTER_CAST(&altitude)); read_global_data_value(GPS_VX, DATA_POINTER_CAST(&gps_vx)); read_global_data_value(GPS_VY, DATA_POINTER_CAST(&gps_vy)); read_global_data_value(GPS_VZ, DATA_POINTER_CAST(&gps_vz)); read_global_data_value(TRUE_YAW, DATA_POINTER_CAST(&true_yaw)); mavlink_message_t msg; mavlink_msg_global_position_int_pack(1, 220, &msg, get_system_time_ms(), //time latitude , //Latitude longitude , //Longitude altitude, //Altitude 0, gps_vx * 1, //Speed-Vx gps_vy * 1, //Speed-Vy gps_vz * 1, //Speed-Vz (uint16_t)true_yaw ); send_package(&msg); }
void position_estimation_telemetry_send_global_position(const position_estimation_t* pos_est, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg) { // send integrated position (for now there is no GPS error correction...!!!) global_position_t gpos = coord_conventions_local_to_global_position(pos_est->local_position); //mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) mavlink_msg_global_position_int_pack( mavlink_stream->sysid, mavlink_stream->compid, msg, time_keeper_get_millis(), gpos.latitude * 10000000, gpos.longitude * 10000000, gpos.altitude * 1000.0f, -pos_est->local_position.pos[2] * 1000, pos_est->vel[0] * 100.0f, pos_est->vel[1] * 100.0f, pos_est->vel[2] * 100.0f, pos_est->local_position.heading); }
void mavlinkSendPosition(void) { uint16_t msgLength; uint8_t gpsFixType = 0; if (!sensors(SENSOR_GPS)) return; if (!STATE(GPS_FIX)) { gpsFixType = 1; } else { if (GPS_numSat < 5) { gpsFixType = 2; } else { gpsFixType = 3; } } mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) micros(), // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gpsFixType, // lat Latitude in 1E7 degrees GPS_coord[LAT], // lon Longitude in 1E7 degrees GPS_coord[LON], // alt Altitude in 1E3 meters (millimeters) above MSL GPS_altitude * 1000, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 GPS_speed, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 GPS_ground_course * 10, // satellites_visible Number of satellites visible. If unknown, set to 255 GPS_numSat); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); // Global position mavlink_msg_global_position_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) micros(), // lat Latitude in 1E7 degrees GPS_coord[LAT], // lon Longitude in 1E7 degrees GPS_coord[LON], // alt Altitude in 1E3 meters (millimeters) above MSL GPS_altitude * 1000, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) #if defined(BARO) || defined(SONAR) (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() * 10 : GPS_altitude * 1000, #else GPS_altitude * 1000, #endif // Ground X Speed (Latitude), expressed as m/s * 100 0, // Ground Y Speed (Longitude), expressed as m/s * 100 0, // Ground Z Speed (Altitude), expressed as m/s * 100 0, // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw) ); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg, // latitude Latitude (WGS84), expressed as * 1E7 GPS_home[LAT], // longitude Longitude (WGS84), expressed as * 1E7 GPS_home[LON], // altitude Altitude(WGS84), expressed as * 1000 0); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); }
void mavlinkSendPosition(timeUs_t currentTimeUs) { uint16_t msgLength; uint8_t gpsFixType = 0; if (!sensors(SENSOR_GPS)) return; if (gpsSol.fixType == GPS_NO_FIX) gpsFixType = 1; else if (gpsSol.fixType == GPS_FIX_2D) gpsFixType = 2; else if (gpsSol.fixType == GPS_FIX_3D) gpsFixType = 3; mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) currentTimeUs, // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gpsFixType, // lat Latitude in 1E7 degrees gpsSol.llh.lat, // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL gpsSol.llh.alt * 10, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 gpsSol.groundSpeed, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 gpsSol.groundCourse * 10, // satellites_visible Number of satellites visible. If unknown, set to 255 gpsSol.numSat); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); // Global position mavlink_msg_global_position_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) currentTimeUs, // lat Latitude in 1E7 degrees gpsSol.llh.lat, // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL gpsSol.llh.alt * 10, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) #if defined(NAV) getEstimatedActualPosition(Z) * 10, #else gpsSol.llh.alt * 10, #endif // Ground X Speed (Latitude), expressed as m/s * 100 0, // Ground Y Speed (Longitude), expressed as m/s * 100 0, // Ground Z Speed (Altitude), expressed as m/s * 100 0, // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw) ); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg, // latitude Latitude (WGS84), expressed as * 1E7 GPS_home.lat, // longitude Longitude (WGS84), expressed as * 1E7 GPS_home.lon, // altitude Altitude(WGS84), expressed as * 1000 GPS_home.alt * 10); // FIXME msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); }
bool MAVLink::sendGlobalPositionInt(int32_t lat, int32_t lng, int32_t alt, int16_t vx, int16_t vy, int16_t vz) { mavlink_message_t msg; mavlink_msg_global_position_int_pack(mySystemId,myComponentId,&msg,lat,lng,alt,vx,vy,vz); return sendMessage(msg); }
int main(int argc, char* argv[]) { // Initialize MAVLink mavlink_wpm_init(&wpm); mavlink_system.sysid = 5; mavlink_system.compid = 20; mavlink_pm_reset_params(&pm); int32_t ground_distance; uint32_t time_ms; // Create socket sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); // Check if --help flag was used if ((argc == 2) && (strcmp(argv[1], help) == 0)) { printf("\n"); printf("\tUsage:\n\n"); printf("\t"); printf("%s", argv[0]); printf(" <ip address of QGroundControl>\n"); printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); exit(EXIT_FAILURE); } // Change the target ip if parameter was given strcpy(target_ip, "127.0.0.1"); if (argc == 2) { strcpy(target_ip, argv[1]); } memset(&locAddr, 0, sizeof(locAddr)); locAddr.sin_family = AF_INET; locAddr.sin_addr.s_addr = INADDR_ANY; locAddr.sin_port = htons(14551); /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) { perror("error bind failed"); close(sock); exit(EXIT_FAILURE); } /* Attempt to make it non blocking */ if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) { fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); close(sock); exit(EXIT_FAILURE); } memset(&gcAddr, 0, sizeof(gcAddr)); gcAddr.sin_family = AF_INET; gcAddr.sin_addr.s_addr = inet_addr(target_ip); gcAddr.sin_port = htons(14550); printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); for (;;) { bytes_sent = 0; /* Send Heartbeat */ mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); /* Send Status */ mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 0, 0, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); /* Send Local Position */ mavlink_msg_local_position_ned_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], position[3], position[4], position[5]); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); /* Send global position */ if (hilEnabled) { mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, time_ms, latitude, longitude, altitude, ground_distance, speedx, speedy, speedz, (yaw/M_PI)*180*100); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); } /* Send attitude */ mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); /* Send HIL outputs */ float roll_ailerons = 0; // -1 .. 1 float pitch_elevator = 0.2; // -1 .. 1 float yaw_rudder = 0.1; // -1 .. 1 float throttle = 0.9; // 0 .. 1 mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); memset(buf, 0, BUFFER_LENGTH); recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); if (recsize > 0) { // Something received - print out all bytes and parse packet mavlink_message_t msg; mavlink_status_t status; printf("Bytes Received: %d\nDatagram: ", (int)recsize); for (i = 0; i < recsize; ++i) { temp = buf[i]; printf("%02x ", (unsigned char)temp); if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) { // Packet received printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); // Handle packet with waypoint component mavlink_wpm_message_handler(&msg); // Handle packet with parameter component mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); // Print HIL values sent to system if (msg.msgid == MAVLINK_MSG_ID_HIL_STATE) { mavlink_hil_state_t hil; mavlink_msg_hil_state_decode(&msg, &hil); printf("Received HIL state:\n"); printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw); roll = hil.roll; pitch = hil.pitch; yaw = hil.yaw; rollspeed = hil.rollspeed; pitchspeed = hil.pitchspeed; yawspeed = hil.yawspeed; speedx = hil.vx; speedy = hil.vy; speedz = hil.vz; latitude = hil.lat; longitude = hil.lon; altitude = hil.alt; hilEnabled = true; } } } printf("\n"); } memset(buf, 0, BUFFER_LENGTH); usleep(10000); // Sleep 10 ms // Send one parameter mavlink_pm_queued_send(); } }