Skip to content

Commit

Permalink
Update mavlink submodule
Browse files Browse the repository at this point in the history
  • Loading branch information
HTRamsey authored and DonLakeFlyer committed Apr 15, 2024
1 parent cd25ed5 commit 9fcfcad
Show file tree
Hide file tree
Showing 4 changed files with 59 additions and 44 deletions.
2 changes: 1 addition & 1 deletion libs/mavlink/include/mavlink/v2.0
Submodule v2.0 updated 461 files
38 changes: 22 additions & 16 deletions src/MissionManager/PlanManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -108,14 +108,17 @@ void PlanManager::_writeMissionCount(void)
mavlink_message_t message;
SharedLinkInterfacePtr sharedLink = weakLink.lock();

mavlink_msg_mission_count_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
sharedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_AUTOPILOT1,
_writeMissionItems.count(),
_planType);
mavlink_msg_mission_count_pack_chan(
qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
sharedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_AUTOPILOT1,
_writeMissionItems.count(),
_planType,
0
);

_vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
}
Expand Down Expand Up @@ -297,14 +300,17 @@ void PlanManager::_readTransactionComplete(void)
SharedLinkInterfacePtr sharedLink = weakLink.lock();
mavlink_message_t message;

mavlink_msg_mission_ack_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
sharedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_AUTOPILOT1,
MAV_MISSION_ACCEPTED,
_planType);
mavlink_msg_mission_ack_pack_chan(
qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
sharedLink->mavlinkChannel(),
&message,
_vehicle->id(),
MAV_COMP_ID_AUTOPILOT1,
MAV_MISSION_ACCEPTED,
_planType,
0
);

_vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
}
Expand Down
25 changes: 14 additions & 11 deletions src/Vehicle/Vehicle.cc
Original file line number Diff line number Diff line change
Expand Up @@ -4458,17 +4458,20 @@ void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, flo
float newThrustCommand = thrust * axesScaling;

mavlink_msg_manual_control_pack_chan(
static_cast<uint8_t>(_mavlink->getSystemId()),
static_cast<uint8_t>(_mavlink->getComponentId()),
sharedLink->mavlinkChannel(),
&message,
static_cast<uint8_t>(_id),
static_cast<int16_t>(newPitchCommand),
static_cast<int16_t>(newRollCommand),
static_cast<int16_t>(newThrustCommand),
static_cast<int16_t>(newYawCommand),
buttons,
0, 0, 0, 0);
static_cast<uint8_t>(_mavlink->getSystemId()),
static_cast<uint8_t>(_mavlink->getComponentId()),
sharedLink->mavlinkChannel(),
&message,
static_cast<uint8_t>(_id),
static_cast<int16_t>(newPitchCommand),
static_cast<int16_t>(newRollCommand),
static_cast<int16_t>(newThrustCommand),
static_cast<int16_t>(newYawCommand),
buttons, 0,
0,
0, 0,
0, 0, 0, 0, 0, 0
);
sendMessageOnLinkThreadSafe(sharedLink.get(), message);
}

Expand Down
38 changes: 22 additions & 16 deletions src/comm/MockLinkMissionItemHandler.cc
Original file line number Diff line number Diff line change
Expand Up @@ -155,14 +155,17 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message

mavlink_message_t responseMsg;

mavlink_msg_mission_count_pack_chan(_mockLink->vehicleId(),
MAV_COMP_ID_AUTOPILOT1,
_mockLink->mavlinkChannel(),
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
itemCount, // Number of mission items
_requestType);
mavlink_msg_mission_count_pack_chan(
_mockLink->vehicleId(),
MAV_COMP_ID_AUTOPILOT1,
_mockLink->mavlinkChannel(),
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
itemCount, // Number of mission items
_requestType,
0
);
_mockLink->respondWithMavlinkMessage(responseMsg);
}
}
Expand Down Expand Up @@ -330,14 +333,17 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)

mavlink_message_t message;

mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(),
MAV_COMP_ID_AUTOPILOT1,
_mockLink->mavlinkChannel(),
&message,
_mavlinkProtocol->getSystemId(),
_mavlinkProtocol->getComponentId(),
ackType,
_requestType);
mavlink_msg_mission_ack_pack_chan(
_mockLink->vehicleId(),
MAV_COMP_ID_AUTOPILOT1,
_mockLink->mavlinkChannel(),
&message,
_mavlinkProtocol->getSystemId(),
_mavlinkProtocol->getComponentId(),
ackType,
_requestType,
0
);
_mockLink->respondWithMavlinkMessage(message);
}

Expand Down

0 comments on commit 9fcfcad

Please sign in to comment.