Changeset 84 in flair-src for branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp
- Timestamp:
- 09/23/16 11:13:25 (8 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp
r78 r84 43 43 debug_messages = true; 44 44 45 heartbeat_period = HEARTBEAT_DEFAULT_PERIOD; 46 45 47 resetCommandAck(); 46 48 resetMissionAck(); … … 50 52 sendingPosition = false; 51 53 sendingSystemStatus = false; 52 sendingAttitudeInterval = -1;53 sendingPositionInterval = -1;54 sendingSystemStatusInterval = -1;54 sendingAttitudeInterval = ATTITUDE_DEFAULT_PERIOD; 55 sendingPositionInterval = POSITION_DEFAULT_PERIOD; 56 sendingSystemStatusInterval = STATUS_DEFAULT_PERIOD; 55 57 56 58 // mission items 57 59 missionFirst = 0; 58 60 missionLast = 0; 59 mission_items_loop = false;61 //mission_items_loop = false; 60 62 missionActive = false; // if the mission is launched 61 63 … … 66 68 autopilot_id = 0; // autopilot component id 67 69 component_id = 0; // companion computer component id 70 71 // setpoint 72 recvSetpointX = 0; 73 recvSetpointY = 0; 68 74 69 75 // mavlink_command_long_t com = { 0 }; … … 210 216 length_to_send = mavlink_msg_to_send_buffer(buff_out, &message); 211 217 something_to_send = true; 212 printf("[SENT] Heartbeat\n");218 //printf("[SENT] Heartbeat\n"); 213 219 } 214 220 … … 219 225 length_to_send = mavlink_msg_to_send_buffer(buff_out,&message); 220 226 something_to_send = true; 221 printf("[SENT] System status\n");227 //printf("[SENT] System status\n"); 222 228 } 223 229 … … 358 364 mavlink_msg_set_position_target_local_ned_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, targetSystem, targetComponent, coordinateFrame, typeMask, x, y, z, vx, vy, vz, afx, afy, afz, yaw, yaw_rate); 359 365 length_to_send = mavlink_msg_to_send_buffer(buff_out,&message); 366 printf("[SENT] SET_POSITION_TARGET_LOCAL_NED x = %.2f y = %.2f\n", x, y); 367 something_to_send = true; 368 } 369 370 // send PositionTargetLocalNED message 371 void MavlinkUDP::sendPositionTargetLocalNED(uint32_t time_boot_ms, uint8_t coordinateFrame, uint16_t typeMask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) { 372 mavlink_message_t message; 373 mavlink_msg_position_target_local_ned_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, coordinateFrame, typeMask, x, y, z, vx, vy, vz, afx, afy, afz, yaw, yaw_rate); 374 length_to_send = mavlink_msg_to_send_buffer(buff_out,&message); 360 375 something_to_send = true; 361 376 } … … 365 380 mavlink_message_t message; 366 381 mavlink_msg_set_position_target_global_int_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, targetSystem, targetComponent, coordinateFrame, typeMask, lat_int, lon_int, alt, vx, vy, vz, afx, afy, afz, yaw, yaw_rate); 382 length_to_send = mavlink_msg_to_send_buffer(buff_out,&message); 383 something_to_send = true; 384 } 385 386 // send PositionTargetGlobalInt message 387 void MavlinkUDP::sendPositionTargetGlobalInt(uint32_t time_boot_ms, uint8_t coordinateFrame, uint16_t typeMask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) { 388 mavlink_message_t message; 389 mavlink_msg_position_target_global_int_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, coordinateFrame, typeMask, lat_int, lon_int, alt, vx, vy, vz, afx, afy, afz, yaw, yaw_rate); 367 390 length_to_send = mavlink_msg_to_send_buffer(buff_out,&message); 368 391 something_to_send = true; … … 374 397 375 398 // command Waypoint 376 void MavlinkUDP::cmd Waypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude) {399 void MavlinkUDP::cmdNavWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude) { 377 400 sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_WAYPOINT, 0, holdTime, proximityRadius, passRadius, desiredYaw, latitude, longitude, altitude); 378 401 } … … 384 407 385 408 // command Land 386 void MavlinkUDP::cmd Land(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) {409 void MavlinkUDP::cmdNavLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) { 387 410 sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_LAND, 0, abortAlt, 0, 0, desiredYaw, latitude, longitude, altitude); 388 411 } … … 394 417 395 418 // command TakeOff 396 void MavlinkUDP::cmd Takeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) {419 void MavlinkUDP::cmdNavTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) { 397 420 sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_TAKEOFF, 0, desiredPitch, 0, 0, magnetometerYaw, latitude, longitude, altitude); 398 421 } … … 403 426 } 404 427 428 // command DoPauseContinue 429 void MavlinkUDP::cmdDoPauseContinue(uint8_t targetSystem, uint8_t targetComponent, uint8_t pauseContinue) { 430 sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_PAUSE_CONTINUE, 0, pauseContinue, 0, 0, 0, 0, 0, 0); 431 } 432 405 433 // command DoSetHome 406 434 void MavlinkUDP::cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent) { … … 408 436 } 409 437 438 // command GetHomePosition 439 void MavlinkUDP::cmdGetHomePosition(uint8_t targetSystem, uint8_t targetComponent) { 440 sendCommandLong(targetSystem, targetComponent, MAV_CMD_GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, 0); 441 } 442 410 443 // command MissionStart 411 444 void MavlinkUDP::cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem) { … … 424 457 425 458 // command ReturnToLaunch 426 void MavlinkUDP::cmd ReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {459 void MavlinkUDP::cmdNavReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) { 427 460 sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0); 428 461 } … … 432 465 // for autopilot and onboardComputer, see TMAV_REBOOT_SHUTDOWN_PARAM 433 466 sendCommandLong(targetSystem, targetComponent, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, autopilot, onboardComputer, 0, 0, 0, 0, 0); 467 } 468 469 470 // callback Heartbeat 471 void MavlinkUDP::startHeartbeat(uint64_t period) { 472 timerHeartbeat.start(period, std::bind(&MavlinkUDP::callbackHeartbeat,this)); 473 } 474 void MavlinkUDP::stopHeartbeat() { 475 timerHeartbeat.stop(); 476 } 477 void MavlinkUDP::callbackHeartbeat() { 478 sendHeartbeat(); 479 } 480 481 // callback SystemTime 482 void MavlinkUDP::startSystemTime(uint64_t period) { 483 timerSystemTime.start(period, std::bind(&MavlinkUDP::callbackSystemTime,this)); 484 } 485 void MavlinkUDP::stopSystemTime() { 486 timerSystemTime.stop(); 487 } 488 void MavlinkUDP::callbackSystemTime() { 489 sendSystemTime(); 490 } 491 492 // stop the callback function timers 493 void MavlinkUDP::stopAllCallbackTimers() { 494 stopHeartbeat(); 495 stopSystemTime(); 434 496 } 435 497 … … 500 562 // clear the queue 501 563 clearMissionPlan(); 502 // 503 // TODO signal the app that the vector is cleared 504 // 564 MavlinkItem mavItem; 565 mavItem.id = message.msgid; 566 missionCommands.push(mavItem); 567 //printf("[DEBUG] sysid = %d compid = %d\n", message.sysid, message.compid); 505 568 sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED); 506 569 break; … … 592 655 case MAV_CMD_NAV_TAKEOFF: 593 656 { 594 printf("[RECV] MISSION ITEM: TAKEOFF\n"); 595 MissionItem item; 596 item.itemID = command; 597 item.sequence = mavlink_msg_mission_item_get_seq(&message); 598 item.yawAngle = mavlink_msg_mission_item_get_param4(&message); 599 item.latitude = mavlink_msg_mission_item_get_x(&message); 600 item.longitude = mavlink_msg_mission_item_get_y(&message); 601 item.altitude = mavlink_msg_mission_item_get_z(&message); 602 missionPlan.push(item); 657 MavlinkItem mavItem; 658 mavItem.id = command; 659 mavItem.sequence = mavlink_msg_mission_item_get_seq(&message); 660 mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message); 661 mavItem.latitude = mavlink_msg_mission_item_get_x(&message); 662 mavItem.longitude = mavlink_msg_mission_item_get_y(&message); 663 mavItem.altitude = mavlink_msg_mission_item_get_z(&message); 664 // adding commands to the mission plan 665 missionPlan.push(mavItem); 666 //missionCommands.push(mavItem); 667 printf("[RECV] MISSION_ITEM: TAKEOFF\n"); 603 668 break; 604 669 } 605 670 case MAV_CMD_NAV_LAND: 606 671 { 607 printf("[RECV] MISSION ITEM: LAND\n"); 608 MissionItem item; 609 item.itemID = command; 610 item.sequence = mavlink_msg_mission_item_get_seq(&message); 611 item.abortAltitude = mavlink_msg_mission_item_get_param1(&message); 612 item.yawAngle = mavlink_msg_mission_item_get_param4(&message); 613 item.latitude = mavlink_msg_mission_item_get_x(&message); 614 item.longitude = mavlink_msg_mission_item_get_y(&message); 615 item.altitude = mavlink_msg_mission_item_get_z(&message); 616 missionPlan.push(item); 672 MavlinkItem mavItem; 673 mavItem.id = command; 674 mavItem.sequence = mavlink_msg_mission_item_get_seq(&message); 675 mavItem.abort_altitude = mavlink_msg_mission_item_get_param1(&message); 676 mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message); 677 mavItem.latitude = mavlink_msg_mission_item_get_x(&message); 678 mavItem.longitude = mavlink_msg_mission_item_get_y(&message); 679 mavItem.altitude = mavlink_msg_mission_item_get_z(&message); 680 // adding commands to the mission plan 681 missionPlan.push(mavItem); 682 printf("[RECV] MISSION_ITEM: LAND\n"); 683 break; 684 } 685 case MAV_CMD_DO_JUMP: 686 { 687 MavlinkItem mavItem; 688 mavItem.id = command; 689 mavItem.sequence = mavlink_msg_mission_item_get_seq(&message); 690 mavItem.jump_sequence = mavlink_msg_mission_item_get_param1(&message); 691 mavItem.jump_repeat_count = mavlink_msg_mission_item_get_param2(&message); 692 // adding commands to the mission plan 693 missionPlan.push(mavItem); 694 printf("[RECV] MISSION_ITEM: MAV_CMD_DO_JUMP seq %d repeat %d\n", mavItem.jump_sequence, mavItem.jump_repeat_count); 695 break; 696 } 697 case MAV_CMD_NAV_RETURN_TO_LAUNCH: 698 { 699 MavlinkItem mavItem; 700 mavItem.id = command; 701 mavItem.sequence = mavlink_msg_mission_item_get_seq(&message); 702 // adding commands to the mission plan 703 missionPlan.push(mavItem); 704 printf("[RECV] MISSION_ITEM: MAV_CMD_NAV_RETURN_TO_LAUNCH\n"); 617 705 break; 618 706 } 619 707 case MAV_CMD_NAV_WAYPOINT: 620 708 { 621 MissionItem item; 622 item.itemID = command; 623 item.sequence = mavlink_msg_mission_item_get_seq(&message); 624 //item.type = (uint8_t)mavlink_msg_mission_item_get_param3(&message); 625 item.holdTime = mavlink_msg_mission_item_get_param1(&message); 626 item.yawAngle = mavlink_msg_mission_item_get_param4(&message); 627 item.latitude = mavlink_msg_mission_item_get_x(&message); 628 item.longitude = mavlink_msg_mission_item_get_y(&message); 629 item.altitude = mavlink_msg_mission_item_get_z(&message); 630 missionPlan.push(item); 631 //printf("[RECV] WAYPOINT: seq = %d, x = %.2f, y = %.2f, type = %d\n", mavlink_msg_mission_item_get_seq(&message), recvWaypoint.m_x, recvWaypoint.m_y, (int)recvWaypoint.m_type); 632 printf("[RECV] WAYPOINT: seq = %d, lat = %.2f, long = %.2f, alt = %.2f\n", item.sequence, item.latitude, item.longitude, item.altitude); 709 MavlinkItem mavItem; 710 mavItem.id = command; 711 mavItem.sequence = mavlink_msg_mission_item_get_seq(&message); 712 mavItem.hold_time = mavlink_msg_mission_item_get_param1(&message); 713 mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message); 714 mavItem.latitude = mavlink_msg_mission_item_get_x(&message); 715 mavItem.longitude = mavlink_msg_mission_item_get_y(&message); 716 mavItem.altitude = mavlink_msg_mission_item_get_z(&message); 717 // adding commands to the mission plan 718 missionPlan.push(mavItem); 719 printf("[RECV] MISSION_ITEM: WAYPOINT seq = %d, lat = %.2f, long = %.2f, alt = %.2f\n", mavItem.sequence, mavItem.latitude, mavItem.longitude, mavItem.altitude); 633 720 break; 634 721 } … … 719 806 case MAVLINK_MSG_ID_LOCAL_POSITION_NED: 720 807 { 721 printf(" MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");808 printf("[RECV] MAVLINK_MSG_ID_LOCAL_POSITION_NED\n"); 722 809 mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned)); 723 810 current_messages.time_stamps.local_position_ned = get_time_usec(); 724 811 this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned; 812 // insert it into the command queue 813 MavlinkItem mavItem; 814 mavItem.id = message.msgid; 815 mavItem.time_boot_ms = mavlink_msg_local_position_ned_get_time_boot_ms(&message); 816 mavItem.x = mavlink_msg_local_position_ned_get_x(&message); 817 mavItem.y = mavlink_msg_local_position_ned_get_y(&message); 818 mavItem.z = mavlink_msg_local_position_ned_get_z(&message); 819 mavItem.vx = mavlink_msg_local_position_ned_get_vx(&message); 820 mavItem.vy = mavlink_msg_local_position_ned_get_vy(&message); 821 mavItem.vz = mavlink_msg_local_position_ned_get_vz(&message); 822 missionCommands.push(mavItem); 823 recvSetpointX = mavItem.x; 824 recvSetpointY = mavItem.y; 725 825 break; 726 826 } … … 728 828 case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: 729 829 { 730 printf(" MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");830 printf("[RECV] MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n"); 731 831 mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int)); 732 832 current_messages.time_stamps.global_position_int = get_time_usec(); 733 833 this_timestamps.global_position_int = current_messages.time_stamps.global_position_int; 734 //USB_port->write_message(message); 834 // insert it into the command queue 835 MavlinkItem mavItem; 836 mavItem.id = message.msgid; 837 mavItem.time_boot_ms = mavlink_msg_global_position_int_get_time_boot_ms(&message); 838 mavItem.latitude = (float)mavlink_msg_global_position_int_get_lat(&message); 839 mavItem.longitude = (float)mavlink_msg_global_position_int_get_lon(&message); 840 mavItem.altitude = (float)mavlink_msg_global_position_int_get_alt(&message); 841 mavItem.relative_alt = (float)mavlink_msg_global_position_int_get_relative_alt(&message); 842 mavItem.vx = (float)mavlink_msg_global_position_int_get_vx(&message); 843 mavItem.vy = (float)mavlink_msg_global_position_int_get_vy(&message); 844 mavItem.vz = (float)mavlink_msg_global_position_int_get_vz(&message); 845 mavItem.yaw_angle = (float)mavlink_msg_global_position_int_get_hdg(&message); // heading 846 missionCommands.push(mavItem); 847 break; 848 } 849 850 case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: 851 { 852 mavlink_msg_set_position_target_local_ned_decode(&message, &(current_messages.set_position_target_local_ned)); 853 current_messages.time_stamps.set_position_target_local_ned = get_time_usec(); 854 this_timestamps.set_position_target_local_ned = current_messages.time_stamps.set_position_target_local_ned; 855 // insert it into the command queue 856 MavlinkItem mavItem; 857 mavItem.id = message.msgid; 858 mavItem.time_boot_ms = mavlink_msg_set_position_target_local_ned_get_time_boot_ms(&message); 859 mavItem.target_system = mavlink_msg_set_position_target_local_ned_get_target_system(&message); 860 mavItem.target_component = mavlink_msg_set_position_target_local_ned_get_target_component(&message); 861 mavItem.coordinate_frame = mavlink_msg_set_position_target_local_ned_get_coordinate_frame(&message); 862 mavItem.type_mask = mavlink_msg_set_position_target_local_ned_get_type_mask(&message); 863 mavItem.x = mavlink_msg_set_position_target_local_ned_get_x(&message); 864 mavItem.y = mavlink_msg_set_position_target_local_ned_get_y(&message); 865 mavItem.z = mavlink_msg_set_position_target_local_ned_get_z(&message); 866 mavItem.vx = mavlink_msg_set_position_target_local_ned_get_vx(&message); 867 mavItem.vy = mavlink_msg_set_position_target_local_ned_get_vy(&message); 868 mavItem.vz = mavlink_msg_set_position_target_local_ned_get_vz(&message); 869 mavItem.afx = mavlink_msg_set_position_target_local_ned_get_afx(&message); 870 mavItem.afy = mavlink_msg_set_position_target_local_ned_get_afy(&message); 871 mavItem.afz = mavlink_msg_set_position_target_local_ned_get_afz(&message); 872 mavItem.yaw = mavlink_msg_set_position_target_local_ned_get_yaw(&message); 873 mavItem.yaw_rate = mavlink_msg_set_position_target_local_ned_get_yaw_rate(&message); 874 missionCommands.push(mavItem); 875 printf("[RECV] MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED x = %.2f y = %.2f\n", mavItem.x, mavItem.y); 735 876 break; 736 877 } … … 738 879 case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: 739 880 { 740 printf(" MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");881 printf("[RECV] MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n"); 741 882 mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned)); 742 883 current_messages.time_stamps.position_target_local_ned = get_time_usec(); 743 884 this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned; 885 // insert it into the command queue 886 MavlinkItem mavItem; 887 mavItem.id = message.msgid; 888 mavItem.time_boot_ms = mavlink_msg_position_target_local_ned_get_time_boot_ms(&message); 889 mavItem.coordinate_frame = mavlink_msg_position_target_local_ned_get_coordinate_frame(&message); 890 mavItem.type_mask = mavlink_msg_position_target_local_ned_get_type_mask(&message); 891 mavItem.x = mavlink_msg_position_target_local_ned_get_x(&message); 892 mavItem.y = mavlink_msg_position_target_local_ned_get_y(&message); 893 mavItem.z = mavlink_msg_position_target_local_ned_get_z(&message); 894 mavItem.vx = mavlink_msg_position_target_local_ned_get_vx(&message); 895 mavItem.vy = mavlink_msg_position_target_local_ned_get_vy(&message); 896 mavItem.vz = mavlink_msg_position_target_local_ned_get_vz(&message); 897 mavItem.afx = mavlink_msg_position_target_local_ned_get_afx(&message); 898 mavItem.afy = mavlink_msg_position_target_local_ned_get_afy(&message); 899 mavItem.afz = mavlink_msg_position_target_local_ned_get_afz(&message); 900 mavItem.yaw = mavlink_msg_position_target_local_ned_get_yaw(&message); 901 mavItem.yaw_rate = mavlink_msg_position_target_local_ned_get_yaw_rate(&message); 902 missionCommands.push(mavItem); 903 break; 904 } 905 906 case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: 907 { 908 printf("[RECV] MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT\n"); 909 mavlink_msg_set_position_target_global_int_decode(&message, &(current_messages.set_position_target_global_int)); 910 current_messages.time_stamps.set_position_target_global_int = get_time_usec(); 911 this_timestamps.set_position_target_global_int = current_messages.time_stamps.set_position_target_global_int; 912 // insert it into the command queue 913 MavlinkItem mavItem; 914 mavItem.id = message.msgid; 915 mavItem.time_boot_ms = mavlink_msg_set_position_target_global_int_get_time_boot_ms(&message); 916 mavItem.target_system = mavlink_msg_set_position_target_global_int_get_target_system(&message); 917 mavItem.target_component = mavlink_msg_set_position_target_global_int_get_target_component(&message); 918 mavItem.coordinate_frame = mavlink_msg_set_position_target_global_int_get_coordinate_frame(&message); 919 mavItem.type_mask = mavlink_msg_set_position_target_global_int_get_type_mask(&message); 920 mavItem.lat_int = mavlink_msg_set_position_target_global_int_get_lat_int(&message); 921 mavItem.lon_int = mavlink_msg_set_position_target_global_int_get_lon_int(&message); 922 mavItem.altitude = mavlink_msg_set_position_target_global_int_get_alt(&message); 923 mavItem.vx = mavlink_msg_set_position_target_global_int_get_vx(&message); 924 mavItem.vy = mavlink_msg_set_position_target_global_int_get_vy(&message); 925 mavItem.vz = mavlink_msg_set_position_target_global_int_get_vz(&message); 926 mavItem.afx = mavlink_msg_set_position_target_global_int_get_afx(&message); 927 mavItem.afy = mavlink_msg_set_position_target_global_int_get_afy(&message); 928 mavItem.afz = mavlink_msg_set_position_target_global_int_get_afz(&message); 929 mavItem.yaw = mavlink_msg_set_position_target_global_int_get_yaw(&message); 930 mavItem.yaw_rate = mavlink_msg_set_position_target_global_int_get_yaw_rate(&message); 931 missionCommands.push(mavItem); 744 932 break; 745 933 } … … 747 935 case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT: 748 936 { 749 printf(" MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");937 printf("[RECV] MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n"); 750 938 mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int)); 751 939 current_messages.time_stamps.position_target_global_int = get_time_usec(); 752 940 this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int; 941 // insert it into the command queue 942 MavlinkItem mavItem; 943 mavItem.id = message.msgid; 944 mavItem.time_boot_ms = mavlink_msg_position_target_global_int_get_time_boot_ms(&message); 945 mavItem.coordinate_frame = mavlink_msg_position_target_global_int_get_coordinate_frame(&message); 946 mavItem.type_mask = mavlink_msg_position_target_global_int_get_type_mask(&message); 947 mavItem.lat_int = mavlink_msg_position_target_global_int_get_lat_int(&message); 948 mavItem.lon_int = mavlink_msg_position_target_global_int_get_lon_int(&message); 949 mavItem.altitude = mavlink_msg_position_target_global_int_get_alt(&message); 950 mavItem.vx = mavlink_msg_position_target_global_int_get_vx(&message); 951 mavItem.vy = mavlink_msg_position_target_global_int_get_vy(&message); 952 mavItem.vz = mavlink_msg_position_target_global_int_get_vz(&message); 953 mavItem.afx = mavlink_msg_position_target_global_int_get_afx(&message); 954 mavItem.afy = mavlink_msg_position_target_global_int_get_afy(&message); 955 mavItem.afz = mavlink_msg_position_target_global_int_get_afz(&message); 956 mavItem.yaw = mavlink_msg_position_target_global_int_get_yaw(&message); 957 mavItem.yaw_rate = mavlink_msg_position_target_global_int_get_yaw_rate(&message); 958 missionCommands.push(mavItem); 753 959 break; 754 960 } … … 756 962 case MAVLINK_MSG_ID_HIGHRES_IMU: 757 963 { 758 //printf(" MAVLINK_MSG_ID_HIGHRES_IMU\n");964 //printf("[RECV] MAVLINK_MSG_ID_HIGHRES_IMU\n"); 759 965 mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu)); 760 966 current_messages.time_stamps.highres_imu = get_time_usec(); … … 781 987 printf("Received command %d\n", current_messages.command_long.command); 782 988 switch(current_messages.command_long.command) { 783 case MAV_CMD_DO_SET_PARAMETER: 989 case MAV_CMD_SET_MESSAGE_INTERVAL: 990 { 991 int64_t desired_interval = (int64_t)current_messages.command_long.param2; 992 printf("[RECV] COMMAND_LONG: MAV_CMD_SET_MESSAGE_INTERVAL %d %ld\n", (int)current_messages.command_long.param1, (int64_t)current_messages.command_long.param2); 993 // adding to the command queue 994 MavlinkItem mavItem; 995 mavItem.id = mavlink_msg_command_long_get_command(&message); 996 mavItem.callback_message = (uint16_t)mavlink_msg_command_long_get_param1(&message); 997 (desired_interval == -1)?(mavItem.callback_flag = false):(mavItem.callback_flag = true); 784 998 switch((int)current_messages.command_long.param1) { 785 case MAV_LOOP_MISSION_ITEMS: 786 if((int)current_messages.command_long.param2) { 787 itemsLoop(); 999 case MAVLINK_MSG_ID_HEARTBEAT: 1000 { 1001 if(current_messages.command_long.param2 == -1) { 1002 stopHeartbeat(); 1003 } else if(current_messages.command_long.param2 == 0) { 1004 startHeartbeat(HEARTBEAT_DEFAULT_PERIOD); 788 1005 } else { 789 itemsNoLoop();1006 startHeartbeat(desired_interval); 790 1007 } 791 792 1008 break; 1009 } 1010 case MAVLINK_MSG_ID_LOCAL_POSITION_NED: 1011 { 1012 if(current_messages.command_long.param2 == -1) { 1013 sendingPosition = false; 1014 } else if(current_messages.command_long.param2 == 0) { 1015 sendingPositionInterval = POSITION_DEFAULT_PERIOD; 1016 sendingPosition = true; 1017 } else { 1018 sendingPositionInterval = desired_interval; 1019 sendingPosition = true; 1020 } 1021 mavItem.callback_period = sendingPositionInterval; 1022 break; 1023 } 1024 case MAVLINK_MSG_ID_ATTITUDE: 1025 { 1026 if(current_messages.command_long.param2 == -1) { 1027 sendingAttitude = false; 1028 } else if(current_messages.command_long.param2 == 0) { 1029 sendingAttitudeInterval = ATTITUDE_DEFAULT_PERIOD; 1030 sendingAttitude = true; 1031 } else { 1032 sendingAttitudeInterval = desired_interval; 1033 sendingAttitude = true; 1034 } 1035 mavItem.callback_period = sendingAttitudeInterval; 1036 break; 1037 } 1038 case MAVLINK_MSG_ID_SYS_STATUS: 1039 { 1040 if(current_messages.command_long.param2 == -1) { 1041 sendingSystemStatus = false; 1042 } else if(current_messages.command_long.param2 == 0) { 1043 sendingSystemStatusInterval = STATUS_DEFAULT_PERIOD; 1044 sendingSystemStatus = true; 1045 } else { 1046 sendingSystemStatusInterval = desired_interval; 1047 sendingSystemStatus = true; 1048 } 1049 mavItem.callback_period = sendingSystemStatusInterval; 1050 break; 1051 } 793 1052 default: 794 1053 break; 795 1054 } 796 797 break; 798 case MAV_CMD_SET_MESSAGE_INTERVAL: 799 cout << "[DEBUG] callback interval value: " << (int64_t)current_messages.command_long.param2 << endl; 800 int64_t desired_interval; 801 desired_interval = (int64_t)current_messages.command_long.param2; 802 switch((int)current_messages.command_long.param1) { 803 case MAVLINK_MSG_ID_HEARTBEAT: 804 if(current_messages.command_long.param2 == -1) { 805 // stop sending position 806 sendingHeartbeat = false; 807 } else { 808 // start sending position 809 sendingHeartbeatInterval = desired_interval; 810 sendingHeartbeat = true; 811 } 812 break; 813 case MAVLINK_MSG_ID_LOCAL_POSITION_NED: 814 if(current_messages.command_long.param2 == -1) { 815 // stop sending position 816 sendingPosition = false; 817 } else { 818 // start sending position 819 sendingPositionInterval = desired_interval; 820 sendingPosition = true; 821 } 822 break; 823 case MAVLINK_MSG_ID_ATTITUDE: 824 if(current_messages.command_long.param2 == -1) { 825 // stop sending attitude 826 sendingAttitude = false; 827 } else { 828 // start sending attitude 829 sendingAttitudeInterval = desired_interval; 830 sendingAttitude = true; 831 } 832 break; 833 case MAVLINK_MSG_ID_SYS_STATUS: 834 if(current_messages.command_long.param2 == -1) { 835 // stop sending status 836 sendingSystemStatus = false; 837 } else { 838 // start sending status 839 sendingSystemStatusInterval = desired_interval; 840 sendingSystemStatus = true; 841 } 842 break; 843 default: 844 break; 845 } 846 847 break; 1055 // finally add the command to the queue 1056 missionCommands.push(mavItem); 1057 break; 1058 } 1059 // * Commands queue: 1060 // * 176 MAV_CMD_DO_SET_MODE (mode MAV_MODE) 1061 // * 179 MAV_CMD_DO_SET_HOME (use current, lat, long, altitude) 1062 // * 193 MAV_CMD_DO_PAUSE_CONTINUE (holdContinue: 0=hold, 1=continue) 1063 // * 300 MAV_CMD_MISSION_START (first item, last item) 1064 // * 410 MAV_CMD_GET_HOME_POS (empty) 1065 // * 20 MAV_CMD_NAV_RETURN_TO_LAUNCH (empty) 1066 848 1067 case MAV_CMD_MISSION_START: 849 1068 { 850 M issionCommand missCom;851 m issCom.commandId = mavlink_msg_command_long_get_command(&message);852 m issCom.firstItem = (uint16_t)mavlink_msg_command_long_get_param1(&message);853 m issCom.lastItem = (uint16_t)mavlink_msg_command_long_get_param2(&message);854 missionCommands.push(m issCom);855 printf("[RECV] MAV_CMD_MISSION_START: firstItem = %d, lastItem = %d\n", missCom.firstItem, missCom.lastItem);1069 MavlinkItem mavItem; 1070 mavItem.id = mavlink_msg_command_long_get_command(&message); 1071 mavItem.first_item = (uint16_t)mavlink_msg_command_long_get_param1(&message); 1072 mavItem.last_item = (uint16_t)mavlink_msg_command_long_get_param2(&message); 1073 missionCommands.push(mavItem); 1074 printf("[RECV] COMMAND_LONG: MAV_CMD_MISSION_START firstItem = %d, lastItem = %d\n", mavItem.first_item, mavItem.last_item); 856 1075 missionStarted(); 857 1076 break; 858 1077 } 859 case MAV_CMD_DO_LAND_START:860 // TODO do land start861 break;862 1078 case MAV_CMD_DO_SET_HOME: 863 1079 { 864 M issionCommand missCom;865 m issCom.commandId = mavlink_msg_command_long_get_command(&message);866 m issCom.useCurrent = (uint8_t)mavlink_msg_command_long_get_param1(&message);867 m issCom.latitude = mavlink_msg_command_long_get_param5(&message);868 m issCom.longitude = mavlink_msg_command_long_get_param6(&message);869 m issCom.altitude = mavlink_msg_command_long_get_param7(&message);870 missionCommands.push(m issCom);871 printf("[RECV] SET HOME: use_current = %d, lat = %.2f, long = %.2f, altitude = %.2f\n", missCom.useCurrent, missCom.latitude, missCom.longitude, missCom.altitude);1080 MavlinkItem mavItem; 1081 mavItem.id = mavlink_msg_command_long_get_command(&message); 1082 mavItem.use_current = (uint8_t)mavlink_msg_command_long_get_param1(&message); 1083 mavItem.latitude = mavlink_msg_command_long_get_param5(&message); 1084 mavItem.longitude = mavlink_msg_command_long_get_param6(&message); 1085 mavItem.altitude = mavlink_msg_command_long_get_param7(&message); 1086 missionCommands.push(mavItem); 1087 printf("[RECV] COMMAND_LONG: MAV_CMD_DO_SET_HOME use_current = %d, lat = %.2f, long = %.2f, altitude = %.2f\n", mavItem.use_current, mavItem.latitude, mavItem.longitude, mavItem.altitude); 872 1088 break; 873 1089 } 874 1090 case MAV_CMD_GET_HOME_POSITION: 875 1091 { 876 MissionCommand missCom; 877 missCom.commandId = mavlink_msg_command_long_get_command(&message); 878 missionCommands.push(missCom); 879 printf("[RECV] GET HOME POSITION\n"); 1092 MavlinkItem mavItem; 1093 mavItem.id = mavlink_msg_command_long_get_command(&message); 1094 missionCommands.push(mavItem); 1095 // TODO respond with a home position 1096 printf("[RECV] COMMAND_LONG: MAV_CMD_GET_HOME_POSITION\n"); 880 1097 break; 881 1098 } 882 1099 case MAV_CMD_DO_SET_MODE: 883 1100 { 884 MissionCommand missCom; 885 missCom.commandId = mavlink_msg_command_long_get_command(&message); 886 missCom.mode = mavlink_msg_command_long_get_param1(&message); 887 missionCommands.push(missCom); 888 printf("[RECV] SET MODE: %d\n", missCom.mode); 889 break; 890 } 891 case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: 892 // TODO request autopilot capabilities 893 break; 1101 MavlinkItem mavItem; 1102 mavItem.id = mavlink_msg_command_long_get_command(&message); 1103 mavItem.mode = mavlink_msg_command_long_get_param1(&message); 1104 missionCommands.push(mavItem); 1105 printf("[RECV] COMMAND_LONG: MAV_CMD_DO_SET_MODE %d\n", mavItem.mode); 1106 break; 1107 } 894 1108 case MAV_CMD_NAV_RETURN_TO_LAUNCH: 895 1109 { 896 MissionItem missItem; 897 missItem.itemID = mavlink_msg_command_long_get_command(&message); 898 missionPlan.push(missItem); 899 MissionCommand missCom; 900 missCom.commandId = mavlink_msg_command_long_get_command(&message); 901 missionCommands.push(missCom); 902 printf("[RECV] Return to launch.\n"); 903 break; 904 } 905 case MAV_CMD_DO_REPOSITION: 906 // TODO reposition 907 printf("[TODO] Do reposition.\n"); 908 break; 1110 MavlinkItem mavItem; 1111 mavItem.id = mavlink_msg_command_long_get_command(&message); 1112 missionCommands.push(mavItem); 1113 printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_RETURN_TO_LAUNCH\n"); 1114 break; 1115 } 909 1116 case MAV_CMD_NAV_TAKEOFF: 910 // TODO takeoff 911 printf("[TODO] Execute takeoff.\n"); 912 break; 1117 { 1118 MavlinkItem mavItem; 1119 mavItem.id = mavlink_msg_command_long_get_command(&message); 1120 mavItem.min_pitch = mavlink_msg_command_long_get_param1(&message); 1121 mavItem.yaw_angle = mavlink_msg_command_long_get_param4(&message); 1122 mavItem.latitude = mavlink_msg_command_long_get_param5(&message); 1123 mavItem.longitude = mavlink_msg_command_long_get_param6(&message); 1124 mavItem.altitude = mavlink_msg_command_long_get_param7(&message); 1125 // adding commands to the command queue 1126 missionCommands.push(mavItem); 1127 printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_TAKEOFF\n"); 1128 break; 1129 } 913 1130 case MAV_CMD_NAV_LAND: 914 // TODO emergency landing 915 printf("[TODO] Execute emergency landing.\n"); 916 break; 1131 { 1132 MavlinkItem mavItem; 1133 mavItem.id = mavlink_msg_command_long_get_command(&message); 1134 mavItem.abort_altitude = mavlink_msg_command_long_get_param1(&message); 1135 mavItem.desired_yaw = mavlink_msg_command_long_get_param4(&message); 1136 mavItem.latitude = mavlink_msg_command_long_get_param5(&message); 1137 mavItem.longitude = mavlink_msg_command_long_get_param6(&message); 1138 mavItem.altitude = mavlink_msg_command_long_get_param7(&message); 1139 // adding commands to the command queue 1140 missionCommands.push(mavItem); 1141 printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_LAND\n"); 1142 break; 1143 } 1144 case MAV_CMD_DO_PAUSE_CONTINUE: 1145 { 1146 MavlinkItem mavItem; 1147 mavItem.id = mavlink_msg_command_long_get_command(&message); 1148 mavItem.pause_continue = (uint16_t)mavlink_msg_command_long_get_param1(&message); 1149 missionCommands.push(mavItem); 1150 printf("[RECV] COMMAND_LONG: MAV_CMD_DO_PAUSE_CONTINUE pauseContinue = %d\n", mavItem.pause_continue); 1151 } 917 1152 case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: 1153 { 918 1154 // shutdown signal received 919 1155 shutdown_flag = true; 920 break; 921 case MAV_CMD_DO_JUMP: 922 { 923 // jump to a desired command in mission list, repeat a specified number of times 924 MissionItem missItem; 925 missItem.itemID = mavlink_msg_command_long_get_command(&message); 926 missItem.sequence = (uint16_t)mavlink_msg_command_long_get_param1(&message); 927 missItem.repeatCount = (uint16_t)mavlink_msg_command_long_get_param2(&message); 928 missionPlan.push(missItem); 929 MissionCommand missCom; 930 missCom.commandId = mavlink_msg_command_long_get_command(&message); 931 missCom.sequence = (uint16_t)mavlink_msg_command_long_get_param1(&message); 932 missCom.repeatCount = (uint16_t)mavlink_msg_command_long_get_param2(&message); 933 missionCommands.push(missCom); 934 printf("[RECV] MAV_CMD_DO_JUMP: seq = %d, repeat = %d\n", missItem.sequence, missItem.repeatCount); 935 } 936 case MAV_CMD_DO_PAUSE_CONTINUE: 937 { 938 MissionCommand missCom; 939 missCom.commandId = mavlink_msg_command_long_get_command(&message); 940 missCom.pauseContinue = (uint16_t)mavlink_msg_command_long_get_param1(&message); 941 missionCommands.push(missCom); 942 printf("[RECV] MAV_CMD_DO_PAUSE_CONTINUE: pauseContinue = %d\n", missCom.pauseContinue); 1156 printf("[RECV] COMMAND_LONG: MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN\n"); 1157 break; 1158 } 1159 case MAV_CMD_DO_REPOSITION: 1160 { 1161 // TODO reposition 1162 printf("[TODO] DO_REPOSITION\n"); 1163 break; 1164 } 1165 case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: 1166 { 1167 // TODO request autopilot capabilities 1168 break; 943 1169 } 944 1170 default:
Note:
See TracChangeset
for help on using the changeset viewer.