summaryrefslogtreecommitdiffstats
path: root/navigation
diff options
context:
space:
mode:
Diffstat (limited to 'navigation')
-rw-r--r--navigation/navigation.cpp194
-rw-r--r--navigation/navigation.h35
2 files changed, 139 insertions, 90 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;
}
}
diff --git a/navigation/navigation.h b/navigation/navigation.h
index 2c8a8ff..eb15ed3 100644
--- a/navigation/navigation.h
+++ b/navigation/navigation.h
@@ -1,17 +1,7 @@
/*
- * 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
*/
#ifndef NAVIGATION_H
@@ -19,6 +9,7 @@
#include <QObject>
#include <QVariant>
+#include <QMutex>
class VehicleSignals;
@@ -27,7 +18,7 @@ class Navigation : public QObject
Q_OBJECT
public:
- explicit Navigation(VehicleSignals *vs, QObject *parent = Q_NULLPTR);
+ explicit Navigation(VehicleSignals *vs, bool router = false, QObject *parent = Q_NULLPTR);
virtual ~Navigation();
Q_INVOKABLE void broadcastPosition(double lat, double lon, double drc, double dst);
@@ -44,19 +35,29 @@ signals:
private slots:
void onConnected();
- void onAuthorized();
- void onDisconnected();
void onSignalNotification(QString path, QString value, QString timestamp);
+ void onSignalNotificationRouter(QString path, QString value, QString timestamp);
private:
+ void handlePositionUpdate();
+
VehicleSignals *m_vs;
- bool m_connected;
+ bool m_router = false;
+ bool m_connected = false;
+
+ QMutex m_position_mutex;
double m_latitude;
+ bool m_latitude_set = false;
double m_longitude;
+ bool m_longitude_set = false;
double m_heading;
- double m_distance;
+ bool m_heading_set = false;
+ double m_distance = 0.0;
+ bool m_distance_set = false;
double m_dest_latitude;
+ bool m_dest_latitude_set = false;
double m_dest_longitude;
+ bool m_dest_longitude_set = false;
};
#endif // NAVIGATION_H