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();
}
}
