Changeset 84 in flair-src for branches/mavlink


Ignore:
Timestamp:
Sep 23, 2016, 11:13:25 AM (8 years ago)
Author:
Thomas Fuhrmann
Message:

Update MavlinkUDP files and add pause function

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  
    4545}
    4646
    47 void GuiFlair::MissionResume() {
     47void GuiFlair::MissionPause() {
    4848        cout << "MavPlanner GuiFlair MissionResume" << endl;
    4949}

  • branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.h

    r77 r84  
    4040  virtual void MissionStart();
    4141  virtual void MissionStop();
    42   virtual void MissionResume();
     42  virtual void MissionPause();
    4343  virtual void MissionSend();
    4444};

  • branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp

    r83 r84  
    4242  //outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort));
    4343
     44  missionPauseEnabled = false;
     45
    4446  mavCom = new MavlinkUDP(outputAddress, outputPort);
    4547}
    … …  
    7880}
    7981
    80 void GuiGcs::MissionResume() {
    81   cout << "MavPlanner GuiGcs MissionResume" << endl;
     82void 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);
    8295}
    8396

  • branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h

    r83 r84  
    4848  virtual void MissionStart();
    4949  virtual void MissionStop();
    50   virtual void MissionResume();
     50  virtual void MissionPause();
    5151  virtual void MissionSend();
    5252
    … …  
    5959  MavlinkUDP* mavCom;
    6060
     61  bool missionPauseEnabled;
     62
    6163  void Initialize();
    6264  void ParametersParse(const std::string& parametersString, std::array<float, 6>& parametersFloat);

  • branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.cpp

    r81 r84  
    5252  btnStartMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Start mission");
    5353  btnStopMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Stop mission");
    54   btnResumeMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Resume mission");
     54  btnPauseMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Pause mission");
    5555
    5656  // Add cmd group
    … …  
    132132                MissionStop();
    133133    }
    134         if (btnResumeMission->Clicked()) {
    135                 MissionResume();
     134        if (btnPauseMission->Clicked()) {
     135                MissionPause();
    136136    }
    137137    if (btnSendMission->Clicked()) {

  • branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.h

    r81 r84  
    5252  virtual void MissionStart() = 0;
    5353  virtual void MissionStop() = 0;
    54   virtual void MissionResume() = 0;
     54  virtual void MissionPause() = 0;
    5555  virtual void MissionSend() = 0;
    5656
    … …  
    6666  flair::gui::PushButton* btnStartMission;
    6767  flair::gui::PushButton* btnStopMission;
    68   flair::gui::PushButton* btnResumeMission;
     68  flair::gui::PushButton* btnPauseMission;
    6969 
    7070  // Add cmd groupbox

  • branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp

    r78 r84  
    4343    debug_messages = true;
    4444
     45    heartbeat_period = HEARTBEAT_DEFAULT_PERIOD;
     46
    4547    resetCommandAck();
    4648    resetMissionAck();
    … …  
    5052    sendingPosition = false;
    5153    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;
    5557
    5658    // mission items
    5759    missionFirst = 0;
    5860    missionLast = 0;
    59     mission_items_loop = false;
     61    //mission_items_loop = false;
    6062    missionActive = false; // if the mission is launched
    6163
    … …  
    6668    autopilot_id = 0; // autopilot component id
    6769    component_id = 0; // companion computer component id
     70
     71    // setpoint
     72    recvSetpointX = 0;
     73    recvSetpointY = 0;
    6874
    6975   // mavlink_command_long_t com = { 0 };
    … …  
    210216    length_to_send = mavlink_msg_to_send_buffer(buff_out, &message);
    211217    something_to_send = true;
    212     printf("[SENT] Heartbeat\n");
     218    //printf("[SENT] Heartbeat\n");
    213219}
    214220
    … …  
    219225    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
    220226    something_to_send = true;
    221     printf("[SENT] System status\n");
     227    //printf("[SENT] System status\n");
    222228}
    223229
    … …  
    358364    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);
    359365    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
     371void 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);
    360375    something_to_send = true;
    361376}
    … …  
    365380    mavlink_message_t message;
    366381    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
     387void 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);
    367390    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
    368391    something_to_send = true;
    … …  
    374397
    375398// command Waypoint
    376 void MavlinkUDP::cmdWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude) {
     399void MavlinkUDP::cmdNavWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude) {
    377400    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_WAYPOINT, 0, holdTime, proximityRadius, passRadius, desiredYaw, latitude, longitude, altitude);
    378401}
    … …  
    384407
    385408// command Land
    386 void MavlinkUDP::cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) {
     409void MavlinkUDP::cmdNavLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) {
    387410    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_LAND, 0, abortAlt, 0, 0, desiredYaw, latitude, longitude, altitude);
    388411}
    … …  
    394417
    395418// command TakeOff
    396 void MavlinkUDP::cmdTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) {
     419void MavlinkUDP::cmdNavTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) {
    397420    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_TAKEOFF, 0, desiredPitch, 0, 0, magnetometerYaw, latitude, longitude, altitude);
    398421}
    … …  
    403426}
    404427
     428// command DoPauseContinue
     429void 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
    405433// command DoSetHome
    406434void MavlinkUDP::cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent) {
    … …  
    408436}
    409437
     438// command GetHomePosition
     439void 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
    410443// command MissionStart
    411444void MavlinkUDP::cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem) {
    … …  
    424457
    425458// command ReturnToLaunch
    426 void MavlinkUDP::cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {
     459void MavlinkUDP::cmdNavReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {
    427460    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0);
    428461}
    … …  
    432465    // for autopilot and onboardComputer, see TMAV_REBOOT_SHUTDOWN_PARAM
    433466    sendCommandLong(targetSystem, targetComponent, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, autopilot, onboardComputer, 0, 0, 0, 0, 0);
     467}
     468
     469
     470// callback Heartbeat
     471void MavlinkUDP::startHeartbeat(uint64_t period) {
     472    timerHeartbeat.start(period, std::bind(&MavlinkUDP::callbackHeartbeat,this));
     473}
     474void MavlinkUDP::stopHeartbeat() {
     475    timerHeartbeat.stop();
     476}
     477void MavlinkUDP::callbackHeartbeat() {
     478    sendHeartbeat();
     479}
     480
     481// callback SystemTime
     482void MavlinkUDP::startSystemTime(uint64_t period) {
     483    timerSystemTime.start(period, std::bind(&MavlinkUDP::callbackSystemTime,this));
     484}
     485void MavlinkUDP::stopSystemTime() {
     486    timerSystemTime.stop();
     487}
     488void MavlinkUDP::callbackSystemTime() {
     489    sendSystemTime();
     490}
     491
     492// stop the callback function timers
     493void MavlinkUDP::stopAllCallbackTimers() {
     494    stopHeartbeat();
     495    stopSystemTime();
    434496}
    435497
    … …  
    500562            // clear the queue
    501563            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);
    505568            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
    506569            break;
    … …  
    592655            case MAV_CMD_NAV_TAKEOFF:
    593656            {
    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");
    603668                break;
    604669            }
    605670            case MAV_CMD_NAV_LAND:
    606671            {
    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");
    617705                break;
    618706            }
    619707            case MAV_CMD_NAV_WAYPOINT:
    620708            {
    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);
    633720                break;
    634721            }
    … …  
    719806        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
    720807        {
    721             printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
     808            printf("[RECV] MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
    722809            mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
    723810            current_messages.time_stamps.local_position_ned = get_time_usec();
    724811            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;
    725825            break;
    726826        }
    … …  
    728828        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
    729829        {
    730             printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
     830            printf("[RECV] MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
    731831            mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
    732832            current_messages.time_stamps.global_position_int = get_time_usec();
    733833            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);
    735876            break;
    736877        }
    … …  
    738879        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
    739880        {
    740             printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
     881            printf("[RECV] MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
    741882            mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
    742883            current_messages.time_stamps.position_target_local_ned = get_time_usec();
    743884            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);
    744932            break;
    745933        }
    … …  
    747935        case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
    748936        {
    749             printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
     937            printf("[RECV] MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
    750938            mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
    751939            current_messages.time_stamps.position_target_global_int = get_time_usec();
    752940            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);
    753959            break;
    754960        }
    … …  
    756962        case MAVLINK_MSG_ID_HIGHRES_IMU:
    757963        {
    758             //printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
     964            //printf("[RECV] MAVLINK_MSG_ID_HIGHRES_IMU\n");
    759965            mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
    760966            current_messages.time_stamps.highres_imu = get_time_usec();
    … …  
    781987            printf("Received command %d\n", current_messages.command_long.command);
    782988            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);
    784998                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);
    7881005                    } else {
    789                         itemsNoLoop();
     1006                        startHeartbeat(desired_interval);
    7901007                    }
    791 
    7921008                    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                }
    7931052                default:
    7941053                    break;
    7951054                }
    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
    8481067            case MAV_CMD_MISSION_START:
    8491068            {
    850                 MissionCommand missCom;
    851                 missCom.commandId = mavlink_msg_command_long_get_command(&message);
    852                 missCom.firstItem = (uint16_t)mavlink_msg_command_long_get_param1(&message);
    853                 missCom.lastItem = (uint16_t)mavlink_msg_command_long_get_param2(&message);
    854                 missionCommands.push(missCom);
    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);
    8561075                missionStarted();
    8571076                break;
    8581077            }
    859             case MAV_CMD_DO_LAND_START:
    860                 // TODO do land start
    861                 break;
    8621078            case MAV_CMD_DO_SET_HOME:
    8631079            {
    864                 MissionCommand missCom;
    865                 missCom.commandId = mavlink_msg_command_long_get_command(&message);
    866                 missCom.useCurrent = (uint8_t)mavlink_msg_command_long_get_param1(&message);
    867                 missCom.latitude = mavlink_msg_command_long_get_param5(&message);
    868                 missCom.longitude = mavlink_msg_command_long_get_param6(&message);
    869                 missCom.altitude = mavlink_msg_command_long_get_param7(&message);
    870                 missionCommands.push(missCom);
    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);
    8721088                break;
    8731089            }
    8741090            case MAV_CMD_GET_HOME_POSITION:
    8751091            {
    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");
    8801097                break;
    8811098            }
    8821099            case MAV_CMD_DO_SET_MODE:
    8831100            {
    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            }
    8941108            case MAV_CMD_NAV_RETURN_TO_LAUNCH:
    8951109            {
    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            }
    9091116            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            }
    9131130            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            }
    9171152            case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
     1153            {
    9181154                // shutdown signal received
    9191155                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;
    9431169            }
    9441170            default:

  • branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h

    r78 r84  
    3030#include <sys/time.h>
    3131#include "include/common/mavlink.h"
     32#include "CallbackTimer.h"
    3233
    3334// TODO these constants should be read from a file
    … …  
    4849
    4950#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
    6166
    6267#ifndef REBOOT_SHUTDOWN_PARAMS
    … …  
    9499    uint64_t local_position_ned;
    95100    uint64_t global_position_int;
     101    uint64_t set_position_target_local_ned;
     102    uint64_t set_position_target_global_int;
    96103    uint64_t position_target_local_ned;
    97104    uint64_t position_target_global_int;
    … …  
    118125        local_position_ned = 0;
    119126        global_position_int = 0;
     127        set_position_target_local_ned = 0;
     128        set_position_target_global_int = 0;
    120129        position_target_local_ned = 0;
    121130        position_target_global_int = 0;
    … …  
    131140    int sysid;
    132141    int compid;
    133     // system time
    134142    mavlink_system_time_t system_time;
    135     // mission_request
    136143    mavlink_mission_request_t mission_request;
    137     // mission request list
    138144    mavlink_mission_request_list_t mission_request_list;
    139     // mission write partial list
    140145    mavlink_mission_write_partial_list_t mission_write_partial_list;
    141     // mission_reached
    142146    mavlink_mission_item_reached_t mission_reached;
    143     // mission_clear_all
    144147    mavlink_mission_clear_all_t clear_all;
    145     // command long
    146148    mavlink_command_long_t command_long;
    147     // command_ack
    148149    mavlink_command_ack_t command_ack;
    149     // timesync
    150150    mavlink_timesync_t timesync;
    151     // mission_count
    152151    mavlink_mission_count_t mission_count;
    153     // mission_ack
    154152    mavlink_mission_ack_t mission_ack;
    155     // mission_item
    156153    mavlink_mission_item_t mission_item;
    157     // Homeposition
    158154    mavlink_home_position_t home_position;
    159     // Heartbeat
    160155    mavlink_heartbeat_t heartbeat;
    161     // System Status
    162156    mavlink_sys_status_t sys_status;
    163     // Battery Status
    164157    mavlink_battery_status_t battery_status;
    165     // Radio Status
    166158    mavlink_radio_status_t radio_status;
    167     // Local Position
    168159    mavlink_local_position_ned_t local_position_ned;
    169     // Global Position
    170160    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;
    172163    mavlink_position_target_local_ned_t position_target_local_ned;
    173     // Global Position Target
    174164    mavlink_position_target_global_int_t position_target_global_int;
    175     // HiRes IMU
    176165    mavlink_highres_imu_t highres_imu;
    177     // Attitude
    178166    mavlink_attitude_t attitude;
    179     // System Parameters?
    180     // To add
    181     // Time Stamps
    182167    Time_Stamps time_stamps;
    183168    void reset_timestamps() {
    … …  
    193178// * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
    194179
    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 values
    210         itemID = sequence = repeatCount = 0;
    211         holdTime = 0;
    212         radius = yawAngle = latitude = longitude = altitude = abortAltitude = x = y = z = 0;
    213     }
    214 };
    215 
    216180// * Commands queue:
    217181// * 176  MAV_CMD_DO_SET_MODE             (mode MAV_MODE)
    218 // * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
    219182// * 179  MAV_CMD_DO_SET_HOME             (use current, lat, long, altitude)
    220183// * 193  MAV_CMD_DO_PAUSE_CONTINUE       (holdContinue: 0=hold, 1=continue)
    … …  
    223186// * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
    224187
    225 struct MissionCommand {
    226     uint16_t commandId;
    227     // DO_JUMP
     188struct MavlinkItem {
     189    // TODO add timestamp
     190    //
     191    uint16_t id;
     192    uint8_t target_system;
     193    uint8_t target_component;
    228194    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;
    233201    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;
    239211    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;
    245240    }
    246241};
    … …  
    282277    ~MavlinkUDP();
    283278
    284     // begin SockUDP
    285279    // threads start and stop
    286280    void startThreads();
    … …  
    297291    socklen_t fromlen;
    298292    void clearData();
    299     // end SockUDP
    300 
    301293
    302294    char control_status;
    … …  
    328320    bool debug_messages;
    329321
     322    // received setpoint values
     323    float recvSetpointX;
     324    float recvSetpointY;
     325
    330326    bool sendingPosition;
    331     uint16_t sendingPositionInterval;
     327    uint64_t sendingPositionInterval;
    332328    bool sendingAttitude;
    333     uint16_t sendingAttitudeInterval;
     329    uint64_t sendingAttitudeInterval;
    334330    bool sendingSystemStatus;
    335     uint16_t sendingSystemStatusInterval;
     331    uint64_t sendingSystemStatusInterval;
    336332    bool sendingHeartbeat;
    337     uint16_t sendingHeartbeatInterval;
     333    uint64_t sendingHeartbeatInterval;
    338334
    339335    // 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); }
    344339    uint16_t currentMissionItem;
    345340    uint16_t missionFirst;
    346341    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; }
    357351
    358352    bool missionActive;
    … …  
    392386    void sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent);
    393387    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);
    394389    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);
    395391
    396392    // mavlink commands
    397393    void cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec);
    398     void cmdWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude);
    399     void cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude); // land at location
     394    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
    400396    void cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude); // mission command to perform landing
    401     void cmdTakeoff(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);
    402398    void cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode);
    403399    void cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent);
     400    void cmdGetHomePosition(uint8_t targetSystem, uint8_t targetComponent);
    404401    void cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem);
    405402    void cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue);
    406403    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);
    408406    //void cmdVideoStartCapture(uint8_t targetSystem, uint8_t targetComponent);
    409407    //void cmdVideoStopCapture(uint8_t targetSystem, uint8_t targetComponent);
    … …  
    430428private:
    431429
    432     // begin SockUDP
    433430    int m_socket_in;
    434431    int m_socket_out;
    … …  
    444441    bool stop_recv;
    445442    bool stop_send;
    446     //std::queue<std::string> queue_out;
    447     //Queue<uint8_t> queue_out;
    448     // end SockUDP
    449443
    450444    inline void clearBuffer(uint8_t *buffer, int len);
    … …  
    467461    bool shutdown_flag;
    468462
    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
    470478    void debug(std::string debug_msg);
    471479};
Note: See TracChangeset for help on using the changeset viewer.