/**************************************************************************** * Private Data ****************************************************************************/ static void *receiveloop(void * arg) //runs as a pthread and listens to uart1 ("/dev/ttyS0") { uint8_t ch; mavlink_message_t msg; while(1) { /* blocking read on next byte */ int nread = read(uart, &ch, 1); if (nread > 0 && mavlink_parse_char(chan,ch,&msg,&status)) {//parse the char // handle generic messages and commands handleMessage(&msg); // Handle packet with waypoint component mavlink_wpm_message_handler(&msg); // Handle packet with parameter component mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); led_toggle(LED_AMBER); } } }
/** * Receive data from UART. */ static void * receive_thread(void *arg) { int uart_fd = *((int *)arg); const int timeout = 1000; uint8_t buf[32]; mavlink_message_t msg; prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); while (!thread_should_exit) { struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; if (poll(fds, 1, timeout) > 0) { /* non-blocking read. read may return negative values */ ssize_t nread = read(uart_fd, buf, sizeof(buf)); /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(chan, buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); /* Handle packet with waypoint component */ mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); /* Handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } } }
void Mavlink::handle_message(const mavlink_message_t *msg) { /* handle packet with mission manager */ _mission_manager->handle_message(msg); /* handle packet with parameter component */ mavlink_pm_message_handler(_channel, msg); if (get_forwarding_on()) { /* forward any messages to other mavlink instances */ Mavlink::forward_message(msg, this); } }
/** * Receive data from UART. */ static void * receive_thread(void *arg) { int uart_fd = *((int *)arg); const int timeout = 1000; uint8_t buf[32]; mavlink_message_t msg; prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); struct pollfd fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; ssize_t nread = 0; while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { if (nread < sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } /* non-blocking read. read may return negative values */ nread = read(uart_fd, buf, sizeof(buf)); /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(chan, buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); /* handle packet with waypoint component */ mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } } } return NULL; }
/** * Receive data from UART. */ static void * receive_thread(void *arg) { int uart_fd = *((int*)arg); const int timeout = 1000; uint8_t ch; mavlink_message_t msg; prctl(PR_SET_NAME, "mavlink offb mod rcv", getpid()); while (!thread_should_exit) { struct pollfd fds[] = { {.fd = uart_fd, .events = POLLIN}}; if (poll(fds, 1, timeout) > 0) { /* non-blocking read until buffer is empty */ int nread = 0; do { nread = read(uart_fd, &ch, 1); if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char /* handle generic messages and commands */ handle_message(&msg); /* Handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } while (nread > 0); } }
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(); } }
/** * Telemetry transmit task. Processes queue events and periodic updates. */ static void telemetryRxTask(void *parameters) { uint32_t inputPort; uint8_t c; // Task loop while (1) { #if defined(PIOS_INCLUDE_USB_HID) // Determine input port (USB takes priority over telemetry port) if (PIOS_USB_HID_CheckAvailable(0)) { inputPort = PIOS_COM_TELEM_USB; } else #endif /* PIOS_INCLUDE_USB_HID */ { inputPort = telemetryPort; } mavlink_channel_t mavlink_chan = MAVLINK_COMM_0; // Block until a byte is available PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY); // And process it if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status)) { // Handle packet with waypoint component mavlink_wpm_message_handler(&rx_msg); // Handle packet with parameter component mavlink_pm_message_handler(mavlink_chan, &rx_msg); switch (rx_msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { // Check if this is the gcs mavlink_heartbeat_t beat; mavlink_msg_heartbeat_decode(&rx_msg, &beat); if (beat.type == MAV_TYPE_GCS) { // Got heartbeat from the GCS, we're good! lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS; } } break; case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(&rx_msg, &mode); // Check if this system should change the mode if (mode.target_system == mavlink_system.sysid) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (mode.base_mode) { case MAV_MODE_MANUAL_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_MANUAL_DISARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; } break; case MAV_MODE_PREFLIGHT: { flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; } break; case MAV_MODE_STABILIZE_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_GUIDED_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_AUTO_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; } bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL); if (newHilEnabled != hilEnabled) { if (newHilEnabled) { // READ-ONLY flag write to ActuatorCommand UAVObjMetadata meta; UAVObjHandle handle = ActuatorCommandHandle(); UAVObjGetMetadata(handle, &meta); meta.access = ACCESS_READONLY; UAVObjSetMetadata(handle, &meta); mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION"); mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++"); mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS"); } else { // READ-ONLY flag write to ActuatorCommand UAVObjMetadata meta; UAVObjHandle handle = ActuatorCommandHandle(); UAVObjGetMetadata(handle, &meta); meta.access = ACCESS_READWRITE; UAVObjSetMetadata(handle, &meta); mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION"); mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++"); mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS"); } } hilEnabled = newHilEnabled; FlightStatusSet(&flightStatus); // Check HIL bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL); enableHil(hilEnabled); } } break; case MAVLINK_MSG_ID_HIL_STATE: { if (hilEnabled) { mavlink_hil_state_t hil; mavlink_msg_hil_state_decode(&rx_msg, &hil); // Write GPSPosition GPSPositionData gps; GPSPositionGet(&gps); gps.Altitude = hil.alt/10; gps.Latitude = hil.lat/10; gps.Longitude = hil.lon/10; GPSPositionSet(&gps); // Write PositionActual PositionActualData pos; PositionActualGet(&pos); // FIXME WRITE POSITION HERE PositionActualSet(&pos); // Write AttitudeActual AttitudeActualData att; AttitudeActualGet(&att); att.Roll = hil.roll; att.Pitch = hil.pitch; att.Yaw = hil.yaw; // FIXME //att.RollSpeed = hil.rollspeed; //att.PitchSpeed = hil.pitchspeed; //att.YawSpeed = hil.yawspeed; // Convert to quaternion formulation RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); // Write AttitudeActual AttitudeActualSet(&att); // Write AttitudeRaw AttitudeRawData raw; AttitudeRawGet(&raw); raw.gyros[0] = hil.rollspeed; raw.gyros[1] = hil.pitchspeed; raw.gyros[2] = hil.yawspeed; raw.accels[0] = hil.xacc; raw.accels[1] = hil.yacc; raw.accels[2] = hil.zacc; // raw.magnetometers[0] = hil.xmag; // raw.magnetometers[0] = hil.ymag; // raw.magnetometers[0] = hil.zmag; AttitudeRawSet(&raw); } } break; case MAVLINK_MSG_ID_COMMAND_LONG: { // FIXME Implement } break; } } } }