145 lines
3.6 KiB
C
145 lines
3.6 KiB
C
#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/schedule.h>
|
|
#include <hexagram/telemetry/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);
|
|
}
|
|
|
|
static int ev_handler(struct can_frame *frame, hexagram_can_if *can_if) {
|
|
return hexagram_can_if_write(can_if, frame);
|
|
}
|
|
|
|
static hexagram_schedule_slot table[4] = {
|
|
{ 16670, { .can_id = 0x280, .can_dlc = 8 }, ev_handler },
|
|
{ 16670, { .can_id = 0x288, .can_dlc = 8 }, ev_handler },
|
|
{ 16670, { .can_id = 0x320, .can_dlc = 8 }, ev_handler },
|
|
{ 16670, { .can_id = 0x5a0, .can_dlc = 8 }, ev_handler }
|
|
};
|
|
|
|
static void table_update(hexagram_schedule_slot *table,
|
|
hexagram_telemetry_dash_packet *packet) {
|
|
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);
|
|
|
|
/* Engine speed */
|
|
table[0].frame.data[3] = (engine_rpm & 0xff00) >> 8;
|
|
table[0].frame.data[2] = engine_rpm & 0x00ff;
|
|
|
|
/* Vehicle speed */
|
|
table[3].frame.data[1] = (speed_rps & 0xff00) >> 8;
|
|
table[3].frame.data[2] = speed_rps & 0x00ff;
|
|
}
|
|
|
|
int hexagram_main_dash2can(int argc, char **argv) {
|
|
int sock;
|
|
|
|
struct sockaddr_in sockaddr;
|
|
hexagram_can_if *can_if;
|
|
hexagram_schedule *schedule;
|
|
|
|
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;
|
|
}
|
|
|
|
/* Engine temperature */
|
|
table[1].frame.data[1] = (uint8_t)((87.0 / 0.75) + 48);
|
|
|
|
/* Fuel status */
|
|
table[2].frame.data[2] = 0x0e;
|
|
|
|
if ((schedule = hexagram_schedule_create(table, 4, can_if)) == NULL) {
|
|
goto error_schedule_create;
|
|
}
|
|
|
|
hexagram_schedule_run(schedule);
|
|
|
|
while (1) {
|
|
hexagram_telemetry_dash_packet packet;
|
|
ssize_t len;
|
|
|
|
if ((len = recv(sock, &packet, sizeof(packet), 0)) < 0) {
|
|
goto error_io;
|
|
} else if (len != (ssize_t)sizeof(packet)) {
|
|
continue;
|
|
}
|
|
|
|
table_update(table, &packet);
|
|
}
|
|
|
|
hexagram_schedule_destroy(schedule);
|
|
hexagram_can_if_close(can_if);
|
|
|
|
close(sock);
|
|
|
|
return 0;
|
|
|
|
error_io:
|
|
hexagram_schedule_destroy(schedule);
|
|
|
|
error_schedule_create:
|
|
hexagram_can_if_close(can_if);
|
|
|
|
error_can_if_open:
|
|
error_bind:
|
|
close(sock);
|
|
|
|
error_socket:
|
|
return 1;
|
|
}
|