static void postHeartbeat( SOCKET sock, const sockaddr_in& gcAddr, float position[] ) { mavlink_message_t msg; uint16_t len; int bytes_sent; uint8_t buf[BUFFER_LENGTH]; //#define OLD_MAV /*Send Heartbeat */ #ifdef OLD_MAV mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); #else mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE); #endif len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(gcAddr)); /* Send Status */ #ifdef OLD_MAV mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 7500, 0, 0, 0); #else mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0); #endif len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof (gcAddr)); /* Send Local Position */ #ifdef OLD_MAV mavlink_msg_local_position_pack(1, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], position[3], position[4], position[5]); #else mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], position[3], position[4], position[5]); #endif len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); /* Send attitude */ //old and new, do difference. static float roll = 1.2; float pitch = 1.7; float yaw = 3.14; #ifdef OLD_MAV mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, 0.01, 0.02, 0.03); #else mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, 0.01, 0.02, 0.03); #endif len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(gcAddr)); //Beep( 880, 100 ); }
void mavlinkSendSystemStatus(void) { uint16_t msgLength; uint32_t onboardControlAndSensors = 35843; /* onboard_control_sensors_present Bitmask fedcba9876543210 1000110000000011 For all = 35843 0001000000000100 With Mag = 4100 0010000000001000 With Baro = 8200 0100000000100000 With GPS = 16416 0000001111111111 */ if (sensors(SENSOR_MAG)) onboardControlAndSensors |= 4100; if (sensors(SENSOR_BARO)) onboardControlAndSensors |= 8200; if (sensors(SENSOR_GPS)) onboardControlAndSensors |= 16416; mavlink_msg_sys_status_pack(0, 200, &mavMsg, // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. //Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, // 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, // 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, // 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control onboardControlAndSensors, // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled onboardControlAndSensors, // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error. onboardControlAndSensors & 1023, // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 0, // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) feature(FEATURE_VBAT) ? vbat * 100 : 0, // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current feature(FEATURE_VBAT) ? amperage : -1, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_count1 Autopilot-specific errors 0, // errors_count2 Autopilot-specific errors 0, // errors_count3 Autopilot-specific errors 0, // errors_count4 Autopilot-specific errors 0); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); }
/** * This function transmits a MAVLink SYS_STATUS message. It relies on various external information such as sensor/actuator status * from ecanSensors.h, the internalVariables struct exported by Simulink, and the drop rate calculated within ecanSensors.c. */ void MavLinkSendStatus(void) { mavlink_message_t msg; // Declare that we have onboard sensors: 3D gyro, 3D accelerometer, 3D magnetometer, absolute pressure, GPS // And that we have the following controllers: yaw position, x/y position control, motor outputs/control. uint32_t systemsPresent = ONBOARD_SENSORS_REVO_GS | ONBOARD_SENSORS_WSO100 | ONBOARD_SENSORS_GPS | ONBOARD_CONTROL_YAW_POS | ONBOARD_CONTROL_XY_POS | ONBOARD_CONTROL_MOTOR; uint32_t systemsEnabled = ONBOARD_CONTROL_YAW_POS; systemsEnabled |= sensorAvailability.gps.enabled?ONBOARD_SENSORS_GPS:0; systemsEnabled |= sensorAvailability.revo_gs.enabled?ONBOARD_SENSORS_REVO_GS:0; systemsEnabled |= sensorAvailability.wso100.enabled?ONBOARD_SENSORS_WSO100:0; // The DST800 doesn't map into this bitfield. // The power node doesn't map into this bitfield.z systemsEnabled |= sensorAvailability.prop.enabled?(ONBOARD_CONTROL_XY_POS|ONBOARD_CONTROL_MOTOR):0; uint32_t systemsActive = ONBOARD_CONTROL_YAW_POS; systemsActive |= sensorAvailability.gps.active?ONBOARD_SENSORS_GPS:0; systemsActive |= sensorAvailability.revo_gs.active?ONBOARD_SENSORS_REVO_GS:0; systemsActive |= sensorAvailability.wso100.active?ONBOARD_SENSORS_WSO100:0; // The DST800 doesn't map into this bitfield. // The power node doesn't map into this bitfield. systemsActive |= sensorAvailability.prop.active?(ONBOARD_CONTROL_XY_POS|ONBOARD_CONTROL_MOTOR):0; // Grab the globally-declared battery sensor data and map into the values necessary for transmission. uint16_t voltage = (uint16_t)(internalVariables.BatteryVoltage * 1000); int16_t amperage = (int16_t)(internalVariables.BatteryAmperage * 100); // Calculate the drop rate uint16_t dropRate = 0; if (mavLinkMessagesFailedParsing) { dropRate = (uint16_t)(((float)mavLinkMessagesFailedParsing) * 10000.0f / ((float)(mavLinkMessagesReceived + mavLinkMessagesFailedParsing))); } mavlink_msg_sys_status_pack(mavlink_system.sysid, mavlink_system.compid, &msg, systemsPresent, systemsEnabled, systemsActive, (uint16_t)(systemStatus.cpu_load)*10, voltage, amperage, -1, dropRate, 0, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); uart1EnqueueData(buf, (uint8_t)len); }
uint16_t PackSysStatus(uint8_t system_id, uint8_t component_id, mavlink_sys_status_t mlSysStatus){ mavlink_system_t mavlink_system; // if (!(mlPending.wpProtState == WP_PROT_IDLE)) // return 0; mavlink_system.sysid = system_id; ///< ID 20 for this airplane mavlink_system.compid = component_id;//MAV_COMP_ID_IMU; ///< The component sending the message is the IMU, it could be also a Linux process ////////////////////////////////////////////////////////////////////////// mavlink_message_t msg; memset(&msg, 0, sizeof (mavlink_message_t)); mavlink_msg_sys_status_pack(system_id, component_id, &msg, mlSysStatus.onboard_control_sensors_present,mlSysStatus.onboard_control_sensors_enabled, mlSysStatus.onboard_control_sensors_health, mlSysStatus.load, mlSysStatus.voltage_battery, mlSysStatus.current_battery, mlSysStatus.battery_remaining, mlSysStatus.drop_rate_comm, mlSysStatus.errors_comm, mlSysStatus.errors_count1, mlSysStatus.errors_count2, mlSysStatus.errors_count3, mlSysStatus.errors_count4); return( mavlink_msg_to_send_buffer(UartOutBuff, &msg)); }
void state_telemetry_send_status(const state_t* state, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg) { float battery_voltage = state->analog_monitor->avg[ANALOG_RAIL_11]; // bat voltage (mV), actual battery pack plugged to the board float battery_remaining = state->analog_monitor->avg[ANALOG_RAIL_10] / 12.4f * 100.0f; //percentage over full 3cells battery (=12.4V) mavlink_msg_sys_status_pack(mavlink_stream->sysid, mavlink_stream->compid, msg, state->sensor_present, // sensors present state->sensor_enabled, // sensors enabled state->sensor_health, // sensors health 0, // load (int32_t)(1000.0f * battery_voltage), // bat voltage (mV) 0, // current (mA) battery_remaining, // battery remaining 0, 0, // comms drop, comms errors 0, 0, 0, 0); // autopilot specific errors }
static void send_system_info(void) { mavlink_message_t msg; mavlink_msg_sys_status_pack(1, 0, &msg, 0, 0, 0, 0, 12.5 * 1000, //Battery voltage -1, 100, //Battery remaining 0, 0, 0, 0, 0, 0 ); send_package(&msg); }
void cDataLink::SendStatusMsg(int load, int millivoltage) { int bytes_sent; uint16_t len; std::bitset<16> onboard_control_sensors_present = 0; std::bitset<16> onboard_control_sensors_enabled = 0; std::bitset<16> onboard_control_sensors_health = 0; onboard_control_sensors_present.set(gyro); onboard_control_sensors_present.set(acc); onboard_control_sensors_present.set(mag); onboard_control_sensors_present.set(absolute_pressure); onboard_control_sensors_present.set(gps); onboard_control_sensors_enabled = onboard_control_sensors_present; onboard_control_sensors_health = onboard_control_sensors_present; /* Send Status */ mavlink_msg_sys_status_pack(1, MAV_COMP_ID_ALL, &m_msg, onboard_control_sensors_present.to_ulong(), //sensors present (uint32_bitmask) onboard_control_sensors_enabled.to_ulong(), //sensors enabled onboard_control_sensors_health.to_ulong(), //sensors health load, //main loop load millivoltage, //voltage in millivolt -1, // current in mA (-1 = no data) -1, // remainig capacity 0, 0, 0, 0, 0, 0); //communication and autopilot errors len = mavlink_msg_to_send_buffer(m_buf, &m_msg); bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); (void)bytes_sent; //avoid compiler warning }
int main(int argc, char* argv[]) { char help[] = "--help"; char target_ip[100]; float position[6] = {}; int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); struct sockaddr_in gcAddr; struct sockaddr_in locAddr; //struct sockaddr_in fromAddr; uint8_t buf[BUFFER_LENGTH]; ssize_t recsize; socklen_t fromlen; int bytes_sent; mavlink_message_t msg; uint16_t len; int i = 0; //int success = 0; unsigned int temp = 0; // 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); for (;;) { /*Send Heartbeat */ mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE); 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, 0, 0, 0, 500, 11000, -1, -1, 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(1, 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 attitude */ mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); 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); } } printf("\n"); } memset(buf, 0, BUFFER_LENGTH); sleep(1); // Sleep one second } }
static void uavoMavlinkBridgeTask(void *parameters) { uint32_t lastSysTime; // Main task loop lastSysTime = PIOS_Thread_Systime(); FlightBatterySettingsData batSettings = {}; if (FlightBatterySettingsHandle() != NULL ) FlightBatterySettingsGet(&batSettings); SystemStatsData systemStats; while (1) { PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ); if (stream_trigger(MAV_DATA_STREAM_EXTENDED_STATUS)) { FlightBatteryStateData batState = {}; if (FlightBatteryStateHandle() != NULL ) FlightBatteryStateGet(&batState); SystemStatsGet(&systemStats); int8_t battery_remaining = 0; if (batSettings.Capacity != 0) { if (batState.ConsumedEnergy < batSettings.Capacity) { battery_remaining = 100 - lroundf(batState.ConsumedEnergy / batSettings.Capacity * 100); } } uint16_t voltage = 0; if (batSettings.VoltagePin != FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE) voltage = lroundf(batState.Voltage * 1000); uint16_t current = 0; if (batSettings.CurrentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE) current = lroundf(batState.Current * 100); mavlink_msg_sys_status_pack(0, 200, mav_msg, // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t)systemStats.CPULoad * 10, // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) voltage, // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current current, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery battery_remaining, // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_count1 Autopilot-specific errors 0, // errors_count2 Autopilot-specific errors 0, // errors_count3 Autopilot-specific errors 0, // errors_count4 Autopilot-specific errors 0); send_message(); } if (stream_trigger(MAV_DATA_STREAM_RC_CHANNELS)) { ManualControlCommandData manualState; FlightStatusData flightStatus; ManualControlCommandGet(&manualState); FlightStatusGet(&flightStatus); SystemStatsGet(&systemStats); //TODO connect with RSSI object and pass in last argument mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg, // time_boot_ms Timestamp (milliseconds since system boot) systemStats.FlightTime, // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. 0, // chan1_raw RC channel 1 value, in microseconds manualState.Channel[0], // chan2_raw RC channel 2 value, in microseconds manualState.Channel[1], // chan3_raw RC channel 3 value, in microseconds manualState.Channel[2], // chan4_raw RC channel 4 value, in microseconds manualState.Channel[3], // chan5_raw RC channel 5 value, in microseconds manualState.Channel[4], // chan6_raw RC channel 6 value, in microseconds manualState.Channel[5], // chan7_raw RC channel 7 value, in microseconds manualState.Channel[6], // chan8_raw RC channel 8 value, in microseconds manualState.Channel[7], // rssi Receive signal strength indicator, 0: 0%, 255: 100% manualState.Rssi); send_message(); } if (stream_trigger(MAV_DATA_STREAM_POSITION)) { GPSPositionData gpsPosData = {}; HomeLocationData homeLocation = {}; SystemStatsGet(&systemStats); if (GPSPositionHandle() != NULL ) GPSPositionGet(&gpsPosData); if (HomeLocationHandle() != NULL ) HomeLocationGet(&homeLocation); SystemStatsGet(&systemStats); uint8_t gps_fix_type; switch (gpsPosData.Status) { case GPSPOSITION_STATUS_NOGPS: gps_fix_type = 0; break; case GPSPOSITION_STATUS_NOFIX: gps_fix_type = 1; break; case GPSPOSITION_STATUS_FIX2D: gps_fix_type = 2; break; case GPSPOSITION_STATUS_FIX3D: case GPSPOSITION_STATUS_DIFF3D: gps_fix_type = 3; break; default: gps_fix_type = 0; break; } mavlink_msg_gps_raw_int_pack(0, 200, mav_msg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)systemStats.FlightTime * 1000, // 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. gps_fix_type, // lat Latitude in 1E7 degrees gpsPosData.Latitude, // lon Longitude in 1E7 degrees gpsPosData.Longitude, // alt Altitude in 1E3 meters (millimeters) above MSL gpsPosData.Altitude * 1000, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 gpsPosData.HDOP * 100, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 gpsPosData.VDOP * 100, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 gpsPosData.Groundspeed * 100, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 gpsPosData.Heading * 100, // satellites_visible Number of satellites visible. If unknown, set to 255 gpsPosData.Satellites); send_message(); mavlink_msg_gps_global_origin_pack(0, 200, mav_msg, // latitude Latitude (WGS84), expressed as * 1E7 homeLocation.Latitude, // longitude Longitude (WGS84), expressed as * 1E7 homeLocation.Longitude, // altitude Altitude(WGS84), expressed as * 1000 homeLocation.Altitude * 1000); send_message(); //TODO add waypoint nav stuff //wp_target_bearing //wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg); //alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg); //aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg); //xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg); //mavlink_msg_nav_controller_output_pack //wp_number //mavlink_msg_mission_current_pack } if (stream_trigger(MAV_DATA_STREAM_EXTRA1)) { AttitudeActualData attActual; SystemStatsData systemStats; AttitudeActualGet(&attActual); SystemStatsGet(&systemStats); mavlink_msg_attitude_pack(0, 200, mav_msg, // time_boot_ms Timestamp (milliseconds since system boot) systemStats.FlightTime, // roll Roll angle (rad) attActual.Roll * DEG2RAD, // pitch Pitch angle (rad) attActual.Pitch * DEG2RAD, // yaw Yaw angle (rad) attActual.Yaw * DEG2RAD, // rollspeed Roll angular speed (rad/s) 0, // pitchspeed Pitch angular speed (rad/s) 0, // yawspeed Yaw angular speed (rad/s) 0); send_message(); } if (stream_trigger(MAV_DATA_STREAM_EXTRA2)) { ActuatorDesiredData actDesired; AttitudeActualData attActual; AirspeedActualData airspeedActual = {}; GPSPositionData gpsPosData = {}; BaroAltitudeData baroAltitude = {}; FlightStatusData flightStatus; if (AirspeedActualHandle() != NULL ) AirspeedActualGet(&airspeedActual); if (GPSPositionHandle() != NULL ) GPSPositionGet(&gpsPosData); if (BaroAltitudeHandle() != NULL ) BaroAltitudeGet(&baroAltitude); ActuatorDesiredGet(&actDesired); AttitudeActualGet(&attActual); FlightStatusGet(&flightStatus); float altitude = 0; if (BaroAltitudeHandle() != NULL) altitude = baroAltitude.Altitude; else if (GPSPositionHandle() != NULL) altitude = gpsPosData.Altitude; // round attActual.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360) int16_t heading = lroundf(attActual.Yaw); if (heading < 0) heading += 360; mavlink_msg_vfr_hud_pack(0, 200, mav_msg, // airspeed Current airspeed in m/s airspeedActual.TrueAirspeed, // groundspeed Current ground speed in m/s gpsPosData.Groundspeed, // heading Current heading in degrees, in compass units (0..360, 0=north) heading, // throttle Current throttle setting in integer percent, 0 to 100 actDesired.Throttle * 100, // alt Current altitude (MSL), in meters altitude, // climb Current climb rate in meters/second 0); send_message(); uint8_t armed_mode = 0; if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED; uint8_t custom_mode = CUSTOM_MODE_STAB; switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: case FLIGHTSTATUS_FLIGHTMODE_MWRATE: case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR: case FLIGHTSTATUS_FLIGHTMODE_HORIZON: /* Kinda a catch all */ custom_mode = CUSTOM_MODE_SPORT; break; case FLIGHTSTATUS_FLIGHTMODE_ACRO: case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK: custom_mode = CUSTOM_MODE_ACRO; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: /* May want these three to try and * infer based on roll axis */ case FLIGHTSTATUS_FLIGHTMODE_LEVELING: custom_mode = CUSTOM_MODE_STAB; break; case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: custom_mode = CUSTOM_MODE_DRIFT; break; case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: custom_mode = CUSTOM_MODE_ALTH; break; case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: custom_mode = CUSTOM_MODE_RTL; break; case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: custom_mode = CUSTOM_MODE_POSH; break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: custom_mode = CUSTOM_MODE_AUTO; break; } mavlink_msg_heartbeat_pack(0, 200, mav_msg, // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) MAV_TYPE_GENERIC, // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM MAV_AUTOPILOT_GENERIC, // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h armed_mode, // custom_mode A bitfield for use for autopilot-specific flags. custom_mode, // system_status System status flag, see MAV_STATE ENUM 0); send_message(); } } }
/*----------------------------------------------------------------------------- * Service MAVlink protocol message routine */ void Timer1_IRQHandler() { mavlink_message_t msg; MadgwickAHRSupdateIMU( DEG_TO_RAD(params[PARAM_GX].val), DEG_TO_RAD(params[PARAM_GY].val), DEG_TO_RAD(params[PARAM_GZ].val), params[PARAM_AX].val, params[PARAM_AY].val, params[PARAM_AZ].val); mavlink_msg_heartbeat_pack( mavlink_system.sysid, mavlink_system.compid, &msg, MAV_TYPE_GROUND_ROVER, MAV_AUTOPILOT_GENERIC, mavlink_sys_mode, 0, mavlink_sys_state); mavlink_send_msg(&msg); mavlink_msg_sys_status_pack( mavlink_system.sysid, mavlink_system.compid, &msg, MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_MAG | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_GPS, MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_MAG, MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_MAG | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_GPS, 50, // TODO remove hardoded values 0, -1, -1, 0, 0, 0, 0, 0, 0); mavlink_send_msg(&msg); mavlink_msg_autopilot_version_pack( mavlink_system.sysid, mavlink_system.compid, &msg, 0, // No capabilities (MAV_PROTOCOL_CAPABILITY). TBD 42, 42, 42, 42, "DEADBEEF", "DEADBEEF", "DEADBEEF", 42, 42, 42); mavlink_send_msg(&msg); mavlink_msg_highres_imu_pack( mavlink_system.sysid, mavlink_system.compid, &msg, usec(), params[PARAM_AX].val, params[PARAM_AY].val, params[PARAM_AZ].val, params[PARAM_GX].val, params[PARAM_GY].val, params[PARAM_GZ].val, params[PARAM_MX].val, params[PARAM_MY].val, params[PARAM_MZ].val, 0, 0, 0, params[PARAM_T].val, (1 << 12) | ((1 << 9) - 1)); mavlink_send_msg(&msg); mavlink_msg_attitude_quaternion_pack( mavlink_system.sysid, mavlink_system.compid, &msg, usec(), q0, q1, q2, q3, DEG_TO_RAD(params[PARAM_GX].val), DEG_TO_RAD(params[PARAM_GY].val), DEG_TO_RAD(params[PARAM_GZ].val)); mavlink_send_msg(&msg); usec_service_routine(); MSS_TIM1_clear_irq(); }
/** * @brief Encode a sys_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * @param sys_status C-struct to read the message contents from */ uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) { return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); }
bool MAVLink::sendSystemStatus(uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { mavlink_message_t msg; mavlink_msg_sys_status_pack(mySystemId,myComponentId,&msg, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop); return sendMessage(msg); }
struct mavlink_drone *mavlink_drone_new(struct mavlink_drone_cfg *cfg) { struct mavlink_drone *c; struct epoll_event ev; struct epoll_event events[3]; struct itimerspec timeout; struct mavlink_comm_cfg comm_cfg = { .local_port = 14551, .remote_port = 14550, .cb = &callback}; if (!cfg) return NULL; c = calloc(1, sizeof(struct mavlink_drone)); if (!c) return NULL; c->remote_system_id = cfg->remote_system_id; c->remote_component_id = cfg->remote_component_id; c->update_status_cb = cfg->update_status_cb; comm_cfg.remote_addr = NULL; c->comm = mavlink_comm_new(&comm_cfg); if (!c->comm) { printf("bad init comm\n"); goto error; } c->exitfd = eventfd(0,0); bzero(&timeout, sizeof(timeout)); timeout.it_value.tv_sec = 1; timeout.it_value.tv_nsec = 0; timeout.it_interval.tv_sec = 1; timeout.it_interval.tv_nsec = 0; c->fd_1hz = timerfd_init(&timeout); if (c->fd_1hz < 0) { printf("Failed to create timerfd 1hz\n"); goto error; } printf("size %lu\n", sizeof(events) / sizeof(events[0])); c->epollfd = epoll_create(sizeof(events) / sizeof(events[0])); if (c->epollfd < 0) { printf("epoll_create failed\n"); goto error; } ev.events = EPOLLIN; ev.data.fd = c->fd_1hz; if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, c->fd_1hz, &ev) == -1) { printf("failed to add fd_1hz\n"); goto error; } ev.events = EPOLLIN; ev.data.fd = mavlink_comm_get_sockfd(c->comm); if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, ev.data.fd, &ev) == -1) { printf ("failed to add sockfd\n"); goto error; } ev.events = EPOLLIN; ev.data.fd = c->exitfd; if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, c->exitfd, &ev) == -1) { printf("failed to add eventfd\n"); goto error; } return c; error: if (c->comm) mavlink_comm_destroy(c->comm); if (c->fd_1hz > 0) close(c->fd_1hz); if (c->epollfd > 0) close(c->epollfd); free(c); return NULL; } void mavlink_drone_destroy(struct mavlink_drone *c) { if (!c) return; if (c->epollfd > 0) close(c->epollfd); free(c); } static void mavlink_drone_send_status(struct mavlink_drone *ctrl) { mavlink_message_t msg; struct mavlink_info_cache *c = &ctrl->cache; int ret; if (ctrl->update_status_cb) (ctrl->update_status_cb)(c); mavlink_msg_heartbeat_pack(ctrl->remote_system_id, ctrl->remote_component_id, &msg, c->hb.type, c->hb.autopilot, c->hb.base_mode, 0, c->hb.system_status); ret = mavlink_comm_send_msg(ctrl->comm, &msg); if (ret < 0) printf("%d error %d\n", __LINE__, ret); /* Send Status */ mavlink_msg_sys_status_pack(ctrl->remote_system_id, ctrl->remote_component_id, &msg, c->sys_stat.onboard_control_sensors_present, c->sys_stat.onboard_control_sensors_enabled, c->sys_stat.onboard_control_sensors_health, c->sys_stat.load, c->sys_stat.voltage_battery, c->sys_stat.current_battery, c->sys_stat.drop_rate_comm, c->sys_stat.errors_comm, c->sys_stat.errors_count1, c->sys_stat.errors_count2, c->sys_stat.errors_count3, c->sys_stat.errors_count4, c->sys_stat.battery_remaining); ret = mavlink_comm_send_msg(ctrl->comm, &msg); if (ret < 0) printf("%d error %d\n", __LINE__, ret); /* Send Local Position */ mavlink_msg_local_position_ned_pack(ctrl->remote_system_id, ctrl->remote_component_id, &msg, c->lpn.time_boot_ms, c->lpn.x, c->lpn.y, c->lpn.z, c->lpn.vx, c->lpn.vy, c->lpn.vz); ret = mavlink_comm_send_msg(ctrl->comm, &msg); if (ret < 0) printf("%d error %d\n", __LINE__, ret); /* Send attitude */ mavlink_msg_attitude_pack(ctrl->remote_system_id, ctrl->remote_component_id, &msg, c->att.time_boot_ms, c->att.roll, c->att.pitch, c->att.yaw, c->att.rollspeed, c->att.pitchspeed, c->att.yawspeed); ret = mavlink_comm_send_msg(ctrl->comm, &msg); if (ret < 0) printf("%d error %d\n", __LINE__, ret); }
void *MavlinkStatusTask(void *ptr) { MavlinkStruct *Mavlink = (MavlinkStruct *) ptr; AttitudeData *AttitudeDesire = Control.AttitudeDesire; AttitudeData *AttitudeMesure = Mavlink->AttitudeMesure; AttData Data, Speed; AttData DataD, SpeedD; AttData DataM, SpeedM; double Error[4] = {0.0, 0.0, 0.0, 0.0}; uint32_t TimeStamp; mavlink_message_t msg; uint16_t len; uint8_t buf[BUFFER_LENGTH]; int bytes_sent; uint32_t sensor = 0xf00f; printf("%s : Mavlink Status démarré\n", __FUNCTION__); pthread_barrier_wait(&(MavlinkStartBarrier)); while (MavlinkActivated) { sem_wait(&MavlinkStatusTimerSem); if (MavlinkActivated == 0) break; memset(buf, 0, BUFFER_LENGTH); pthread_spin_lock(&(AttitudeMesure->AttitudeLock)); memcpy((void *) &Data, (void *) &(AttitudeMesure->Data), sizeof(AttData)); memcpy((void *) &Speed, (void *) &(AttitudeMesure->Speed), sizeof(AttData)); TimeStamp = AttitudeMesure->timestamp_s*1000 + AttitudeMesure->timestamp_n/1000000L; pthread_spin_unlock(&(AttitudeMesure->AttitudeLock)); //Send Heartbeat mavlink_msg_heartbeat_pack(SYSTEM_ID, COMPONENT_ID, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in)); // Send Status mavlink_msg_sys_status_pack(SYSTEM_ID, COMPONENT_ID, &msg, sensor, sensor, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof (struct sockaddr_in)); // Send Local Position mavlink_msg_local_position_ned_pack(SYSTEM_ID, COMPONENT_ID, &msg, TimeStamp, 0, 0, (float) Data.Elevation, 0, 0, (float) Speed.Elevation); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in)); pthread_spin_lock(&(AttitudeDesire->AttitudeLock)); memcpy((void *) &DataD, (void *) &(AttitudeDesire->Data), sizeof(AttData)); memcpy((void *) &SpeedD, (void *) &(AttitudeDesire->Speed), sizeof(AttData)); pthread_spin_unlock(&(AttitudeDesire->AttitudeLock)); pthread_spin_lock(&(AttitudeMesure->AttitudeLock)); memcpy((void *) &DataM, (void *) &(AttitudeMesure->Data), sizeof(AttData)); memcpy((void *) &SpeedM, (void *) &(AttitudeMesure->Speed), sizeof(AttData)); pthread_spin_unlock(&(AttitudeMesure->AttitudeLock)); Error[HEIGHT] = DataD.Elevation - DataM.Elevation; Error[ROLL] = DataD.Roll - DataM.Roll; Error[PITCH] = DataD.Pitch - DataM.Pitch; Error[YAW] = DataD.Yaw - DataM.Yaw; // Send Attitude mavlink_msg_attitude_pack(SYSTEM_ID, COMPONENT_ID, &msg, TimeStamp, (float) Data.Roll, (float) Data.Pitch, (float) Data.Yaw, (float) Speed.Roll, (float) Speed.Pitch, (float) Speed.Yaw); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in)); } printf("%s : Mavlink Status Arrêté\n", __FUNCTION__); pthread_exit(NULL); }
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(); } }
int main(int argc, char* argv[]) { // Initialize MAVLink mavlink_wpm_init(&wpm); mavlink_system.sysid = 1; mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER; // 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 (;;) { /*Send Heartbeat */ mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC); 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, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 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_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 attitude */ mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); 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 } } printf("\n"); } memset(buf, 0, BUFFER_LENGTH); usleep(50000); // Sleep one second } }