Skip to content

UTM: Refactor Network Remote ID telemetry control based on vehicle state #12073

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 6 commits into from
Nov 23, 2024
Merged

UTM: Refactor Network Remote ID telemetry control based on vehicle state #12073

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
1b7ff8a
Update vehicle state based on MAVLINK messages
royzah Nov 7, 2024
e152868
Merge branch 'master' into master
royzah Nov 20, 2024
21e0cb5
Fix typo in state variables in networkRemoteID function
royzah Nov 21, 2024
26dc98f
Merge remote-tracking branch 'origin/master'
royzah Nov 21, 2024
dd1f568
Merge branch 'master' into master
royzah Nov 21, 2024
10c1a6c
Merge branch 'master' into master
royzah Nov 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
88 changes: 47 additions & 41 deletions src/UTMSP/UTMSPServiceController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,60 +72,65 @@ std::string UTMSPServiceController::flightPlanAuthorization()

return _responseFlightID.toStdString();
}
bool UTMSPServiceController::networkRemoteID(const mavlink_message_t& message,
bool UTMSPServiceController::networkRemoteID(const mavlink_message_t &message,
const std::string &serialNumber,
const std::string &operatorID,
const std::string &flightID)
{
double vehicleLatitude;
double vehicleLongitude;
double lastLatitude;
double lastLongitude;

switch (message.msgid)
{
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:

// rate-limit updates to 3 Hz
static uint8_t lastVehicleState = MAV_STATE_UNINIT;
switch (message.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(&message, &heartbeat);
if (lastVehicleState == MAV_STATE_ACTIVE && heartbeat.system_status != MAV_STATE_ACTIVE) {
// State 4 --> Stop telemetry when transitioning out of ACTIVE state
bool stopFlag = _utmspNetworkRemoteIDManager.stopTelemetry();
emit stopTelemetryFlagChanged(stopFlag);
_streamingFlag = true;
}
lastVehicleState = heartbeat.system_status;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
// Only process if vehicle is in ACTIVE state
if (lastVehicleState != MAV_STATE_ACTIVE) {
return false;
}
// Rate-limit updates to 3 Hz
if (!_timerLastSent.hasExpired(3000)) {
return false;
}
_timerLastSent.restart();

mavlink_global_position_int_t globalPosition;
mavlink_msg_global_position_int_decode(&message, &globalPosition);
_vehicleLatitude = (static_cast<double>(globalPosition.lat /1e7));
_vehicleLongitude = (static_cast<double>(globalPosition.lon / 1e7));
_vehicleAltitude = static_cast<double>(globalPosition.alt) / 1000;
_vehicleHeading = static_cast<double>(globalPosition.hdg);
_vehicleVelocityX = (globalPosition.vx / 100.f);
_vehicleVelocityY = (globalPosition.vy / 100.f);
_vehicleVelocityZ = (globalPosition.vz / 100.f);
// Update vehicle state
_vehicleLatitude = static_cast<double>(globalPosition.lat) / 1e7;
_vehicleLongitude = static_cast<double>(globalPosition.lon) / 1e7;
_vehicleAltitude = static_cast<double>(globalPosition.alt) / 1000;
_vehicleHeading = static_cast<double>(globalPosition.hdg);
_vehicleVelocityX = globalPosition.vx / 100.f;
_vehicleVelocityY = globalPosition.vy / 100.f;
_vehicleVelocityZ = globalPosition.vz / 100.f;
_vehicleRelativeAltitude = static_cast<double>(globalPosition.relative_alt) / 1000.0;

vehicleLatitude = std::round(_vehicleLatitude* 1e4)/ 1e4;
vehicleLongitude = std::round(_vehicleLongitude* 1e4)/ 1e4;
lastLatitude = std::round(_lastLatitude* 1e4)/ 1e4;
lastLongitude = std::round(_lastLongitude* 1e4)/ 1e4;

if((std::abs(vehicleLatitude-lastLatitude) > 0.0001) || (std::abs(vehicleLongitude-lastLongitude) > 0.0001))
{
// State 3 --> Start telemetry
_currentState = UTMSPFlightPlanManager::FlightState::StartTelemetryStreaming;
_utmspNetworkRemoteIDManager.startTelemetry(_vehicleLatitude, _vehicleLongitude, _vehicleAltitude, _vehicleHeading, _vehicleVelocityX, _vehicleVelocityY, _vehicleVelocityZ, _vehicleRelativeAltitude, serialNumber, operatorID, flightID);
_utmspFlightPlanManager.updateFlightPlanState(_currentState);
_streamingFlag = false;
}
else
{
// State 4 --> Stop telemetry
bool stopflag = _utmspNetworkRemoteIDManager.stopTelemetry();
emit stopTelemetryFlagChanged(stopflag);
_streamingFlag = true;
}
// State 3 --> Start telemetry
_currentState = UTMSPFlightPlanManager::FlightState::StartTelemetryStreaming;
_utmspNetworkRemoteIDManager.startTelemetry(_vehicleLatitude,
_vehicleLongitude,
_vehicleAltitude,
_vehicleHeading,
_vehicleVelocityX,
_vehicleVelocityY,
_vehicleVelocityZ,
_vehicleRelativeAltitude,
serialNumber,
operatorID,
flightID);
_utmspFlightPlanManager.updateFlightPlanState(_currentState);
_streamingFlag = false;
break;
}

case MAVLINK_MSG_ID_GPS_RAW_INT:
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t gps_raw;
mavlink_msg_gps_raw_int_decode(&message, &gps_raw);
if (gps_raw.eph == UINT16_MAX) {
Expand All @@ -135,6 +140,7 @@ bool UTMSPServiceController::networkRemoteID(const mavlink_message_t& message,
}
break;
}
}

return _streamingFlag;
}
Loading