Skip to content

Q
qgroundcontrol
Commit a52b110b authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Add support for HIGH_LATENCY2 

* Flight modes still not implemented. Waiting on message change.
* Also includes various cauterization of UI things which should not be
available on high latency link.
parent 8bf441af
Hide whitespace changes
Inline Side-by-side
Showing with 238 additions and 110 deletions
src/FactSystem/ParameterManager.cc
View file @ a52b110b
...@@ -81,7 +81,17 @@ ParameterManager::ParameterManager(Vehicle* vehicle) ...@@ -81,7 +81,17 @@ ParameterManager::ParameterManager(Vehicle* vehicle)
// Ensure the cache directory exists // Ensure the cache directory exists
QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache"); QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache");
refreshAllParameters(); if (_vehicle->highLatencyLink()) {
// High latency links don't load parameters
_parametersReady = true;
_missingParameters = true;
_initialLoadComplete = true;
_waitingForDefaultComponent = false;
emit parametersReadyChanged(_parametersReady);
emit missingParametersChanged(_missingParameters);
} else {
refreshAllParameters();
}
} }
ParameterManager::~ParameterManager() ParameterManager::~ParameterManager()
...@@ -133,12 +143,12 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString ...@@ -133,12 +143,12 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString
} }
#endif #endif
if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") { if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") {
if (!_initialLoadComplete && !_logReplay) { if (!_initialLoadComplete && !_logReplay) {
/* we received a cache hash, potentially load from cache */ /* we received a cache hash, potentially load from cache */
_tryCacheHashLoad(vehicleId, componentId, value); _tryCacheHashLoad(vehicleId, componentId, value);
} }
return; return;
} }
_initialRequestTimeoutTimer.stop(); _initialRequestTimeoutTimer.stop();
......
src/MissionManager/PlanMasterController.cc
View file @ a52b110b
...@@ -157,6 +157,11 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) ...@@ -157,6 +157,11 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle)
void PlanMasterController::loadFromVehicle(void) void PlanMasterController::loadFromVehicle(void)
{ {
if (_managerVehicle->highLatencyLink()) {
qgcApp()->showMessage(tr("Download not supported on high latency links."));
return;
}
if (offline()) { if (offline()) {
qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while offline"; qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while offline";
} else if (!_editMode) { } else if (!_editMode) {
...@@ -256,6 +261,11 @@ void PlanMasterController::_sendRallyPointsComplete(void) ...@@ -256,6 +261,11 @@ void PlanMasterController::_sendRallyPointsComplete(void)
void PlanMasterController::sendToVehicle(void) void PlanMasterController::sendToVehicle(void)
{ {
if (_managerVehicle->highLatencyLink()) {
qgcApp()->showMessage(tr("Upload not supported on high latency links."));
return;
}
if (offline()) { if (offline()) {
qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while offline"; qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while offline";
} else if (syncInProgress()) { } else if (syncInProgress()) {
......
src/MissionManager/VisualMissionItem.cc
View file @ a52b110b
...@@ -63,7 +63,7 @@ VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* pa ...@@ -63,7 +63,7 @@ VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* pa
{ {
*this = other; *this = other;
// Don't get terrain altitude information for submarines or boards // Don't get terrain altitude information for submarines or boats
if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) { if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) {
connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude); connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude);
} }
......
src/Vehicle/MultiVehicleManager.cc
View file @ a52b110b
...@@ -72,7 +72,7 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox) ...@@ -72,7 +72,7 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox)
this); this);
} }
void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType) void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType)
{ {
if (componentId != MAV_COMP_ID_AUTOPILOT1) { if (componentId != MAV_COMP_ID_AUTOPILOT1) {
// Special case for PX4 Flow // Special case for PX4 Flow
...@@ -82,7 +82,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle ...@@ -82,7 +82,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
<< link->getName() << link->getName()
<< vehicleId << vehicleId
<< componentId << componentId
<< vehicleMavlinkVersion
<< vehicleFirmwareType << vehicleFirmwareType
<< vehicleType; << vehicleType;
return; return;
...@@ -108,11 +107,10 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle ...@@ -108,11 +107,10 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
break; break;
} }
qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:componentId:vehicleMavlinkVersion:vehicleFirmwareType:vehicleType " qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:componentId:vehicleFirmwareType:vehicleType "
<< link->getName() << link->getName()
<< vehicleId << vehicleId
<< componentId << componentId
<< vehicleMavlinkVersion
<< vehicleFirmwareType << vehicleFirmwareType
<< vehicleType; << vehicleType;
...@@ -120,16 +118,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle ...@@ -120,16 +118,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
_app->showMessage(tr("Warning: A vehicle is using the same system id as %1: %2").arg(qgcApp()->applicationName()).arg(vehicleId)); _app->showMessage(tr("Warning: A vehicle is using the same system id as %1: %2").arg(qgcApp()->applicationName()).arg(vehicleId));
} }
// QSettings settings;
// bool mavlinkVersionCheck = settings.value("VERSION_CHECK_ENABLED", true).toBool();
// if (mavlinkVersionCheck && vehicleMavlinkVersion != MAVLINK_VERSION) {
// _ignoreVehicleIds += vehicleId;
// _app->showMessage(QString("The MAVLink protocol version on vehicle #%1 and %2 differ! "
// "It is unsafe to use different MAVLink versions. "
// "%2 therefore refuses to connect to vehicle #%1, which sends MAVLink version %3 (%2 uses version %4).").arg(vehicleId).arg(qgcApp()->applicationName()).arg(vehicleMavlinkVersion).arg(MAVLINK_VERSION));
// return;
// }
Vehicle* vehicle = new Vehicle(link, vehicleId, componentId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _joystickManager); Vehicle* vehicle = new Vehicle(link, vehicleId, componentId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _joystickManager);
connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1); connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1);
connect(vehicle, &Vehicle::requestProtocolVersion, this, &MultiVehicleManager::_requestProtocolVersion); connect(vehicle, &Vehicle::requestProtocolVersion, this, &MultiVehicleManager::_requestProtocolVersion);
......
src/Vehicle/MultiVehicleManager.h
View file @ a52b110b
...@@ -94,7 +94,7 @@ private slots: ...@@ -94,7 +94,7 @@ private slots:
void _setActiveVehiclePhase2(void); void _setActiveVehiclePhase2(void);
void _vehicleParametersReadyChanged(bool parametersReady); void _vehicleParametersReadyChanged(bool parametersReady);
void _sendGCSHeartbeat(void); void _sendGCSHeartbeat(void);
void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType); void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType);
void _requestProtocolVersion(unsigned version); void _requestProtocolVersion(unsigned version);
private: private:
......
src/Vehicle/Vehicle.cc
View file @ a52b110b
...@@ -233,18 +233,27 @@ Vehicle::Vehicle(LinkInterface* link, ...@@ -233,18 +233,27 @@ Vehicle::Vehicle(LinkInterface* link,
connect(_mav, SIGNAL(attitudeChanged (UASInterface*,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64))); connect(_mav, SIGNAL(attitudeChanged (UASInterface*,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64))); connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
if (_highLatencyLink) {
// High latency links don't request information
_setMaxProtoVersion(100);
_setCapabilities(0);
_initialPlanRequestComplete = true;
_missionManagerInitialRequestSent = true;
_geoFenceManagerInitialRequestSent = true;
_rallyPointManagerInitialRequestSent = true;
} else {
// Ask the vehicle for protocol version info.
sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet.
MAV_CMD_REQUEST_PROTOCOL_VERSION,
false, // No error shown if fails
1); // Request protocol version
// Ask the vehicle for protocol version info. // Ask the vehicle for firmware version info.
sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet.
MAV_CMD_REQUEST_PROTOCOL_VERSION, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
false, // No error shown if fails false, // No error shown if fails
1); // Request protocol version 1); // Request firmware version
}
// Ask the vehicle for firmware version info.
sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet.
MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
false, // No error shown if fails
1); // Request firmware version
_firmwarePlugin->initializeVehicle(this); _firmwarePlugin->initializeVehicle(this);
...@@ -677,13 +686,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes ...@@ -677,13 +686,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
break; break;
case MAVLINK_MSG_ID_SCALED_PRESSURE3: case MAVLINK_MSG_ID_SCALED_PRESSURE3:
_handleScaledPressure3(message); _handleScaledPressure3(message);
break; break;
case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED: case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
_handleCameraImageCaptured(message); _handleCameraImageCaptured(message);
break; break;
case MAVLINK_MSG_ID_ADSB_VEHICLE: case MAVLINK_MSG_ID_ADSB_VEHICLE:
_handleADSBVehicle(message); _handleADSBVehicle(message);
break; break;
case MAVLINK_MSG_ID_HIGH_LATENCY2:
_handleHighLatency2(message);
break;
case MAVLINK_MSG_ID_SERIAL_CONTROL: case MAVLINK_MSG_ID_SERIAL_CONTROL:
{ {
...@@ -693,7 +705,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes ...@@ -693,7 +705,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
} }
break; break;
// Following are ArduPilot dialect messages // Following are ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT) #if !defined(NO_ARDUPILOT_DIALECT)
case MAVLINK_MSG_ID_CAMERA_FEEDBACK: case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
_handleCameraFeedback(message); _handleCameraFeedback(message);
...@@ -797,6 +809,47 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message) ...@@ -797,6 +809,47 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
emit coordinateChanged(_coordinate); emit coordinateChanged(_coordinate);
} }
void Vehicle::_handleHighLatency2(mavlink_message_t& message)
{
mavlink_high_latency2_t highLatency2;
mavlink_msg_high_latency2_decode(&message, &highLatency2);
#if 0
typedef struct __mavlink_high_latency2_t {
uint16_t wp_num; /*< Current waypoint number*/
uint16_t failure_flags; /*< Indicates failures as defined in MAV_FAILURE_FLAG ENUM. */
uint8_t flight_mode; /*< Flight Mode of the vehicle as defined in the FLIGHT_MODE ENUM*/
uint8_t failsafe; /*< Indicates if a failsafe mode is triggered, defined in MAV_FAILSAFE_FLAG ENUM*/
}) mavlink_high_latency2_t;
#endif
// FIXME: flight mode not yet supported
_coordinate.setLatitude(highLatency2.latitude / (double)1E7);
_coordinate.setLongitude(highLatency2.longitude / (double)1E7);
_coordinate.setAltitude(highLatency2.altitude);
emit coordinateChanged(_coordinate);
_airSpeedFact.setRawValue((double)highLatency2.airspeed / 5.0);
_groundSpeedFact.setRawValue((double)highLatency2.groundspeed / 5.0);
_climbRateFact.setRawValue((double)highLatency2.climb_rate / 10.0);
_headingFact.setRawValue((double)highLatency2.heading * 2.0);
_windFactGroup.direction()->setRawValue((double)highLatency2.wind_heading * 2.0);
_windFactGroup.speed()->setRawValue((double)highLatency2.windspeed / 5.0);
_batteryFactGroup.percentRemaining()->setRawValue(highLatency2.battery);
_temperatureFactGroup.temperature1()->setRawValue(highLatency2.temperature_air);
_gpsFactGroup.lat()->setRawValue(highLatency2.latitude * 1e-7);
_gpsFactGroup.lon()->setRawValue(highLatency2.longitude * 1e-7);
_gpsFactGroup.count()->setRawValue(0);
_gpsFactGroup.hdop()->setRawValue(highLatency2.eph == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.eph / 10.0);
_gpsFactGroup.vdop()->setRawValue(highLatency2.epv == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.epv / 10.0);
}
void Vehicle::_handleAltitude(mavlink_message_t& message) void Vehicle::_handleAltitude(mavlink_message_t& message)
{ {
mavlink_altitude_t altitude; mavlink_altitude_t altitude;
...@@ -909,14 +962,14 @@ QString Vehicle::vehicleUIDStr() ...@@ -909,14 +962,14 @@ QString Vehicle::vehicleUIDStr()
QString uid; QString uid;
uint8_t* pUid = (uint8_t*)(void*)&_uid; uint8_t* pUid = (uint8_t*)(void*)&_uid;
uid.sprintf("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X", uid.sprintf("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X",
pUid[0] & 0xff, pUid[0] & 0xff,
pUid[1] & 0xff, pUid[1] & 0xff,
pUid[2] & 0xff, pUid[2] & 0xff,
pUid[3] & 0xff, pUid[3] & 0xff,
pUid[4] & 0xff, pUid[4] & 0xff,
pUid[5] & 0xff, pUid[5] & 0xff,
pUid[6] & 0xff, pUid[6] & 0xff,
pUid[7] & 0xff); pUid[7] & 0xff);
return uid; return uid;
} }
...@@ -926,22 +979,22 @@ void Vehicle::_handleHilActuatorControls(mavlink_message_t &message) ...@@ -926,22 +979,22 @@ void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
mavlink_msg_hil_actuator_controls_decode(&message, &hil); mavlink_msg_hil_actuator_controls_decode(&message, &hil);
emit hilActuatorControlsChanged(hil.time_usec, hil.flags, emit hilActuatorControlsChanged(hil.time_usec, hil.flags,
hil.controls[0], hil.controls[0],
hil.controls[1], hil.controls[1],
hil.controls[2], hil.controls[2],
hil.controls[3], hil.controls[3],
hil.controls[4], hil.controls[4],
hil.controls[5], hil.controls[5],
hil.controls[6], hil.controls[6],
hil.controls[7], hil.controls[7],
hil.controls[8], hil.controls[8],
hil.controls[9], hil.controls[9],
hil.controls[10], hil.controls[10],
hil.controls[11], hil.controls[11],
hil.controls[12], hil.controls[12],
hil.controls[13], hil.controls[13],
hil.controls[14], hil.controls[14],
hil.controls[15], hil.controls[15],
hil.mode); hil.mode);
} }
void Vehicle::_handleCommandLong(mavlink_message_t& message) void Vehicle::_handleCommandLong(mavlink_message_t& message)
...@@ -954,19 +1007,19 @@ void Vehicle::_handleCommandLong(mavlink_message_t& message) ...@@ -954,19 +1007,19 @@ void Vehicle::_handleCommandLong(mavlink_message_t& message)
mavlink_msg_command_long_decode(&message, &cmd); mavlink_msg_command_long_decode(&message, &cmd);
switch (cmd.command) { switch (cmd.command) {
// Other component on the same system // Other component on the same system
// requests that QGC frees up the serial port of the autopilot // requests that QGC frees up the serial port of the autopilot
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (cmd.param6 > 0) { if (cmd.param6 > 0) {
// disconnect the USB link if its a direct connection to a Pixhawk // disconnect the USB link if its a direct connection to a Pixhawk
for (int i = 0; i < _links.length(); i++) { for (int i = 0; i < _links.length(); i++) {
SerialLink *sl = qobject_cast<SerialLink*>(_links.at(i)); SerialLink *sl = qobject_cast<SerialLink*>(_links.at(i));
if (sl && sl->getSerialConfig()->usbDirect()) { if (sl && sl->getSerialConfig()->usbDirect()) {
qDebug() << "Disconnecting:" << _links.at(i)->getName(); qDebug() << "Disconnecting:" << _links.at(i)->getName();
qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i)); qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i));
}
} }
} }
}
break; break;
} }
#endif #endif
...@@ -1351,6 +1404,7 @@ void Vehicle::_addLink(LinkInterface* link) ...@@ -1351,6 +1404,7 @@ void Vehicle::_addLink(LinkInterface* link)
qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16); qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
_links += link; _links += link;
_updatePriorityLink(); _updatePriorityLink();
_updateHighLatencyLink();
connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted); connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted); connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink); connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink);
...@@ -2098,9 +2152,9 @@ void Vehicle::_connectionActive(void) ...@@ -2098,9 +2152,9 @@ void Vehicle::_connectionActive(void)
// Re-negotiate protocol version for the link // Re-negotiate protocol version for the link
sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet.
MAV_CMD_REQUEST_PROTOCOL_VERSION, MAV_CMD_REQUEST_PROTOCOL_VERSION,
false, // No error shown if fails false, // No error shown if fails
1); // Request protocol version 1); // Request protocol version
} }
} }
...@@ -2618,7 +2672,7 @@ void Vehicle::setSoloFirmware(bool soloFirmware) ...@@ -2618,7 +2672,7 @@ void Vehicle::setSoloFirmware(bool soloFirmware)
} }
#if 0 #if 0
// Temporarily removed, waiting for new command implementation // Temporarily removed, waiting for new command implementation
void Vehicle::motorTest(int motor, int percent, int timeoutSecs) void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
{ {
doCommandLongUnverified(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs); doCommandLongUnverified(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs);
...@@ -2765,11 +2819,11 @@ void Vehicle::_ackMavlinkLogData(uint16_t sequence) ...@@ -2765,11 +2819,11 @@ void Vehicle::_ackMavlinkLogData(uint16_t sequence)
ack.target_component = _defaultComponentId; ack.target_component = _defaultComponentId;
ack.target_system = id(); ack.target_system = id();
mavlink_msg_logging_ack_encode_chan( mavlink_msg_logging_ack_encode_chan(
_mavlink->getSystemId(), _mavlink->getSystemId(),
_mavlink->getComponentId(), _mavlink->getComponentId(),
priorityLink()->mavlinkChannel(), priorityLink()->mavlinkChannel(),
&msg, &msg,
&ack); &ack);
sendMessageOnLink(priorityLink(), msg); sendMessageOnLink(priorityLink(), msg);
} }
...@@ -2778,7 +2832,7 @@ void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message) ...@@ -2778,7 +2832,7 @@ void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
mavlink_logging_data_t log; mavlink_logging_data_t log;
mavlink_msg_logging_data_decode(&message, &log); mavlink_msg_logging_data_decode(&message, &log);
emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
log.first_message_offset, QByteArray((const char*)log.data, log.length), false); log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
} }
void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message) void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
...@@ -2787,7 +2841,7 @@ void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message) ...@@ -2787,7 +2841,7 @@ void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
mavlink_msg_logging_data_acked_decode(&message, &log); mavlink_msg_logging_data_acked_decode(&message, &log);
_ackMavlinkLogData(log.sequence); _ackMavlinkLogData(log.sequence);
emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
log.first_message_offset, QByteArray((const char*)log.data, log.length), true); log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
} }
void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData) void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
...@@ -2946,7 +3000,7 @@ QString Vehicle::hobbsMeter() ...@@ -2946,7 +3000,7 @@ QString Vehicle::hobbsMeter()
static const char* HOOBS_LO = "LND_FLIGHT_T_LO"; static const char* HOOBS_LO = "LND_FLIGHT_T_LO";
//-- TODO: Does this exist on non PX4? //-- TODO: Does this exist on non PX4?
if (_parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_HI) && if (_parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_HI) &&
_parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) { _parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) {
Fact* factHi = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_HI); Fact* factHi = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_HI);
Fact* factLo = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_LO); Fact* factLo = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_LO);
uint64_t hobbsTimeSeconds = ((uint64_t)factHi->rawValue().toUInt() << 32 | (uint64_t)factLo->rawValue().toUInt()) / 1000000; uint64_t hobbsTimeSeconds = ((uint64_t)factHi->rawValue().toUInt() << 32 | (uint64_t)factLo->rawValue().toUInt()) / 1000000;
......
src/Vehicle/Vehicle.h
View file @ a52b110b
...@@ -915,6 +915,7 @@ private: ...@@ -915,6 +915,7 @@ private:
void _handleScaledPressure(mavlink_message_t& message); void _handleScaledPressure(mavlink_message_t& message);
void _handleScaledPressure2(mavlink_message_t& message); void _handleScaledPressure2(mavlink_message_t& message);
void _handleScaledPressure3(mavlink_message_t& message); void _handleScaledPressure3(mavlink_message_t& message);
void _handleHighLatency2(mavlink_message_t& message);
// ArduPilot dialect messages // ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT) #if !defined(NO_ARDUPILOT_DIALECT)
void _handleCameraFeedback(const mavlink_message_t& message); void _handleCameraFeedback(const mavlink_message_t& message);
......
src/VehicleSetup/SetupView.qml
View file @ a52b110b
...@@ -299,7 +299,9 @@ Rectangle { ...@@ -299,7 +299,9 @@ Rectangle {
SubMenuButton { SubMenuButton {
setupIndicator: false setupIndicator: false
exclusiveGroup: setupButtonGroup exclusiveGroup: setupButtonGroup
visible: QGroundControl.multiVehicleManager && QGroundControl.multiVehicleManager.parameterReadyVehicleAvailable && _corePlugin.showAdvancedUI visible: QGroundControl.multiVehicleManager.parameterReadyVehicleAvailable &&
!QGroundControl.multiVehicleManager.activeVehicle.highLatencyLink &&
_corePlugin.showAdvancedUI
text: qsTr("Parameters") text: qsTr("Parameters")
Layout.fillWidth: true Layout.fillWidth: true
......
src/comm/MAVLinkProtocol.cc
View file @ a52b110b
...@@ -267,11 +267,17 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -267,11 +267,17 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
} }
if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
// Start loggin on first heartbeat
_startLogging(); _startLogging();
mavlink_heartbeat_t heartbeat; mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(&message, &heartbeat); mavlink_msg_heartbeat_decode(&message, &heartbeat);
emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.mavlink_version, heartbeat.autopilot, heartbeat.type); emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.autopilot, heartbeat.type);
}
if (message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY2) {
_startLogging();
mavlink_high_latency2_t highLatency2;
mavlink_msg_high_latency2_decode(&message, &highLatency2);
emit vehicleHeartbeatInfo(link, message.sysid, message.compid, highLatency2.autopilot, highLatency2.type);
} }
// Detect if we are talking to an old radio not supporting v2 // Detect if we are talking to an old radio not supporting v2
......
src/comm/MAVLinkProtocol.h
View file @ a52b110b
...@@ -137,7 +137,7 @@ protected: ...@@ -137,7 +137,7 @@ protected:
signals: signals:
/// Heartbeat received on link /// Heartbeat received on link
void vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType); void vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType);
/** @brief Message received and directly copied via signal */ /** @brief Message received and directly copied via signal */
void messageReceived(LinkInterface* link, mavlink_message_t message); void messageReceived(LinkInterface* link, mavlink_message_t message);
......
src/comm/MockLink.cc
View file @ a52b110b
...@@ -13,7 +13,7 @@ ...@@ -13,7 +13,7 @@
#include "QGCApplication.h" #include "QGCApplication.h"
#ifdef UNITTEST_BUILD #ifdef UNITTEST_BUILD
#include "UnitTest.h" #include "UnitTest.h"
#endif #endif
#include <QTimer> #include <QTimer>
...@@ -166,27 +166,35 @@ void MockLink::run(void) ...@@ -166,27 +166,35 @@ void MockLink::run(void)
void MockLink::_run1HzTasks(void) void MockLink::_run1HzTasks(void)
{ {
if (_mavlinkStarted && _connected) { if (_mavlinkStarted && _connected) {
_sendVibration(); if (_highLatency) {
_sendADSBVehicles(); _sendHighLatency2();
if (!qgcApp()->runningUnitTests()) {
// Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
_sendRCChannels();
}
if (_sendHomePositionDelayCount > 0) {
// We delay home position for better testing
_sendHomePositionDelayCount--;
} else { } else {
_sendHomePosition(); _sendVibration();
} _sendADSBVehicles();
if (_sendStatusText) { if (!qgcApp()->runningUnitTests()) {
_sendStatusText = false; // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
_sendStatusTextMessages(); _sendRCChannels();
}
if (_sendHomePositionDelayCount > 0) {
// We delay home position for better testing
_sendHomePositionDelayCount--;
} else {
_sendHomePosition();
}
if (_sendStatusText) {
_sendStatusText = false;
_sendStatusTextMessages();
}
} }
} }
} }
void MockLink::_run10HzTasks(void) void MockLink::_run10HzTasks(void)
{ {
if (_highLatency) {
return;
}
if (_mavlinkStarted && _connected) { if (_mavlinkStarted && _connected) {
_sendHeartBeat(); _sendHeartBeat();
if (_sendGPSPositionDelayCount > 0) { if (_sendGPSPositionDelayCount > 0) {
...@@ -200,6 +208,10 @@ void MockLink::_run10HzTasks(void) ...@@ -200,6 +208,10 @@ void MockLink::_run10HzTasks(void)
void MockLink::_run500HzTasks(void) void MockLink::_run500HzTasks(void)
{ {
if (_highLatency) {
return;
}
if (_mavlinkStarted && _connected) { if (_mavlinkStarted && _connected) {
_paramRequestListWorker(); _paramRequestListWorker();
_logDownloadWorker(); _logDownloadWorker();
...@@ -296,6 +308,44 @@ void MockLink::_sendHeartBeat(void) ...@@ -296,6 +308,44 @@ void MockLink::_sendHeartBeat(void)
respondWithMavlinkMessage(msg); respondWithMavlinkMessage(msg);
} }
void MockLink::_sendHighLatency2(void)
{
mavlink_message_t msg;
mavlink_msg_high_latency2_pack_chan(_vehicleSystemId,
_vehicleComponentId,
_mavlinkChannel,
&msg,
0, // timestamp
_vehicleType, // MAV_TYPE
_firmwareType, // MAV_AUTOPILOT
_flightModeEnumValue(), // flight_mode
(int32_t)(_vehicleLatitude * 1E7),
(int32_t)(_vehicleLongitude * 1E7),
(int16_t)_vehicleAltitude,
(int16_t)_vehicleAltitude, // target_altitude,
0, // heading
0, // target_heading
0, // target_distance
0, // throttle
0, // airspeed
0, // airspeed_sp
0, // groundspeed
0, // windspeed,
0, // wind_heading
UINT8_MAX, // eph not known
UINT8_MAX, // epv not known
0, // temperature_air
0, // climb_rate
-1, // battery, do not use?
0, // wp_num
0, // failure_flags
0, // failsafe
0, 0, 0); // custom0, custom1, custom2
respondWithMavlinkMessage(msg);
}
void MockLink::_sendVibration(void) void MockLink::_sendVibration(void)
{ {
mavlink_message_t msg; mavlink_message_t msg;
...@@ -842,8 +892,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) ...@@ -842,8 +892,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
case MAV_CMD_USER_3: case MAV_CMD_USER_3:
// Test command which returns MAV_RESULT_ACCEPTED on second attempt // Test command which returns MAV_RESULT_ACCEPTED on second attempt
if (firstCmdUser3) { if (firstCmdUser3) {
firstCmdUser3 = false; firstCmdUser3 = false;
return; return;
} else { } else {
firstCmdUser3 = true; firstCmdUser3 = true;
commandResult = MAV_RESULT_ACCEPTED; commandResult = MAV_RESULT_ACCEPTED;
...@@ -852,8 +902,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) ...@@ -852,8 +902,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
case MAV_CMD_USER_4: case MAV_CMD_USER_4:
// Test command which returns MAV_RESULT_FAILED on second attempt // Test command which returns MAV_RESULT_FAILED on second attempt
if (firstCmdUser4) { if (firstCmdUser4) {
firstCmdUser4 = false; firstCmdUser4 = false;
return; return;
} else { } else {
firstCmdUser4 = true; firstCmdUser4 = true;
commandResult = MAV_RESULT_FAILED; commandResult = MAV_RESULT_FAILED;
...@@ -943,8 +993,8 @@ void MockLink::_sendHomePosition(void) ...@@ -943,8 +993,8 @@ void MockLink::_sendHomePosition(void)
(int32_t)(_vehicleAltitude * 1000), (int32_t)(_vehicleAltitude * 1000),
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
&bogus[0], &bogus[0],
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
0); 0);
respondWithMavlinkMessage(msg); respondWithMavlinkMessage(msg);
} }
...@@ -972,7 +1022,7 @@ void MockLink::_sendGpsRawInt(void) ...@@ -972,7 +1022,7 @@ void MockLink::_sendGpsRawInt(void)
0, // Altitude uncertainty in meters * 1000 (positive for up). 0, // Altitude uncertainty in meters * 1000 (positive for up).
0, // Speed uncertainty in meters * 1000 (positive for up). 0, // Speed uncertainty in meters * 1000 (positive for up).
0); // Heading / track uncertainty in degrees * 1e5. 0); // Heading / track uncertainty in degrees * 1e5.
respondWithMavlinkMessage(msg); respondWithMavlinkMessage(msg);
} }
void MockLink::_sendStatusTextMessages(void) void MockLink::_sendStatusTextMessages(void)
...@@ -1237,9 +1287,9 @@ void MockLink::_handleLogRequestData(const mavlink_message_t& msg) ...@@ -1237,9 +1287,9 @@ void MockLink::_handleLogRequestData(const mavlink_message_t& msg)
mavlink_msg_log_request_data_decode(&msg, &request); mavlink_msg_log_request_data_decode(&msg, &request);
if (_logDownloadFilename.isEmpty()) { if (_logDownloadFilename.isEmpty()) {
#ifdef UNITTEST_BUILD #ifdef UNITTEST_BUILD
_logDownloadFilename = UnitTest::createRandomFile(_logDownloadFileSize); _logDownloadFilename = UnitTest::createRandomFile(_logDownloadFileSize);
#endif #endif
} }
if (request.id != 0) { if (request.id != 0) {
...@@ -1320,3 +1370,8 @@ void MockLink::_sendADSBVehicles(void) ...@@ -1320,3 +1370,8 @@ void MockLink::_sendADSBVehicles(void)
respondWithMavlinkMessage(responseMsg); respondWithMavlinkMessage(responseMsg);
} }
uint8_t MockLink::_flightModeEnumValue(void)
{
return FLIGHT_MODE_STABILIZED;
}
src/comm/MockLink.h
View file @ a52b110b
...@@ -175,6 +175,7 @@ private: ...@@ -175,6 +175,7 @@ private:
// MockLink methods // MockLink methods
void _sendHeartBeat(void); void _sendHeartBeat(void);
void _sendHighLatency2(void);
void _handleIncomingNSHBytes(const char* bytes, int cBytes); void _handleIncomingNSHBytes(const char* bytes, int cBytes);
void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes); void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes);
void _loadParams(void); void _loadParams(void);
...@@ -201,6 +202,7 @@ private: ...@@ -201,6 +202,7 @@ private:
void _logDownloadWorker(void); void _logDownloadWorker(void);
void _sendADSBVehicles(void); void _sendADSBVehicles(void);
void _moveADSBVehicle(void); void _moveADSBVehicle(void);
uint8_t _flightModeEnumValue(void);
static MockLink* _startMockLink(MockConfiguration* mockConfig); static MockLink* _startMockLink(MockConfiguration* mockConfig);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment