void mzd_gps2_start()
{
if (gps_client != NULL)
return;
try
{
DBus::Connection gpservice_bus(SERVICE_BUS_ADDRESS, false);
gpservice_bus.register_bus();
gps_client = new GPSLDSCLient(gpservice_bus, "/com/jci/lds/data", "com.jci.lds.data");
}
catch(DBus::Error& error)
{
loge("DBUS: Failed to connect to SERVICE bus %s: %s", error.name(), error.message());
mzd_gps2_stop();
return;
}
printf("GPS service connection established.\n");
}
static void gps_thread_func(std::condition_variable& quitcv, std::mutex& quitmutex)
{
GPSData data, newData;
uint64_t oldTs = 0;
int debugLogCount = 0;
mzd_gps2_start();
//Not sure if this is actually required but the built-in Nav code on CMU does it
mzd_gps2_set_enabled(true);
config::readConfig();
while (true)
{
if (config::carGPS && mzd_gps2_get(newData) && !data.IsSame(newData))
{
data = newData;
timeval tv;
gettimeofday(&tv, nullptr);
uint64_t timestamp = tv.tv_sec * 1000000 + tv.tv_usec;
if (debugLogCount < 50) //only print the first 50 to avoid spamming the log and breaking the opera text box
{
logd("GPS data: %d %d %f %f %d %f %f %f %f \n",data.positionAccuracy, data.uTCtime, data.latitude, data.longitude, data.altitude, data.heading, data.velocity, data.horizontalAccuracy, data.verticalAccuracy);
logd("Delta %f\n", (timestamp - oldTs)/1000000.0);
debugLogCount++;
}
oldTs = timestamp;
g_hu->hu_queue_command([data, timestamp](IHUConnectionThreadInterface& s)
{
HU::SensorEvent sensorEvent;
HU::SensorEvent::LocationData* location = sensorEvent.add_location_data();
//AA uses uS and the gps data just has seconds, just use the current time to get more precision so AA can
//interpolate better
location->set_timestamp(timestamp);
location->set_latitude(static_cast<int32_t>(data.latitude * 1E7));
location->set_longitude(static_cast<int32_t>(data.longitude * 1E7));
// If the sd card exists then reverse heading. This should only be used on installs that have the
// reversed heading issue.
double newHeading = data.heading;
if (config::reverseGPS)
{
const char* sdCardFolder;
sdCardFolder = SD_CARD_PATH;
struct stat sb;
if (stat(sdCardFolder, &sb) == 0 && S_ISDIR(sb.st_mode))
{
newHeading = data.heading + 180;
if (newHeading >= 360)
{
newHeading = newHeading - 360;
}
}
}
location->set_bearing(static_cast<int32_t>(newHeading * 1E6));
//assuming these are the same units as the Android Location API (the rest are)
double velocityMetersPerSecond = data.velocity * 0.277778; //convert km/h to m/s
location->set_speed(static_cast<int32_t>(velocityMetersPerSecond * 1E3));
location->set_altitude(static_cast<int32_t>(data.altitude * 1E2));
location->set_accuracy(static_cast<int32_t>(data.horizontalAccuracy * 1E3));
s.hu_aap_enc_send_message(0, AA_CH_SEN, HU_SENSOR_CHANNEL_MESSAGE::SensorEvent, sensorEvent);
});
}
{
std::unique_lock<std::mutex> lk(quitmutex);
//The timestamps on the GPS events are in seconds, but based on logging the data actually changes faster with the same timestamp
if (quitcv.wait_for(lk, std::chrono::milliseconds(250)) == std::cv_status::no_timeout)
{
break;
}
}
}
mzd_gps2_set_enabled(false);
mzd_gps2_stop();
}
