From 03133560964aecc8371d2605efbe11bb58029168 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Tue, 1 Sep 2020 12:37:22 -0700 Subject: [PATCH] Mavlink update --- libs/mavlink/include/mavlink/v2.0 | 2 +- src/Camera/QGCCameraControl.cc | 13 +++++++------ src/Camera/QGCCameraIO.cc | 17 +++++++++-------- src/comm/MockLink.cc | 14 ++++++-------- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0 index 99a977433..d1eea85b6 160000 --- a/libs/mavlink/include/mavlink/v2.0 +++ b/libs/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 99a977433a13705cd0624f8ff3b5f5210d526ba9 +Subproject commit d1eea85b685eec2204ad31c7f7f51486f218f86b diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index bef5c7c49..54c228474 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -1174,12 +1174,13 @@ QGCCameraControl::_requestAllParameters() MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); mavlink_message_t msg; mavlink_msg_param_ext_request_list_pack_chan( - static_cast(mavlink->getSystemId()), - static_cast(mavlink->getComponentId()), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - static_cast(_vehicle->id()), - static_cast(compID())); + static_cast(mavlink->getSystemId()), + static_cast(mavlink->getComponentId()), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + static_cast(_vehicle->id()), + static_cast(compID()), + 0); // trimmed messages = false _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); qCDebug(CameraControlVerboseLog) << "Request all parameters"; } diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc index a0cbb4cc3..4a6c10e1c 100644 --- a/src/Camera/QGCCameraIO.cc +++ b/src/Camera/QGCCameraIO.cc @@ -356,14 +356,15 @@ QGCCameraParamIO::paramRequest(bool reset) strncpy(param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN); mavlink_message_t msg; mavlink_msg_param_ext_request_read_pack_chan( - static_cast(_pMavlink->getSystemId()), - static_cast(_pMavlink->getComponentId()), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - static_cast(_vehicle->id()), - static_cast(_control->compID()), - param_id, - -1); + static_cast(_pMavlink->getSystemId()), + static_cast(_pMavlink->getComponentId()), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + static_cast(_vehicle->id()), + static_cast(_control->compID()), + param_id, + -1, + 0); // trimmed messages = false _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); _paramRequestTimer.start(); } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 9cb3bb446..bbc917568 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -7,7 +7,6 @@ * ****************************************************************************/ - #include "MockLink.h" #include "QGCLoggingCategory.h" #include "QGCApplication.h" @@ -28,11 +27,6 @@ QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog") QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") -/// @file -/// @brief Mock implementation of a Link. -/// -/// @author Don Gagne - // Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle // testing of a gazebo vehicle and a mocklink vehicle #if 1 @@ -410,12 +404,14 @@ void MockLink::_sendBatteryStatus(void) mavlink_message_t msg; uint16_t rgVoltages[10]; uint16_t rgVoltagesNone[10]; + uint16_t rgVoltagesExtNone[4]; for (int i=0; i<10; i++) { rgVoltages[i] = UINT16_MAX; rgVoltagesNone[i] = UINT16_MAX; } rgVoltages[0] = rgVoltages[1] = rgVoltages[2] = 4200; + memset(rgVoltagesExtNone, 0, sizeof(rgVoltagesExtNone)); mavlink_msg_battery_status_pack_chan( _vehicleSystemId, @@ -432,7 +428,8 @@ void MockLink::_sendBatteryStatus(void) -1, // energy consumed not supported _battery1PctRemaining, _battery1TimeRemaining, - _battery1ChargeState); + _battery1ChargeState, + rgVoltagesExtNone); respondWithMavlinkMessage(msg); mavlink_msg_battery_status_pack_chan( @@ -450,7 +447,8 @@ void MockLink::_sendBatteryStatus(void) -1, // energy consumed not supported _battery2PctRemaining, _battery2TimeRemaining, - _battery2ChargeState); + _battery2ChargeState, + rgVoltagesExtNone); respondWithMavlinkMessage(msg); } -- 2.22.0