Add support for Forza telemetry to PQ35 CAN bus

bin/Makefile

@@ -7,7 +7,8 @@ INCLUDE_PATH	= ../include
 CFLAGS		+= -I$(INCLUDE_PATH)
 LDFLAGS		= -L../src -lhexagram
 STATIC		= ../src/libhexagram.a
-OBJS		= capture.o can2dump.o replay.o pcapreplay.o pcap2can.o main.o
+OBJS		= capture.o can2dump.o replay.o pcapreplay.o pcap2can.o main.o \
+		  dash2can.o
 NAME		= hexagram
 
 INSTALL		= install

bin/dash2can.c

@@ -0,0 +1,143 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#include <math.h>
+#include <inttypes.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <errno.h>
+
+#include <hexagram/can.h>
+#include <hexagram/capture.h>
+#include <hexagram/telemetry/forza-dash.h>
+
+#include "dash2can.h"
+
+char *hexagram_arglist_dash2can(void) {
+    return "udp-port canif";
+}
+
+static void usage(int argc, char **argv, const char *message, ...) {
+    if (message) {
+        va_list args;
+
+        va_start(args, message);
+        vfprintf(stderr, message, args);
+        va_end(args);
+    }
+
+    fprintf(stderr, "usage: hexagram %s %s\n", argv[0], hexagram_arglist_dash2can());
+
+    exit(1);
+}
+
+int hexagram_main_dash2can(int argc, char **argv) {
+    int sock;
+
+    struct sockaddr_in sockaddr;
+    hexagram_can_if *can_if;
+
+    if (argc < 2) {
+        usage(argc, argv, "No listening UDP port provided");
+    } else if (argc < 3) {
+        usage(argc, argv, "No CAN interface name provided");
+    } else if (argc > 3) {
+        usage(argc, argv, "Too many arguments provided");
+    }
+
+    if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+        goto error_socket;
+    }
+
+    memset(&sockaddr, '\0', sizeof(sockaddr));
+    sockaddr.sin_family      = AF_INET;
+    sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+    sockaddr.sin_port        = htons(atoi(argv[1]));
+
+    if (bind(sock, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) < 0) {
+        goto error_bind;
+    }
+
+    if ((can_if = hexagram_can_if_open(argv[2])) == NULL) {
+        perror("hexagram_can_if_open()");
+
+        goto error_can_if_open;
+    }
+
+    while (1) {
+        hexagram_telemetry_forza_dash_packet packet;
+        struct can_frame frame;
+        ssize_t len;
+
+        float speed_ms = sqrt(powf(packet.velocity.x, 2)
+                            + powf(packet.velocity.z, 2));
+
+        uint16_t engine_rpm = (uint16_t)(packet.engine_rpm.current * 4),
+                 speed_rps  = (uint16_t)(speed_ms * 2.0);
+
+        if ((len = recv(sock, &packet, sizeof(packet), 0)) < 0) {
+            goto error_io;
+        } else if (len < (ssize_t)sizeof(packet)) {
+            continue;
+        }
+
+        /* Engine speed */
+        memset(&frame, '\0', sizeof(frame));
+        frame.can_id  = 0x280;
+        frame.data[3] = (engine_rpm & 0xff00) >> 8;
+        frame.data[2] =  engine_rpm & 0x00ff;
+
+        if (hexagram_can_if_write(can_if, &frame) < 0) {
+            goto error_io;
+        }
+
+        /* Engine temperature  */
+        memset(frame.data, '\0', sizeof(frame.data));
+        frame.can_id  = 0x288;
+        frame.data[1] = (uint8_t)((87.0 / 0.75) + 48);
+
+        if (hexagram_can_if_write(can_if, &frame) < 0) {
+            goto error_io;
+        }
+
+        /* Fuel status */
+        memset(frame.data, '\0', sizeof(frame.data));
+        frame.can_id  = 0x320;
+        frame.data[2] = ((uint8_t)(packet.car.fuel / 15.0) & 0xff);
+
+        if (hexagram_can_if_write(can_if, &frame) < 0) {
+            goto error_io;
+        }
+
+        /* Vehicle speed */
+        memset(frame.data, '\0', sizeof(frame.data));
+        frame.can_id  = 0x5a0;
+        frame.data[1] = (speed_rps & 0xff00) >> 8;
+        frame.data[2] =  speed_rps & 0x00ff;
+
+        if (hexagram_can_if_write(can_if, &frame) < 0) {
+            goto error_io;
+        }
+    }
+
+    hexagram_can_if_close(can_if);
+
+    close(sock);
+
+    return 0;
+
+error_io:
+    fprintf(stderr, "BOO: %s\n", strerror(errno));
+    hexagram_can_if_close(can_if);
+
+error_can_if_open:
+error_bind:
+    close(sock);
+
+error_socket:
+    return 1;
+}

bin/dash2can.h

@@ -0,0 +1,8 @@
+#ifndef _DASH2CAN_H
+#define _DASH2CAN_H
+
+char *hexagram_arglist_dash2can(void);
+
+int hexagram_main_dash2can(int argc, char **argv);
+
+#endif /* _DASH2CAN_H */

bin/main.c

@@ -8,6 +8,7 @@
 #include "can2dump.h"
 #include "pcap2can.h"
 #include "pcapreplay.h"
+#include "dash2can.h"
 
 typedef struct {
     const char *name;
@@ -21,6 +22,7 @@ hexagram_app apps[] = {
     { "can2dump",   hexagram_arglist_can2dump,   hexagram_main_can2dump   },
     { "pcap2can",   hexagram_arglist_pcap2can,   hexagram_main_pcap2can   },
     { "pcapreplay", hexagram_arglist_pcapreplay, hexagram_main_pcapreplay },
+    { "dash2can",   hexagram_arglist_dash2can,   hexagram_main_dash2can   },
     { NULL, NULL, NULL }
 };
 

include/hexagram/speedo.h

@@ -6,6 +6,7 @@
 #include <hexagram/gauge.h>
 
 #define HEXAGRAM_SPEEDO_MAX_KPH 289.682
+#define HEXAGRAM_SPEEDO_MAX_MPH 180.000
 
 typedef struct _hexagram_speedo {
     hexagram_gauge gauge;

include/hexagram/telemetry/forza-dash.h

@@ -4,14 +4,6 @@
 #include <inttypes.h>
 #include <hexagram/types.h>
 
-hexagram_coord {
-    float x, y, z;
-};
-
-hexagram_corner {
-    float front_left, front_right, rear_left, rear_right;
-};
-
 typedef struct _hexagram_telemetry_forza_dash_car {
     int32_t id,
             class,

src/speedo.c

@@ -61,16 +61,17 @@ void hexagram_speedo_draw_face(hexagram_speedo *speedo,
 void hexagram_speedo_draw_needle(hexagram_speedo *speedo,
                                  cairo_t *cr,
                                  double rps) {
-    double kph = (2.00152 * rps * 3600) / 1000.0;
+    double kph = (2.00152 * rps * 3600) / 1000.0,
+           mph = kph * 0.621371;
 
-    if (kph < 0) {
-        kph = 0;
-    } else if (kph > 290) {
-        kph = 290;
+    if (mph < 0) {
+        mph = 0;
+    } else if (mph > HEXAGRAM_SPEEDO_MAX_MPH) {
+        mph = HEXAGRAM_SPEEDO_MAX_MPH;
     }
 
     hexagram_gauge_draw_needle(&speedo->gauge,
                                cr,
                                0.8,
-                               kph / HEXAGRAM_SPEEDO_MAX_KPH);
+                               mph / HEXAGRAM_SPEEDO_MAX_MPH);
 }