diff options
Diffstat (limited to 'navigation/navigation.cpp')
| -rw-r--r-- | navigation/navigation.cpp | 194 |
1 files changed, 121 insertions, 73 deletions
diff --git a/navigation/navigation.cpp b/navigation/navigation.cpp index cb9f6cd..a39684f 100644 --- a/navigation/navigation.cpp +++ b/navigation/navigation.cpp @@ -1,33 +1,23 @@ /* - * Copyright (C) 2019-2022 Konsulko Group + * Copyright (C) 2019-2023 Konsulko Group * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. + * SPDX-License-Identifier: Apache-2.0 */ #include <QDebug> +#include <math.h> #include "navigation.h" #include "vehiclesignals.h" -Navigation::Navigation(VehicleSignals *vs, QObject * parent) : - QObject(parent), - m_vs(vs), - m_connected(false) +#define ROUND9(x) (round((x) * 1000000000) / 1000000000) +Navigation::Navigation(VehicleSignals *vs, bool router, QObject * parent) : + m_vs(vs), + m_router(router), + QObject(parent) { QObject::connect(m_vs, &VehicleSignals::connected, this, &Navigation::onConnected); - QObject::connect(m_vs, &VehicleSignals::authorized, this, &Navigation::onAuthorized); - QObject::connect(m_vs, &VehicleSignals::disconnected, this, &Navigation::onDisconnected); if (m_vs) m_vs->connect(); @@ -40,7 +30,7 @@ Navigation::~Navigation() void Navigation::sendWaypoint(double lat, double lon) { - if (!(m_vs && m_connected)) + if (!(m_vs && m_connected && m_router)) return; // The original implementation resulted in at least 9 decimal places @@ -48,18 +38,18 @@ void Navigation::sendWaypoint(double lat, double lon) // practice going from the QString default 6 to 9 does make a difference // with respect to smoothness in the position-based map rotations done // in tbtnavi. - m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude", QString::number(lat, 'f', 9)); - m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude", QString::number(lon, 'f', 9)); + m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude", lat); + m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude", lon); } void Navigation::broadcastPosition(double lat, double lon, double drc, double dst) { - if (!(m_vs && m_connected)) + if (!(m_vs && m_connected && m_router)) return; - m_vs->set("Vehicle.CurrentLocation.Latitude", QString::number(lat, 'f', 9)); - m_vs->set("Vehicle.CurrentLocation.Longitude", QString::number(lon, 'f', 9)); - m_vs->set("Vehicle.CurrentLocation.Heading", QString::number(drc, 'f', 9)); + m_vs->set("Vehicle.CurrentLocation.Latitude", lat); + m_vs->set("Vehicle.CurrentLocation.Longitude", lon); + m_vs->set("Vehicle.CurrentLocation.Heading", drc); // NOTES: // - This signal is an AGL addition, it may make sense to engage with the @@ -67,24 +57,24 @@ void Navigation::broadcastPosition(double lat, double lon, double drc, double ds // - The signal makes more sense in kilometers wrt VSS expectations, so // conversion from meters happens here for now to avoid changing the // existing clients. This may be worth revisiting down the road. - m_vs->set("Vehicle.Cabin.Infotainment.Navigation.ElapsedDistance", QString::number(dst / 1000, 'f', 9)); + m_vs->set("Vehicle.Cabin.Infotainment.Navigation.ElapsedDistance", (float) (dst / 1000)); } void Navigation::broadcastRouteInfo(double lat, double lon, double route_lat, double route_lon) { - if (!(m_vs && m_connected)) + if (!(m_vs && m_connected && m_router)) return; - m_vs->set("Vehicle.CurrentLocation.Latitude", QString::number(lat, 'f', 9)); - m_vs->set("Vehicle.CurrentLocation.Longitude", QString::number(lon, 'f', 9)); + m_vs->set("Vehicle.CurrentLocation.Latitude", lat); + m_vs->set("Vehicle.CurrentLocation.Longitude", lon); - m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude", QString::number(route_lat, 'f', 9)); - m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude", QString::number(route_lon, 'f', 9)); + m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude", route_lat, true); + m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude", route_lon, true); } void Navigation::broadcastStatus(QString state) { - if (!(m_vs && m_connected)) + if (!(m_vs && m_connected && m_router)) return; // The signal states have been changed to all uppercase to match @@ -97,58 +87,121 @@ void Navigation::onConnected() if (!m_vs) return; - m_vs->authorize(); -} - -void Navigation::onAuthorized() -{ - if (!m_vs) - return; - m_connected = true; - QObject::connect(m_vs, &VehicleSignals::signalNotification, this, &Navigation::onSignalNotification); + if (m_router) { + qInfo() << "Connected as router"; + QObject::connect(m_vs, &VehicleSignals::signalNotification, this, &Navigation::onSignalNotificationRouter); + } else { + QObject::connect(m_vs, &VehicleSignals::signalNotification, this, &Navigation::onSignalNotification); + } // NOTE: This signal is another AGL addition where it is possible // upstream may be open to adding it to VSS. - m_vs->subscribe("Vehicle.Cabin.Infotainment.Navigation.State"); - m_vs->subscribe("Vehicle.CurrentLocation.Latitude"); - m_vs->subscribe("Vehicle.CurrentLocation.Longitude"); - m_vs->subscribe("Vehicle.CurrentLocation.Heading"); - m_vs->subscribe("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude"); - m_vs->subscribe("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude"); - m_vs->subscribe("Vehicle.Cabin.Infotainment.Navigation.ElapsedDistance"); -} - -void Navigation::onDisconnected() -{ - QObject::disconnect(m_vs, &VehicleSignals::signalNotification, this, &Navigation::onSignalNotification); - - m_connected = false; + QMap<QString, bool> s; + s["Vehicle.Cabin.Infotainment.Navigation.State"] = false; + if (!m_router) { + // Router broadcasts these signals and does not need to listen to them + s["Vehicle.CurrentLocation.Latitude"] = false; + s["Vehicle.CurrentLocation.Longitude"] = false; + s["Vehicle.CurrentLocation.Heading"] = false; + s["Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude"] = false; + s["Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude"] = false; + s["Vehicle.Cabin.Infotainment.Navigation.ElapsedDistance"] = false; + } else { + // Router acts as actuator for these signals + // This is a bit convoluted, but would allow wiring up external destination + // setting with e.g. Alexa as has been done in the past. + s["Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude"] = true; + s["Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude"] = true; + } + m_vs->subscribe(s); } void Navigation::onSignalNotification(QString path, QString value, QString timestamp) { - // NOTE: Since all the known AGL users of the VSS signals are users of - // this API, we know that updates occur in certain sequences and - // can leverage this to roll up for emitting the existing events. - // This is the path of least effort with respect to changing - // the existing clients, but it may make sense down the road to - // either switch them to using VehicleSignals directly or having - // a more granular signal scheme that maps more directly onto - // VSS. if (path == "Vehicle.Cabin.Infotainment.Navigation.State") { QVariantMap event; event["state"] = value; emit statusEvent(event); } else if (path == "Vehicle.CurrentLocation.Latitude") { + m_position_mutex.lock(); m_latitude = value.toDouble(); + m_latitude_set = true; + handlePositionUpdate(); + m_position_mutex.unlock(); } else if (path == "Vehicle.CurrentLocation.Longitude") { + m_position_mutex.lock(); m_longitude = value.toDouble(); + m_longitude_set = true; + handlePositionUpdate(); + m_position_mutex.unlock(); } else if (path == "Vehicle.CurrentLocation.Heading") { + m_position_mutex.lock(); m_heading = value.toDouble(); + m_heading_set = true; + handlePositionUpdate(); + m_position_mutex.unlock(); } else if (path == "Vehicle.Cabin.Infotainment.Navigation.ElapsedDistance") { + m_position_mutex.lock(); m_distance = value.toDouble(); + m_distance_set = true; + handlePositionUpdate(); + m_position_mutex.unlock(); + } else if (path == "Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude") { + m_position_mutex.lock(); + m_dest_latitude = value.toDouble(); + m_dest_latitude_set = true; + handlePositionUpdate(); + m_position_mutex.unlock(); + } else if (path == "Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude") { + m_position_mutex.lock(); + m_dest_longitude = value.toDouble(); + m_dest_longitude_set = true; + handlePositionUpdate(); + m_position_mutex.unlock(); + + // NOTE: Potentially could emit a waypointsEvent here, but + // nothing in the demo currently requires it, so do + // not bother for now. If something like Alexa is + // added it or some other replacement / rework will + // be required. + } +} + +void Navigation::onSignalNotificationRouter(QString path, QString value, QString timestamp) +{ + if (path == "Vehicle.Cabin.Infotainment.Navigation.State") { + QVariantMap event; + event["state"] = value; + emit statusEvent(event); + } else if (path == "Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude") { + m_vs->set(path, value.toDouble()); + } else if (path == "Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude") { + m_vs->set(path, value.toDouble()); + } +} + +// Private + +void Navigation::handlePositionUpdate() +{ + // NOTE: Since all the known AGL users of the VSS signals are users of + // this API, we know that updates occur in certain sequences and + // can leverage this to roll up for emitting the existing events. + // + // The switch to the gRPC API for KUKSA.val complicates this as + // we now have to live with the combination of only getting + // notified when a signal changes to a new value on top of + // notifications not necessarily happening in order by the time + // they get through gRPC client/server/client/Qt stacks. The "set" + // flags give us something that mostly works for now, but further + // investigation is required. It is possible that either a switch + // to a custom gRPC API (i.e. standalone nav service daemon) or + // pushing for a rolled up position signal in a struct in upstream + // VSS might be needed to do this better. + + if (m_latitude_set && m_longitude_set && m_heading_set && m_distance_set) { QVariantMap event; event["position"] = "car"; event["latitude"] = m_latitude; @@ -156,10 +209,9 @@ void Navigation::onSignalNotification(QString path, QString value, QString times event["direction"] = m_heading; event["distance"] = m_distance * 1000; emit positionEvent(event); - } else if (path == "Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude") { - m_dest_latitude = value.toDouble(); - } else if (path == "Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude") { - m_dest_longitude = value.toDouble(); + // The distance being updated is our trigger, so clear its flag. + m_distance_set = false; + } else if (m_latitude_set && m_longitude_set && m_dest_latitude_set && m_dest_longitude_set) { QVariantMap event; event["position"] = "route"; event["latitude"] = m_latitude; @@ -167,11 +219,7 @@ void Navigation::onSignalNotification(QString path, QString value, QString times event["route_latitude"] = m_dest_latitude; event["route_longitude"] = m_dest_longitude; emit positionEvent(event); - - // NOTE: Potentially could emit a waypointsEvent here, but - // nothing in the demo currently requires it, so do - // not bother for now. If something like Alexa is - // added it or some other replacement / rework will - // be required. + // The destination values are the triggers, so clear their flags. + m_dest_latitude_set = m_dest_longitude_set = false; } } |