Changeset 84 in flair-src
- Timestamp:
- Sep 23, 2016, 11:13:25 AM (8 years ago)
- Location:
- branches/mavlink/tools/Controller/Mavlink/src
- Files:
-
- 1 added
- 8 edited
Legend:
- Unmodified
- Added
- Removed
-
branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.cpp
r77 r84 45 45 } 46 46 47 void GuiFlair::Mission Resume() {47 void GuiFlair::MissionPause() { 48 48 cout << "MavPlanner GuiFlair MissionResume" << endl; 49 49 } -
branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.h
r77 r84 40 40 virtual void MissionStart(); 41 41 virtual void MissionStop(); 42 virtual void Mission Resume();42 virtual void MissionPause(); 43 43 virtual void MissionSend(); 44 44 }; -
branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp
r83 r84 42 42 //outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort)); 43 43 44 missionPauseEnabled = false; 45 44 46 mavCom = new MavlinkUDP(outputAddress, outputPort); 45 47 } … … 78 80 } 79 81 80 void GuiGcs::MissionResume() { 81 cout << "MavPlanner GuiGcs MissionResume" << endl; 82 void GuiGcs::MissionPause() { 83 cout << "MavPlanner GuiGcs MissionPause" << endl; 84 // send pause/continue depending on the state of the button 85 if(missionPauseEnabled) { 86 missionPauseEnabled = false; 87 cout << "Sent pause command.\n"; 88 mavCom->cmdDoPauseContinue(mavCom->target.getSysID(), mavCom->target.getCompID(), 0); 89 } else { 90 missionPauseEnabled = true; 91 cout << "Sent continue command.\n"; 92 mavCom->cmdDoPauseContinue(mavCom->target.getSysID(), mavCom->target.getCompID(), 1); 93 } 94 mavCom->waitCommandAck(ACK_TIMEOUT); 82 95 } 83 96 -
branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h
r83 r84 48 48 virtual void MissionStart(); 49 49 virtual void MissionStop(); 50 virtual void Mission Resume();50 virtual void MissionPause(); 51 51 virtual void MissionSend(); 52 52 … … 59 59 MavlinkUDP* mavCom; 60 60 61 bool missionPauseEnabled; 62 61 63 void Initialize(); 62 64 void ParametersParse(const std::string& parametersString, std::array<float, 6>& parametersFloat); -
branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.cpp
r81 r84 52 52 btnStartMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Start mission"); 53 53 btnStopMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Stop mission"); 54 btn ResumeMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Resume mission");54 btnPauseMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Pause mission"); 55 55 56 56 // Add cmd group … … 132 132 MissionStop(); 133 133 } 134 if (btn ResumeMission->Clicked()) {135 Mission Resume();134 if (btnPauseMission->Clicked()) { 135 MissionPause(); 136 136 } 137 137 if (btnSendMission->Clicked()) { -
branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.h
r81 r84 52 52 virtual void MissionStart() = 0; 53 53 virtual void MissionStop() = 0; 54 virtual void Mission Resume() = 0;54 virtual void MissionPause() = 0; 55 55 virtual void MissionSend() = 0; 56 56 … … 66 66 flair::gui::PushButton* btnStartMission; 67 67 flair::gui::PushButton* btnStopMission; 68 flair::gui::PushButton* btn ResumeMission;68 flair::gui::PushButton* btnPauseMission; 69 69 70 70 // Add cmd groupbox -
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: -
branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h
r78 r84 30 30 #include <sys/time.h> 31 31 #include "include/common/mavlink.h" 32 #include "CallbackTimer.h" 32 33 33 34 // TODO these constants should be read from a file … … 48 49 49 50 #define ACK_TIMEOUT 100000 // us 50 51 #ifndef MAV_CUSTOM_PARAM 52 #define MAV_CUSTOM_PARAM 53 54 enum TMAVCustomParams { 55 MAV_LOOP_MISSION_ITEMS=42, 56 MAV_CUSTOM_PARAM1=43, 57 MAV_CUSTOM_PARAM2=44 58 }; 59 60 #endif 51 #define HEARTBEAT_DEFAULT_PERIOD 1000000 52 #define POSITION_DEFAULT_PERIOD 1000000 53 #define ATTITUDE_DEFAULT_PERIOD 1000000 54 #define STATUS_DEFAULT_PERIOD 1000000 55 56 //#ifndef MAV_CUSTOM_PARAM 57 //#define MAV_CUSTOM_PARAM 58 59 //enum TMAVCustomParams { 60 // MAV_LOOP_MISSION_ITEMS=42, 61 // MAV_CUSTOM_PARAM1=43, 62 // MAV_CUSTOM_PARAM2=44 63 //}; 64 65 //#endif 61 66 62 67 #ifndef REBOOT_SHUTDOWN_PARAMS … … 94 99 uint64_t local_position_ned; 95 100 uint64_t global_position_int; 101 uint64_t set_position_target_local_ned; 102 uint64_t set_position_target_global_int; 96 103 uint64_t position_target_local_ned; 97 104 uint64_t position_target_global_int; … … 118 125 local_position_ned = 0; 119 126 global_position_int = 0; 127 set_position_target_local_ned = 0; 128 set_position_target_global_int = 0; 120 129 position_target_local_ned = 0; 121 130 position_target_global_int = 0; … … 131 140 int sysid; 132 141 int compid; 133 // system time134 142 mavlink_system_time_t system_time; 135 // mission_request136 143 mavlink_mission_request_t mission_request; 137 // mission request list138 144 mavlink_mission_request_list_t mission_request_list; 139 // mission write partial list140 145 mavlink_mission_write_partial_list_t mission_write_partial_list; 141 // mission_reached142 146 mavlink_mission_item_reached_t mission_reached; 143 // mission_clear_all144 147 mavlink_mission_clear_all_t clear_all; 145 // command long146 148 mavlink_command_long_t command_long; 147 // command_ack148 149 mavlink_command_ack_t command_ack; 149 // timesync150 150 mavlink_timesync_t timesync; 151 // mission_count152 151 mavlink_mission_count_t mission_count; 153 // mission_ack154 152 mavlink_mission_ack_t mission_ack; 155 // mission_item156 153 mavlink_mission_item_t mission_item; 157 // Homeposition158 154 mavlink_home_position_t home_position; 159 // Heartbeat160 155 mavlink_heartbeat_t heartbeat; 161 // System Status162 156 mavlink_sys_status_t sys_status; 163 // Battery Status164 157 mavlink_battery_status_t battery_status; 165 // Radio Status166 158 mavlink_radio_status_t radio_status; 167 // Local Position168 159 mavlink_local_position_ned_t local_position_ned; 169 // Global Position170 160 mavlink_global_position_int_t global_position_int; 171 // Local Position Target 161 mavlink_set_position_target_local_ned_t set_position_target_local_ned; 162 mavlink_set_position_target_global_int_t set_position_target_global_int; 172 163 mavlink_position_target_local_ned_t position_target_local_ned; 173 // Global Position Target174 164 mavlink_position_target_global_int_t position_target_global_int; 175 // HiRes IMU176 165 mavlink_highres_imu_t highres_imu; 177 // Attitude178 166 mavlink_attitude_t attitude; 179 // System Parameters?180 // To add181 // Time Stamps182 167 Time_Stamps time_stamps; 183 168 void reset_timestamps() { … … 193 178 // * 20 MAV_CMD_NAV_RETURN_TO_LAUNCH (empty) 194 179 195 struct MissionItem {196 uint16_t itemID;197 uint8_t type;198 uint64_t holdTime;199 float radius;200 float yawAngle;201 float latitude;202 float longitude;203 float altitude;204 float x, y, z;205 float abortAltitude;206 uint16_t sequence;207 uint16_t repeatCount;208 MissionItem() {209 // struct construtor resets all the values210 itemID = sequence = repeatCount = 0;211 holdTime = 0;212 radius = yawAngle = latitude = longitude = altitude = abortAltitude = x = y = z = 0;213 }214 };215 216 180 // * Commands queue: 217 181 // * 176 MAV_CMD_DO_SET_MODE (mode MAV_MODE) 218 // * 177 MAV_CMD_DO_JUMP (sequence, repeat count)219 182 // * 179 MAV_CMD_DO_SET_HOME (use current, lat, long, altitude) 220 183 // * 193 MAV_CMD_DO_PAUSE_CONTINUE (holdContinue: 0=hold, 1=continue) … … 223 186 // * 20 MAV_CMD_NAV_RETURN_TO_LAUNCH (empty) 224 187 225 struct MissionCommand { 226 uint16_t commandId; 227 // DO_JUMP 188 struct MavlinkItem { 189 // TODO add timestamp 190 // 191 uint16_t id; 192 uint8_t target_system; 193 uint8_t target_component; 228 194 uint16_t sequence; 229 uint16_t repeatCount; 230 // DO_SET_HOME 231 uint8_t useCurrent; 232 float latitude, longitude, altitude; 195 uint16_t jump_sequence; 196 uint16_t jump_repeat_count; 197 uint8_t use_current; 198 uint64_t hold_time; 199 float radius; 200 float latitude, longitude, altitude, relative_alt; 233 201 float x, y, z; 234 // DO_PAUSE_CONTINUE 235 uint8_t pauseContinue; 236 // MISSION_START 237 uint16_t firstItem, lastItem; 238 // DO_SET_MODE 202 float vx, vy, vz; 203 float afx, afy, afz; 204 float yaw, yaw_rate; 205 float abort_altitude; 206 float desired_yaw; 207 float yaw_angle; 208 float min_pitch; 209 uint8_t pause_continue; 210 uint16_t first_item, last_item; 239 211 uint8_t mode; 240 MissionCommand() { 241 // reset all the attributes 242 pauseContinue = useCurrent = 0; 243 commandId = sequence = repeatCount = 0; 244 latitude = longitude = altitude = x = y = z = 0; 212 uint32_t time_boot_ms; 213 uint8_t coordinate_frame; 214 uint16_t type_mask; 215 int32_t lat_int, lon_int; 216 bool callback_flag; 217 uint64_t callback_period; 218 uint16_t callback_message; 219 MavlinkItem() { 220 target_system = target_component = 0; 221 first_item = last_item = 0; 222 mode = 0; 223 pause_continue = use_current = 0; 224 hold_time = 0; 225 id = sequence = 0; 226 jump_sequence = jump_repeat_count = 0; 227 radius = yaw_angle = latitude = longitude = altitude = relative_alt = x = y = z = 0; 228 desired_yaw = abort_altitude = 0; 229 min_pitch = 0; 230 vx = vy = vz = 0; 231 afx = afy = afz = 0; 232 yaw = yaw_rate = 0; 233 time_boot_ms = 0; 234 coordinate_frame = 0; 235 type_mask = 0; 236 lat_int = lon_int = 0; 237 callback_flag = false; 238 callback_message = 0; 239 callback_period = HEARTBEAT_DEFAULT_PERIOD; 245 240 } 246 241 }; … … 282 277 ~MavlinkUDP(); 283 278 284 // begin SockUDP285 279 // threads start and stop 286 280 void startThreads(); … … 297 291 socklen_t fromlen; 298 292 void clearData(); 299 // end SockUDP300 301 293 302 294 char control_status; … … 328 320 bool debug_messages; 329 321 322 // received setpoint values 323 float recvSetpointX; 324 float recvSetpointY; 325 330 326 bool sendingPosition; 331 uint 16_t sendingPositionInterval;327 uint64_t sendingPositionInterval; 332 328 bool sendingAttitude; 333 uint 16_t sendingAttitudeInterval;329 uint64_t sendingAttitudeInterval; 334 330 bool sendingSystemStatus; 335 uint 16_t sendingSystemStatusInterval;331 uint64_t sendingSystemStatusInterval; 336 332 bool sendingHeartbeat; 337 uint 16_t sendingHeartbeatInterval;333 uint64_t sendingHeartbeatInterval; 338 334 339 335 // queue of mission items (plan de vol) 340 std::queue<MissionItem> missionPlan; 341 //void getMissionPlan(std::vector<Waypoint>& x) { x = missionPlan; } 342 const std::queue<MissionItem>& getMissionPlan() const { return missionPlan; } 343 void clearMissionPlan() { std::queue<MissionItem> empty; std::swap(missionPlan, empty); } 336 std::queue<MavlinkItem> missionPlan; 337 const std::queue<MavlinkItem>& getMissionPlan() const { return missionPlan; } 338 void clearMissionPlan() { std::queue<MavlinkItem> empty; std::swap(missionPlan, empty); } 344 339 uint16_t currentMissionItem; 345 340 uint16_t missionFirst; 346 341 uint16_t missionLast; 347 348 // queue of received navigation commands 349 std::queue<MissionCommand> missionCommands; 350 const std::queue<MissionCommand>& getMissionCommands() const { return missionCommands; } 351 void clearMissionCommands() { std::queue<MissionCommand> empty; std::swap(missionCommands, empty); } 352 353 bool mission_items_loop; 354 void itemsLoop() { mission_items_loop = true; } 355 void itemsNoLoop() { mission_items_loop = false; } 356 bool getItemsLoop() const { return mission_items_loop; } 342 // queue of received commands 343 std::queue<MavlinkItem> missionCommands; 344 const std::queue<MavlinkItem>& getMissionCommands() const { return missionCommands; } 345 void clearMissionCommands() { std::queue<MavlinkItem> empty; std::swap(missionCommands, empty); } 346 347 // bool mission_items_loop; 348 // void itemsLoop() { mission_items_loop = true; } 349 // void itemsNoLoop() { mission_items_loop = false; } 350 // bool getItemsLoop() const { return mission_items_loop; } 357 351 358 352 bool missionActive; … … 392 386 void sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent); 393 387 void sendSetPositionTargetLocalNED(uint32_t timeBootMs, uint8_t targetSystem, uint8_t targetComponent, 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); 388 void sendPositionTargetLocalNED(uint32_t timeBootMs, 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); 394 389 void sendSetPositionTargetGlobalInt(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, 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); 390 void 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); 395 391 396 392 // mavlink commands 397 393 void cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec); 398 void cmd Waypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude);399 void cmd Land(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude); // land at location394 void cmdNavWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude); 395 void cmdNavLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude); // land at location 400 396 void cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude); // mission command to perform landing 401 void cmd Takeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude);397 void cmdNavTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude); 402 398 void cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode); 403 399 void cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent); 400 void cmdGetHomePosition(uint8_t targetSystem, uint8_t targetComponent); 404 401 void cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem); 405 402 void cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue); 406 403 void cmdRequestAutopilotCapabilities(uint8_t targetSystem, uint8_t targetComponent); 407 void cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent); 404 void cmdNavReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent); 405 void cmdDoPauseContinue(uint8_t targetSystem, uint8_t targetComponent, uint8_t pauseContinue); 408 406 //void cmdVideoStartCapture(uint8_t targetSystem, uint8_t targetComponent); 409 407 //void cmdVideoStopCapture(uint8_t targetSystem, uint8_t targetComponent); … … 430 428 private: 431 429 432 // begin SockUDP433 430 int m_socket_in; 434 431 int m_socket_out; … … 444 441 bool stop_recv; 445 442 bool stop_send; 446 //std::queue<std::string> queue_out;447 //Queue<uint8_t> queue_out;448 // end SockUDP449 443 450 444 inline void clearBuffer(uint8_t *buffer, int len); … … 467 461 bool shutdown_flag; 468 462 469 // TODO change to mission item class 463 int64_t heartbeat_period; 464 // callback timers 465 CallbackTimer timerHeartbeat; 466 void startHeartbeat(uint64_t period); 467 void stopHeartbeat(); 468 void callbackHeartbeat(); 469 470 CallbackTimer timerSystemTime; 471 void startSystemTime(uint64_t period); 472 void stopSystemTime(); 473 void callbackSystemTime(); 474 475 void stopAllCallbackTimers(); 476 477 // debug 470 478 void debug(std::string debug_msg); 471 479 };
Note:
See TracChangeset
for help on using the changeset viewer.