icsim.c

/*
 * Instrument cluster simulator
 *
 * (c) 2014 Open Garages - Craig Smith <craig@theialabs.com>
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <time.h>
#include <getopt.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <SDL2/SDL.h>
#include <SDL2/SDL_image.h>

#include "lib.h"

#ifndef DATA_DIR
#define DATA_DIR "./data/"  // Needs trailing slash
#endif

#define SCREEN_WIDTH 692
#define SCREEN_HEIGHT 329
#define DOOR_LOCKED 0
#define DOOR_UNLOCKED 1
#define OFF 0
#define ON 1
#define DEFAULT_DOOR_ID 411 // 0x19b
#define DEFAULT_DOOR_BYTE 2
#define CAN_DOOR1_LOCK 1
#define CAN_DOOR2_LOCK 2 
#define CAN_DOOR3_LOCK 4
#define CAN_DOOR4_LOCK 8
#define DEFAULT_SIGNAL_ID 392 // 0x188
#define DEFAULT_SIGNAL_BYTE 0
#define CAN_LEFT_SIGNAL 1
#define CAN_RIGHT_SIGNAL 2
#define DEFAULT_SPEED_ID 580 // 0x244
#define DEFAULT_SPEED_BYTE 3 // bytes 3,4

// For now, specific models will be done as constants.  Later
// We should use a config file
#define MODEL_BMW_X1_SPEED_ID 0x1B4
#define MODEL_BMW_X1_SPEED_BYTE 0
#define MODEL_BMW_X1_RPM_ID 0x0AA
#define MODEL_BMW_X1_RPM_BYTE 4
#define MODEL_BMW_X1_HANDBRAKE_ID 0x1B4  // Not implemented yet
#define MODEL_BMW_X1_HANDBRAKE_BYTE 5

const int canfd_on = 1;
int debug = 0;
int randomize = 0;
int seed = 0;
int door_pos = DEFAULT_DOOR_BYTE;
int signal_pos = DEFAULT_SIGNAL_BYTE;
int speed_pos = DEFAULT_SPEED_BYTE;
long current_speed = 0;
int door_status[4];
int turn_status[2];
char *model = NULL;
char data_file[256];
SDL_Renderer *renderer = NULL;
SDL_Texture *base_texture = NULL;
SDL_Texture *needle_tex = NULL;
SDL_Texture *sprite_tex = NULL;
SDL_Rect speed_rect;

// Simple map function
long map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// Adds data dir to file name
// Uses a single pointer so not to have a memory leak
// returns point to data_files or NULL if append is too large
char *get_data(char *fname) {
  if(strlen(DATA_DIR) + strlen(fname) > 255) return NULL;
  strncpy(data_file, DATA_DIR, 255);
  strncat(data_file, fname, 255-strlen(data_file));
  return data_file;
}

/* Default vehicle state */
void init_car_state() {
  door_status[0] = DOOR_LOCKED;
  door_status[1] = DOOR_LOCKED;
  door_status[2] = DOOR_LOCKED;
  door_status[3] = DOOR_LOCKED;
  turn_status[0] = OFF;
  turn_status[1] = OFF;
}

/* Empty IC */
void blank_ic() {
  SDL_RenderCopy(renderer, base_texture, NULL, NULL);
}

/* Updates speedo */
void update_speed() {
  SDL_Rect dial_rect;
  SDL_Point center;
  double angle = 0;
  dial_rect.x = 200;
  dial_rect.y = 80;
  dial_rect.h = 130;
  dial_rect.w = 300;
  SDL_RenderCopy(renderer, base_texture, &dial_rect, &dial_rect);
  /* Because it's a curve we do a smaller rect for the top */
  dial_rect.x = 250;
  dial_rect.y = 30;
  dial_rect.h = 65;
  dial_rect.w = 200;
  SDL_RenderCopy(renderer, base_texture, &dial_rect, &dial_rect);
  // And one more smaller box for the pivot point of the needle
  dial_rect.x = 323;
  dial_rect.y = 171;
  dial_rect.h = 52;
  dial_rect.w = 47;
  SDL_RenderCopy(renderer, base_texture, &dial_rect, &dial_rect);
  center.x = 135;
  center.y = 20;
  angle = map(current_speed, 0, 280, 0, 180);
  if(angle < 0) angle = 0;
  if(angle > 180) angle = 180;
  SDL_RenderCopyEx(renderer, needle_tex, NULL, &speed_rect, angle, &center, SDL_FLIP_NONE);
}

/* Updates door unlocks simulated by door open icons */
void update_doors() {
  SDL_Rect door_area, update, pos;
  door_area.x = 390;
  door_area.y = 215;
  door_area.w = 110;
  door_area.h = 85;
  SDL_RenderCopy(renderer, base_texture, &door_area, &door_area);
  // No update if all doors are locked
  if(door_status[0] == DOOR_LOCKED && door_status[1] == DOOR_LOCKED &&
     door_status[2] == DOOR_LOCKED && door_status[3] == DOOR_LOCKED) return;
  // Make the base body red if even one door is unlocked
  update.x = 440;
  update.y = 239;
  update.w = 45;
  update.h = 83;
  memcpy(&pos, &update, sizeof(SDL_Rect));
  pos.x -= 22;
  pos.y -= 22;
  SDL_RenderCopy(renderer, sprite_tex, &update, &pos);
  if(door_status[0] == DOOR_UNLOCKED) {
    update.x = 420;
    update.y = 263;
    update.w = 21;
    update.h = 22;
    memcpy(&pos, &update, sizeof(SDL_Rect));
    pos.x -= 22;
    pos.y -= 22;
    SDL_RenderCopy(renderer, sprite_tex, &update, &pos);
  }
  if(door_status[1] == DOOR_UNLOCKED) {
    update.x = 484;
    update.y = 261;
    update.w = 21;
    update.h = 22;
    memcpy(&pos, &update, sizeof(SDL_Rect));
    pos.x -= 22;
    pos.y -= 22;
    SDL_RenderCopy(renderer, sprite_tex, &update, &pos);
  }
  if(door_status[2] == DOOR_UNLOCKED) {
    update.x = 420;
    update.y = 284;
    update.w = 21;
    update.h = 22;
    memcpy(&pos, &update, sizeof(SDL_Rect));
    pos.x -= 22;
    pos.y -= 22;
    SDL_RenderCopy(renderer, sprite_tex, &update, &pos);
  }
  if(door_status[3] == DOOR_UNLOCKED) {
    update.x = 484;
    update.y = 287;
    update.w = 21;
    update.h = 22;
    memcpy(&pos, &update, sizeof(SDL_Rect));
    pos.x -= 22;
    pos.y -= 22;
    SDL_RenderCopy(renderer, sprite_tex, &update, &pos);
  }
}

/* Updates turn signals */
void update_turn_signals() {
  SDL_Rect left, right, lpos, rpos;
  left.x = 213;
  left.y = 51;
  left.w = 45;
  left.h = 45;
  memcpy(&right, &left, sizeof(SDL_Rect));
  right.x = 482;
  memcpy(&lpos, &left, sizeof(SDL_Rect));
  memcpy(&rpos, &right, sizeof(SDL_Rect));
  lpos.x -= 22;
  lpos.y -= 22;
  rpos.x -= 22;
  rpos.y -= 22;
  if(turn_status[0] == OFF) {
	SDL_RenderCopy(renderer, base_texture, &lpos, &lpos);
  } else {
	SDL_RenderCopy(renderer, sprite_tex, &left, &lpos);
  }
  if(turn_status[1] == OFF) {
	SDL_RenderCopy(renderer, base_texture, &rpos, &rpos);
  } else {
	SDL_RenderCopy(renderer, sprite_tex, &right, &rpos);
  }
}

/* Redraws the IC updating everything 
 * Slowest way to go.  Should only use on init
 */
void redraw_ic() {
  blank_ic();
  update_speed();
  update_doors();
  update_turn_signals();
  SDL_RenderPresent(renderer);
}

/* Parses CAN fram and updates current_speed */
void update_speed_status(struct canfd_frame *cf, int maxdlen) {
  int len = (cf->len > maxdlen) ? maxdlen : cf->len;
  if(len < speed_pos + 1) return;
  if (model) {
	if (!strncmp(model, "bmw", 3)) {
		current_speed = (((cf->data[speed_pos + 1] - 208) * 256) + cf->data[speed_pos]) / 16;
	}
  } else {
	  int speed = cf->data[speed_pos] << 8;
	  speed += cf->data[speed_pos + 1];
	  speed = speed / 100; // speed in kilometers
	  current_speed = speed * 0.6213751; // mph
  }
  update_speed();
  SDL_RenderPresent(renderer);
}

/* Parses CAN frame and updates turn signal status */
void update_signal_status(struct canfd_frame *cf, int maxdlen) {
  int len = (cf->len > maxdlen) ? maxdlen : cf->len;
  if(len < signal_pos) return;
  if(cf->data[signal_pos] & CAN_LEFT_SIGNAL) {
    turn_status[0] = ON;
  } else {
    turn_status[0] = OFF;
  }
  if(cf->data[signal_pos] & CAN_RIGHT_SIGNAL) {
    turn_status[1] = ON;
  } else {
    turn_status[1] = OFF;
  }
  update_turn_signals();
  SDL_RenderPresent(renderer);
}

/* Parses CAN frame and updates door status */
void update_door_status(struct canfd_frame *cf, int maxdlen) {
  int len = (cf->len > maxdlen) ? maxdlen : cf->len;
  if(len < door_pos) return;
  if(cf->data[door_pos] & CAN_DOOR1_LOCK) {
	door_status[0] = DOOR_LOCKED;
  } else {
	door_status[0] = DOOR_UNLOCKED;
  }
  if(cf->data[door_pos] & CAN_DOOR2_LOCK) {
	door_status[1] = DOOR_LOCKED;
  } else {
	door_status[1] = DOOR_UNLOCKED;
  }
  if(cf->data[door_pos] & CAN_DOOR3_LOCK) {
	door_status[2] = DOOR_LOCKED;
  } else {
	door_status[2] = DOOR_UNLOCKED;
  }
  if(cf->data[door_pos] & CAN_DOOR4_LOCK) {
	door_status[3] = DOOR_LOCKED;
  } else {
	door_status[3] = DOOR_UNLOCKED;
  }
  update_doors();
  SDL_RenderPresent(renderer);
}

void Usage(char *msg) {
  if(msg) printf("%s\n", msg);
  printf("Usage: icsim [options] <can>\n");
  printf("\t-r\trandomize IDs\n");
  printf("\t-s\tseed value\n");
  printf("\t-d\tdebug mode\n");
  printf("\t-m\tmodel NAME  (Ex: -m bmw)\n");
  exit(1);
}

int main(int argc, char *argv[]) {
  int opt;
  int can;
  struct ifreq ifr;
  struct sockaddr_can addr;
  struct canfd_frame frame;
  struct iovec iov;
  struct msghdr msg;
  struct cmsghdr *cmsg;
  struct stat dirstat;
  char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))];
  int running = 1;
  int nbytes, maxdlen;
  int seed = 0;
  canid_t door_id, signal_id, speed_id;
  SDL_Event event;

  while ((opt = getopt(argc, argv, "rs:dm:h?")) != -1) {
    switch(opt) {
	case 'r':
		randomize = 1;
		break;
	case 's':
		seed = atoi(optarg);
		break;
	case 'd':
		debug = 1;
		break;
	case 'm':
		model = optarg;
		break;
	case 'h':
	case '?':
	default:
		Usage(NULL);
		break;
    }
  }

  if (optind >= argc) Usage("You must specify at least one can device");

  if (seed && randomize) Usage("You can not specify a seed value AND randomize the seed");

  // Verify data directory exists
  if(stat(DATA_DIR, &dirstat) == -1) {
  	printf("ERROR: DATA_DIR not found.  Define in make file or run in src dir\n");
	exit(34);
  }
  
  // Create a new raw CAN socket
  can = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  if(can < 0) Usage("Couldn't create raw socket");

  addr.can_family = AF_CAN;
  memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
  strncpy(ifr.ifr_name, argv[optind], strlen(argv[optind]));
  printf("Using CAN interface %s\n", ifr.ifr_name);
  if (ioctl(can, SIOCGIFINDEX, &ifr) < 0) {
    perror("SIOCGIFINDEX");
    exit(1);
  }
  addr.can_ifindex = ifr.ifr_ifindex;
  // CAN FD Mode
  setsockopt(can, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on));

  iov.iov_base = &frame;
  iov.iov_len = sizeof(frame);
  msg.msg_name = &addr;
  msg.msg_namelen = sizeof(addr);
  msg.msg_iov = &iov;
  msg.msg_iovlen = 1;
  msg.msg_control = &ctrlmsg;
  msg.msg_controllen = sizeof(ctrlmsg);
  msg.msg_flags = 0;

  if (bind(can, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
	perror("bind");
	return 1;
  }

  init_car_state();

  door_id = DEFAULT_DOOR_ID;
  signal_id = DEFAULT_SIGNAL_ID;
  speed_id = DEFAULT_SPEED_ID;

  if (randomize || seed) {
	if(randomize) seed = time(NULL);
	srand(seed);
	door_id = (rand() % 2046) + 1;
	signal_id = (rand() % 2046) + 1;
	speed_id = (rand() % 2046) + 1;
	door_pos = rand() % 9;
	signal_pos = rand() % 9;
	speed_pos = rand() % 8;
	printf("Seed: %d\n", seed);
	FILE *fdseed = fopen("/tmp/icsim_seed.txt", "w");
	fprintf(fdseed, "%d\n", seed);
	fclose(fdseed);
  } else if (model) {
	if (!strncmp(model, "bmw", 3)) {
		speed_id = MODEL_BMW_X1_SPEED_ID;
		speed_pos = MODEL_BMW_X1_SPEED_BYTE;
	} else {
		printf("Unknown model.  Acceptable models: bmw\n");
		exit(3);
	}
  }

  SDL_Window *window = NULL;
  if(SDL_Init ( SDL_INIT_VIDEO ) < 0 ) {
	printf("SDL Could not initializes\n");
	exit(40);
  }
  window = SDL_CreateWindow("IC Simulator", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, SCREEN_WIDTH, SCREEN_HEIGHT,
                            SDL_WINDOW_SHOWN); // | SDL_WINDOW_RESIZABLE);
  if(window == NULL) {
	printf("Window could not be shown\n");
  }
  renderer = SDL_CreateRenderer(window, -1, 0);
  SDL_Surface *image = IMG_Load(get_data("ic.png"));
  SDL_Surface *needle = IMG_Load(get_data("needle.png"));
  SDL_Surface *sprites = IMG_Load(get_data("spritesheet.png"));
  base_texture = SDL_CreateTextureFromSurface(renderer, image);
  needle_tex = SDL_CreateTextureFromSurface(renderer, needle);
  sprite_tex = SDL_CreateTextureFromSurface(renderer, sprites);

  speed_rect.x = 212;
  speed_rect.y = 175;
  speed_rect.h = needle->h;
  speed_rect.w = needle->w;

  // Draw the IC
  redraw_ic();

  /* For now we will just operate on one CAN interface */
  while(running) {
    while( SDL_PollEvent(&event) != 0 ) {
	switch(event.type) {
	    case SDL_QUIT:
		running = 0;
		break;
	    case SDL_WINDOWEVENT:
	    switch(event.window.event) {
		case SDL_WINDOWEVENT_ENTER:
		case SDL_WINDOWEVENT_RESIZED:
			redraw_ic();
		break;
	    }
   	}
      SDL_Delay(3);
    }

      nbytes = recvmsg(can, &msg, 0);
      if (nbytes < 0) {
        perror("read");
        return 1;
      }  
      if ((size_t)nbytes == CAN_MTU)
        maxdlen = CAN_MAX_DLEN;
      else if ((size_t)nbytes == CANFD_MTU)
        maxdlen = CANFD_MAX_DLEN;
      else {
        fprintf(stderr, "read: incomplete CAN frame\n");
        return 1;
      }
      for (cmsg = CMSG_FIRSTHDR(&msg);
           cmsg && (cmsg->cmsg_level == SOL_SOCKET);
           cmsg = CMSG_NXTHDR(&msg,cmsg)) {
             if (cmsg->cmsg_type == SO_TIMESTAMP) {
               // struct timeval tv = *(struct timeval *)CMSG_DATA(cmsg);
             }
             else if (cmsg->cmsg_type == SO_RXQ_OVFL)
               //dropcnt[i] = *(__u32 *)CMSG_DATA(cmsg);
  	     fprintf(stderr, "Dropped packet\n");
             }
//      if(debug) fprint_canframe(stdout, &frame, "\n", 0, maxdlen);
      if(frame.can_id == door_id) update_door_status(&frame, maxdlen);
      if(frame.can_id == signal_id) update_signal_status(&frame, maxdlen);
      if(frame.can_id == speed_id) update_speed_status(&frame, maxdlen);
  }

  SDL_DestroyTexture(base_texture);
  SDL_DestroyTexture(needle_tex);
  SDL_DestroyTexture(sprite_tex);
  SDL_FreeSurface(image);
  SDL_FreeSurface(needle);
  SDL_FreeSurface(sprites);
  SDL_DestroyRenderer(renderer);
  SDL_DestroyWindow(window);
  IMG_Quit();
  SDL_Quit();

  return 0;
}