From c130714bca63d3abf30e879947e0ff2f8e79296b Mon Sep 17 00:00:00 2001 From: XANTRONIX Development Date: Sun, 17 Dec 2023 15:50:25 -0500 Subject: [PATCH] Add support for Forza telemetry to PQ35 CAN bus --- bin/Makefile | 3 +- bin/dash2can.c | 143 ++++++++++++++++++++++++ bin/dash2can.h | 8 ++ bin/main.c | 2 + include/hexagram/speedo.h | 1 + include/hexagram/telemetry/forza-dash.h | 8 -- src/speedo.c | 13 ++- 7 files changed, 163 insertions(+), 15 deletions(-) create mode 100644 bin/dash2can.c create mode 100644 bin/dash2can.h diff --git a/bin/Makefile b/bin/Makefile index 32022bb..be6c1c7 100644 --- a/bin/Makefile +++ b/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 diff --git a/bin/dash2can.c b/bin/dash2can.c new file mode 100644 index 0000000..98b6c35 --- /dev/null +++ b/bin/dash2can.c @@ -0,0 +1,143 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#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; +} diff --git a/bin/dash2can.h b/bin/dash2can.h new file mode 100644 index 0000000..98dff98 --- /dev/null +++ b/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 */ diff --git a/bin/main.c b/bin/main.c index 84b5630..096040c 100644 --- a/bin/main.c +++ b/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 } }; diff --git a/include/hexagram/speedo.h b/include/hexagram/speedo.h index bb56e7d..5665840 100644 --- a/include/hexagram/speedo.h +++ b/include/hexagram/speedo.h @@ -6,6 +6,7 @@ #include #define HEXAGRAM_SPEEDO_MAX_KPH 289.682 +#define HEXAGRAM_SPEEDO_MAX_MPH 180.000 typedef struct _hexagram_speedo { hexagram_gauge gauge; diff --git a/include/hexagram/telemetry/forza-dash.h b/include/hexagram/telemetry/forza-dash.h index 5c60c6d..b15d12d 100644 --- a/include/hexagram/telemetry/forza-dash.h +++ b/include/hexagram/telemetry/forza-dash.h @@ -4,14 +4,6 @@ #include #include -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, diff --git a/src/speedo.c b/src/speedo.c index 8fceb95..91c74b2 100644 --- a/src/speedo.c +++ b/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); }