int main() { CyGlobalIntEnable; UART_Start(); printf("Start\r\n"); /* //IR receiver// ---------------------------------------------------- unsigned int IR_val; for(;;) { IR_val = get_IR(); printf("%x\r\n\n",IR_val); } ///---------------------------------------------------------- */ /* //Ambient// ---------------------------------------------------- I2C_Start(); uint16 value =0; I2C_write(0x29,0x80,0x00); value = I2C_read(0x29,0x80); printf("%x ",value); I2C_write(0x29,0x80,0x03); value = I2C_read(0x29,0x80); printf("%x\r\n",value); value = I2C_read(0x29,0x81); printf("%x\r\n",value); for(;;) { uint8 Data0Low,Data0High,Data1Low,Data1High; Data0Low = I2C_read(0x29,CH0_L); Data0High = I2C_read(0x29,CH0_H); Data1Low = I2C_read(0x29,CH1_L); Data1High = I2C_read(0x29,CH1_H); uint8 CH0, CH1; CH0 = convert_raw(Data0Low,Data0High); CH1 = convert_raw(Data1Low,Data1High); // printf("%d %d %d %d\r\n",Data0Low,Data0High, Data1Low,Data1High); // printf("%d %d\r\n",CH0,CH1); // printf("%f\r\n",(float)CH1/CH0); double Ch0 = CH0; double Ch1 = CH1; double data = 0; data = getLux(Ch0,Ch1); printf("%lf\r\n",data); } ///---------------------------------------------------------- */ /* //nunchuk// ---------------------------------------------------- nunchuk_start(); nunchuk_init(); for(;;) { nunchuk_read(); } //----------------------------------------------------*/ //accelerometer// //-------------------------------------------------------------- I2C_Start(); uint8 X_L_A, X_H_A, Y_L_A, Y_H_A, Z_L_A, Z_H_A; int16 X_AXIS, Y_AXIS, Z_AXIS; I2C_write(ACCEL_MAG_ADDR, ACCEL_CTRL1_REG, 0x37); // set accelerometer & magnetometer into active mode I2C_write(ACCEL_MAG_ADDR, ACCEL_CTRL7_REG, 0x22); for(;;) { //print out accelerometer output X_L_A = I2C_read(ACCEL_MAG_ADDR, OUT_X_L_A); X_H_A = I2C_read(ACCEL_MAG_ADDR, OUT_X_H_A); X_AXIS = convert_raw(X_L_A, X_H_A); Y_L_A = I2C_read(ACCEL_MAG_ADDR, OUT_Y_L_A); Y_H_A = I2C_read(ACCEL_MAG_ADDR, OUT_Y_H_A); Y_AXIS = convert_raw(Y_L_A, Y_H_A); Z_L_A = I2C_read(ACCEL_MAG_ADDR, OUT_Z_L_A); Z_H_A = I2C_read(ACCEL_MAG_ADDR, OUT_Z_H_A); Z_AXIS = convert_raw(Z_L_A, Z_H_A); //printf("ACCEL: %d %d %d %d %d %d \r\n", X_L_A, X_H_A, Y_L_A, Y_H_A, Z_L_A, Z_H_A); value_convert_accel(X_AXIS, Y_AXIS, Z_AXIS); printf("\n"); CyDelay(50); } ///---------------------------------------------------------- /* //ultra// ---------------------------------------------------- ultra_isr_StartEx(ultra_isr_handler); // Ultra Sonic Interrupt Ultra_Start(); // Ultra Sonic Start function for(;;) { CyDelay(100); Trig_Write(1); // Trigger High CyDelayUs(10); // 10 micro seconds for trigger input signals Trig_Write(0); // Trigger Low } //----------------------------------------------------*/ /* //reflectance// ---------------------------------------------------- sensor_isr_StartEx(sensor_isr_handler); Refelctance_Start(); IR_led_Write(1); for(;;) { reflectance_period(); //print out each period of reflectance sensors reflectance_digital(); //print out 0 or 1 according to results of reflectance period CyDelay(500); } ///----------------------------------------------------*/ /* //motor// ---------------------------------------------------- motor_Start(); // motor start motor_forward(50,2000); // moving forward motor_turn(10,50,2000); // turn motor_turn(50,10,2000); // turn motor_backward(50,2000); // movinb backward motor_Stop(); // motor stop for(;;) { } ///----------------------------------------------------*/ /* //gyroscope// //----------------------------------------------------- I2C_Start(); uint8 X_AXIS_L, X_AXIS_H, Y_AXIS_L, Y_AXIS_H, Z_AXIS_L, Z_AXIS_H; int16 X_AXIS, Y_AXIS, Z_AXIS; I2C_write(GYRO_ADDR, GYRO_CTRL1_REG, 0x0F); // set gyroscope into active mode I2C_write(GYRO_ADDR, GYRO_CTRL4_REG, 0x30); // set full scale selection to 2000dps for(;;) { //print out gyroscope output X_AXIS_L = I2C_read(GYRO_ADDR, OUT_X_AXIS_L); X_AXIS_H = I2C_read(GYRO_ADDR, OUT_X_AXIS_H); X_AXIS = convert_raw(X_AXIS_H, X_AXIS_L); Y_AXIS_L = I2C_read(GYRO_ADDR, OUT_Y_AXIS_L); Y_AXIS_H = I2C_read(GYRO_ADDR, OUT_Y_AXIS_H); Y_AXIS = convert_raw(Y_AXIS_H, Y_AXIS_L); Z_AXIS_L = I2C_read(GYRO_ADDR, OUT_Z_AXIS_L); Z_AXIS_H = I2C_read(GYRO_ADDR, OUT_Z_AXIS_H); Z_AXIS = convert_raw(Z_AXIS_H, Z_AXIS_L); //printf("X_AXIS_L: %d, X_AXIS_H: %d, average: %d \r\n", X_AXIS_L, X_AXIS_H, (X_AXIS_H+X_AXIS_L)/2); //printf("Y_AXIS_L: %d, Y_AXIS_H: %d, average: %d \r\n", Y_AXIS_L, Y_AXIS_H, (Y_AXIS_H+Y_AXIS_L)/2); //printf("Z_AXIS_L: %d, Z_AXIS_H: %d, average: %d \r\n", Z_AXIS_L, Z_AXIS_H, (Z_AXIS_H+Z_AXIS_L)/2); //printf("H L : %d %d %d %d %d %d \r\n", X_AXIS_L, X_AXIS_H, Y_AXIS_L, Y_AXIS_H, Z_AXIS_L, Z_AXIS_H); //printf("%d %d %d \r\n", X_AXIS, Y_AXIS, Z_AXIS); printf("%d %d %d \r\n", value_convert_gyro(X_AXIS), value_convert_gyro(Y_AXIS), value_convert_gyro(Z_AXIS)); CyDelay(50); } ///-----------------------------------------------------------------*/ /* //magnetometer// //-------------------------------------------------------------- I2C_Start(); uint8 X_L_M, X_H_M, Y_L_M, Y_H_M, Z_L_M, Z_H_M; int16 X_AXIS, Y_AXIS, Z_AXIS; I2C_write(ACCEL_MAG_ADDR, ACCEL_CTRL1_REG, 0x37); // set accelerometer & magnetometer into active mode I2C_write(ACCEL_MAG_ADDR, ACCEL_CTRL5_REG, 0x10); // set a data rate of 50Hz I2C_write(ACCEL_MAG_ADDR, ACCEL_CTRL6_REG, 0x60); // set the full scale selection to +/- 12 Gauss I2C_write(ACCEL_MAG_ADDR, ACCEL_CTRL7_REG, 0x80); // set to continuous-conversion mode for(;;) { X_L_M = I2C_read(ACCEL_MAG_ADDR, OUT_X_L_M); X_H_M = I2C_read(ACCEL_MAG_ADDR, OUT_X_H_M); X_AXIS = convert_raw(X_L_M, X_H_M); Y_L_M = I2C_read(ACCEL_MAG_ADDR, OUT_Y_L_M); Y_H_M = I2C_read(ACCEL_MAG_ADDR, OUT_Y_H_M); Y_AXIS = convert_raw(Y_L_M, Y_H_M); Z_L_M = I2C_read(ACCEL_MAG_ADDR, OUT_Z_L_M); Z_H_M = I2C_read(ACCEL_MAG_ADDR, OUT_Z_H_M); Z_AXIS = convert_raw(Z_L_M, Z_H_M); heading(X_AXIS, Y_AXIS); // printf("MAGNET: %d %d %d %d %d %d \r\n", X_L_M, X_H_M, Y_L_M, Y_H_M, Z_L_M, Z_H_M); //printf("%d %d %d \r\n", X_AXIS,Y_AXIS, Z_AXIS); CyDelay(50); } ///----------------------------------------------------------*/ }
int main(int argc, char **argv) { pid_t pid; extern int optind, opterr, optopt; int opt; struct can_frame frame; int status, datalen, sd, *ip_flags; //const int on = 1; char *interface, *target, *src_ip, *dst_ip; struct ip iphdr; struct udphdr udphdr; unsigned char *data, *packet; struct addrinfo hints, *res; struct sockaddr_in *ipv4, sin; struct ifreq ifr; void *tmp; int sa, sc; // UDP socket , CAN socket struct sockaddr_in saddr; struct sockaddr_can caddr; struct ifreq CAN_ifr; socklen_t caddrlen = sizeof(caddr); fd_set readfds; int i, s, nbytes, ret; int local_port = 1470; int destination_port = 1470; // // int broadcast_address; int verbose = 1; int background = 0; //1; int canid = 0; const int on = 1; strcpy(CAN_ifr.ifr_name, "can0"); char bufferHexa[40]; int udp_bytes, raw_len, CAN_err, DCcom; char calc_FCS[2], UDP_CMD[2], PCID[2], Nbytes[2], FCS[2], RX_UDP_Frame[33], Data[25]; char SID[5], EID[5]; char bin_val[32], bin_str[32], DomoCANframe[32]; char temp_msg[64], fcs[2], msg_out[40], tmp_string[40], frm_content[80], buf[10]; char *hex_byte, hex_val[32], HexBuffer[32]; // Check modified Process arguments while ((opt = getopt(argc, argv, "l:d:b:i:e:fv?")) != -1) { switch (opt) { case 'l': local_port = strtoul(optarg, (char **)NULL, 10); break; case 'd': destination_port = strtoul(optarg, (char **)NULL, 10); break; case 'b': s = inet_pton(AF_INET,optarg,buf); if (s <= 0) { if (s == 0) { fprintf(stderr, "Not in presentation format"); } else { perror("inet_pton"); } exit(1); } break; case 'i': strcpy(CAN_ifr.ifr_name, optarg); break; case 'e': strcpy(interface, optarg); break; case 'v': verbose = 1; break; case 'f': background = 0; break; case '?': print_usage(basename(argv[0])); exit(0); break; default: fprintf(stderr, "Unknown option %c\n", opt); print_usage(basename(argv[0])); exit(1); break; } // END SWITCH } // END WHILE // Allocate memory for various arrays. // Maximum UDP payload size = 65535 - IPv4 header (20 bytes) - UDP header (8 bytes) tmp = (unsigned char *) malloc ((IP_MAXPACKET - IP4_HDRLEN - UDP_HDRLEN) * sizeof (unsigned char)); if (tmp != NULL) { data = tmp; } else { fprintf (stderr, "ERROR: Cannot allocate memory for array 'data'.\n"); exit (EXIT_FAILURE); } memset (data, 0, (IP_MAXPACKET - IP4_HDRLEN - UDP_HDRLEN) * sizeof (unsigned char)); tmp = (unsigned char *) malloc (IP_MAXPACKET * sizeof (unsigned char)); if (tmp != NULL) { packet = tmp; } else { fprintf (stderr, "ERROR: Cannot allocate memory for array 'packet'.\n"); exit (EXIT_FAILURE); } memset (packet, 0, IP_MAXPACKET * sizeof (unsigned char)); tmp = (char *) malloc (40 * sizeof (char)); if (tmp != NULL) { interface = tmp; } else { fprintf (stderr, "ERROR: Cannot allocate memory for array 'interface'.\n"); exit (EXIT_FAILURE); } memset (interface, 0, 40 * sizeof (char)); tmp = (char *) malloc (40 * sizeof (char)); if (tmp != NULL) { target = tmp; } else { fprintf (stderr, "ERROR: Cannot allocate memory for array 'target'.\n"); exit (EXIT_FAILURE); } memset (target, 0, 40 * sizeof (char)); tmp = (char *) malloc (16 * sizeof (char)); if (tmp != NULL) { src_ip = tmp; } else { fprintf (stderr, "ERROR: Cannot allocate memory for array 'src_ip'.\n"); exit (EXIT_FAILURE); } memset (src_ip, 0, 16 * sizeof (char)); tmp = (char *) malloc (16 * sizeof (char)); if (tmp != NULL) { dst_ip = tmp; } else { fprintf (stderr, "ERROR: Cannot allocate memory for array 'dst_ip'.\n"); exit (EXIT_FAILURE); } memset (dst_ip, 0, 16 * sizeof (char)); tmp = (int *) malloc (4 * sizeof (int)); if (tmp != NULL) { ip_flags = tmp; } else { fprintf (stderr, "ERROR: Cannot allocate memory for array 'ip_flags'.\n"); exit (EXIT_FAILURE); } memset (ip_flags, 0, 4 * sizeof (int)); // Interface to send packet through. // strcpy (interface, "eth0"); strcpy (interface, Domocan_UDP_Int); get_IP_conf(interface, source_ip, destination_ip); if (verbose) printf("\nInt: %s, IP Source: %s, Dest: %s\n", interface, source_ip, destination_ip); // Submit request for a socket descriptor to lookup interface. if ((sd = socket (AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0) { perror ("socket() failed to get socket descriptor for using ioctl() "); exit (EXIT_FAILURE); } // Use ioctl() to lookup interface. memset (&ifr, 0, sizeof (ifr)); snprintf (ifr.ifr_name, sizeof (ifr.ifr_name), "%s", interface); if (ioctl (sd, SIOCGIFINDEX, &ifr) < 0) { perror ("ioctl() failed to find interface "); return (EXIT_FAILURE); } close (sd); if (verbose) printf ("Index for interface %s is %i\n", interface, ifr.ifr_ifindex); // Source IPv4 address: you need to fill this out strcpy (src_ip, source_ip); // Destination URL or IPv4 address strcpy (target, destination_ip); // Fill out hints for getaddrinfo(). memset (&hints, 0, sizeof (struct addrinfo)); hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; hints.ai_flags = hints.ai_flags | AI_CANONNAME; // Resolve target using getaddrinfo(). if ((status = getaddrinfo (target, NULL, &hints, &res)) != 0) { fprintf (stderr, "getaddrinfo() failed: %s\n", gai_strerror (status)); exit (EXIT_FAILURE); } ipv4 = (struct sockaddr_in *) res->ai_addr; tmp = &(ipv4->sin_addr); inet_ntop (AF_INET, tmp, dst_ip, 16); freeaddrinfo (res); // UDP data datalen = 16; // IPv4 header // IPv4 header length (4 bits): Number of 32-bit words in header = 5 iphdr.ip_hl = IP4_HDRLEN / sizeof (unsigned long int); // Internet Protocol version (4 bits): IPv4 iphdr.ip_v = 4; // Type of service (8 bits) iphdr.ip_tos = 0; // Total length of datagram (16 bits): IP header + UDP header + datalen iphdr.ip_len = htons (IP4_HDRLEN + UDP_HDRLEN + datalen); // ID sequence number (16 bits): unused, since single datagram iphdr.ip_id = htons (0); // Flags, and Fragmentation offset (3, 13 bits): 0 since single datagram // Zero (1 bit) ip_flags[0] = 0; // Do not fragment flag (1 bit) ip_flags[1] = 0; // More fragments following flag (1 bit) ip_flags[2] = 0; // Fragmentation offset (13 bits) ip_flags[3] = 0; iphdr.ip_off = htons ((ip_flags[0] << 15) + (ip_flags[1] << 14) + (ip_flags[2] << 13) + ip_flags[3]); // Time-to-Live (8 bits): default to maximum value iphdr.ip_ttl = 255; // Transport layer protocol (8 bits): 17 for UDP iphdr.ip_p = IPPROTO_UDP; // Source IPv4 address (32 bits) inet_pton (AF_INET, src_ip, &(iphdr.ip_src)); // Destination IPv4 address (32 bits) inet_pton (AF_INET, dst_ip, &iphdr.ip_dst); // IPv4 header checksum (16 bits): set to 0 when calculating checksum iphdr.ip_sum = 0; iphdr.ip_sum = checksum ((unsigned short int *) &iphdr, IP4_HDRLEN); // UDP header // Source port number (16 bits): pick a number udphdr.source = htons (1470); // Destination port number (16 bits): pick a number udphdr.dest = htons (1470); // Length of UDP datagram (16 bits): UDP header + UDP data udphdr.len = htons (UDP_HDRLEN + datalen); // UDP checksum (16 bits) udphdr.check = udp4_checksum (iphdr, udphdr, data, datalen); // The kernel is going to prepare layer 2 information (ethernet frame header) for us. // For that, we need to specify a destination for the kernel in order for it // to decide where to send the raw datagram. We fill in a struct in_addr with // the desired destination IP address, and pass this structure to the sendto() function. memset (&sin, 0, sizeof (struct sockaddr_in)); sin.sin_family = AF_INET; sin.sin_addr.s_addr = iphdr.ip_dst.s_addr; // Submit request for a raw socket descriptor. if ((sd = socket (AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0) { perror ("socket() failed "); exit (EXIT_FAILURE); } // END IF // Set flag so socket expects us to provide IPv4 header. if (setsockopt (sd, IPPROTO_IP, IP_HDRINCL, &on, sizeof (on)) < 0) { perror ("setsockopt() failed to set IP_HDRINCL "); exit (EXIT_FAILURE); } // END IF // Bind socket to interface index. if (setsockopt (sd, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof (ifr)) < 0) { perror ("setsockopt() failed to bind to interface "); exit (EXIT_FAILURE); } // END IF // Enable Broadcast if (setsockopt (sd, SOL_SOCKET, SO_BROADCAST,&ifr,sizeof(ifr)) < 0) { perror ("setsockopt() failed to enable Broadcast "); exit (EXIT_FAILURE); } // END IF // if((sa = socket(PF_INET, SOCK_DGRAM, 0)) < 0) { if((sa = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { perror("inetsocket"); exit(1); } // END IF // SOCKADDR_IN saddr = { 0 }; saddr.sin_family = AF_INET; saddr.sin_addr.s_addr = htonl(INADDR_ANY); saddr.sin_port = htons(local_port); while(bind(sa,(struct sockaddr*)&saddr, sizeof(saddr)) < 0) { printf("."); fflush(NULL); usleep(100000); } // END WHILE // CAN Interface Init if ((sc = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { perror("socket"); exit(1); } // END IF caddr.can_family = AF_CAN; caddr.can_ifindex = 0; // bind to all interfaces if (ioctl(sc, SIOCGIFINDEX, &CAN_ifr) < 0) { perror("SIOCGIFINDEX"); exit(1); } // END IF caddr.can_ifindex = CAN_ifr.ifr_ifindex; if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) { perror("CAN bind"); exit(1); } // END IF if (background) { // Fork off the parent process pid = fork(); if (pid < 0) { exit(EXIT_FAILURE); } // END IF // If we got a good PID, then we can exit the parent process. if (pid > 0) { printf("Going into background ...\n"); exit(EXIT_SUCCESS); } // END IF pid } // END IF background while (1) { // I/O Interup Disptach FD_ZERO(&readfds); // zero out the read set FD_SET(sc, &readfds); // add CAN socket to the read set FD_SET(sa, &readfds); // add UDP Serveur socket to the read set ret = select((sc > sa)?sc+1:sa+1, &readfds, NULL, NULL, NULL); ////////////////////////// // received a CAN frame // ////////////////////////// if (FD_ISSET(sc, &readfds)) { if ((nbytes = read(sc, &frame, sizeof(struct can_frame))) != 0) { // CAN frame IN sprintf(frm_content, "", ""); for (i = 0; i < frame.can_dlc; i++) { sprintf(buf, "%02X",frame.data[i]); strcat(frm_content, buf); } // END FOR // printf(" (%s)\n", frm_content); if (verbose) printf("\n\n>RX CAN: header=%8X data=%s dlc=%d", frame.can_id, frm_content, frame.can_dlc); // Transform to DomoCAN UDP Format hex_val[0] = '\0'; sprintf(hex_val, "%8X", frame.can_id); strcpy(hex_val, hex_val); // Extract SDIH and SIDL hex_byte = strndup(hex_val, 4); // Convert to 16 bit binary htoi(hex_byte, bin_val, bin_str); bin_val[0] = '\0'; // Transform 16bit CAN to Domocan UDP DomoCANaddr(bin_str, bin_val); //printf("\n DomoCAN binary SDIH+SIDL:%s\n", bin_val); // Convert back form 16bit to 2 HEX bytes SID[0] = '\0'; bin_str[0] = '\0'; bintohex(bin_val,bin_str); sprintf(SID,"%s", bin_str); SID[4]='\0'; //printf("\n DomoCAN HEX Destination:%s", SID); // Determine Length (dlc+4) nbytes = frame.can_dlc+4; // Frame length (CAN Header + dlc) sprintf(buf, "%0d", nbytes); buf[2]='\0'; if (nbytes==0) { sprintf(buf, "%s", "00"); buf[2]='\0'; } else { if (nbytes<10) { sprintf(buf, "0%d", nbytes); buf[2]='\0'; } if (nbytes==10) { sprintf(buf, "%s", "0A"); buf[2]='\0'; } if (nbytes==11) { sprintf(buf, "%s", "0B"); buf[2]='\0'; } if (nbytes==12) { sprintf(buf, "%s", "0C"); buf[2]='\0'; } if (nbytes >12) { sprintf(buf, "%s", "0C"); buf[2]='\0'; } } // END IF nbytes // Extract EIDH and EIDL EID[0] = '\0'; strncpy(EID, hex_val+4, 4); EID[4]='\0'; // Build UDP Frame DomoCANframe[0] = '\0'; sprintf(DomoCANframe, "%s%s%s%s%s", "70FF", buf, SID, EID, frm_content); // Dummy bytes if (nbytes<12) { for (i=6+(nbytes*2); i<=29;i++) { DomoCANframe[i]='0'; } // END FOR } // END IF //DomoCANframe[30] = '\0'; // Calculate Checksum (FCS) domocan_checksum(DomoCANframe, fcs); DomoCANframe[30] = fcs[0]; DomoCANframe[31] = fcs[1]; DomoCANframe[32] = '\0'; // Serialize Message msg_out[0] = '\0'; raw_len = convert_raw(DomoCANframe, msg_out); if (verbose) printf("\n=>TX UDP Frame 0x70: %s, len=%d",DomoCANframe,raw_len); // Resets Interupts FD_ZERO(&readfds); // zero out the read set FD_SET(sc, &readfds); // add CAN socket to the read set FD_SET(sa, &readfds); // add UDP Serveur socket to the read set // Prepare packet. // First part is an IPv4 header. memcpy (packet, &iphdr, IP4_HDRLEN); // UDP checksum (16 bits) udphdr.check = udp4_checksum (iphdr, udphdr, msg_out, datalen); // Next part of packet is upper layer protocol header. memcpy ((packet + IP4_HDRLEN), &udphdr, UDP_HDRLEN); // Finally, add the UDP data. memcpy (packet + IP4_HDRLEN + UDP_HDRLEN, msg_out, datalen); // Send packet. if (sendto (sd, packet, IP4_HDRLEN + UDP_HDRLEN + datalen, 0, (struct sockaddr *) &sin, sizeof (struct sockaddr)) < 0) { perror ("sendto() failed "); exit (EXIT_FAILURE); } // END IF sendto } // END IF nbytes } // END IF received a CAN frame /////////////////////////// // Received a UDP packet // /////////////////////////// if (FD_ISSET(sa, &readfds)) { read(sa, udpframe, 32); // if (read(sa, udpframe, MAXDG)!=0) { // UDP Packet Received //strncpy(tmp_string, bufferHexa, sizeof(bufferHexa)); bufferHexa[0] = '\0'; RX_UDP_Frame[0] ='\0'; for (i=0;i<16;i++) { sprintf(bufferHexa, "%s%0.2X", bufferHexa, udpframe[i]); } // Extract Command, PCID, ... strncpy(UDP_CMD , bufferHexa , 2); UDP_CMD[2]='\0'; strcpy(RX_UDP_Frame, UDP_CMD); strncpy(PCID , bufferHexa+2 , 2); PCID[2]='\0'; strcat(RX_UDP_Frame, PCID); strncpy(Nbytes , bufferHexa+4 , 2); Nbytes[2]='\0'; xtoi(Nbytes, &udp_bytes); strcat(RX_UDP_Frame, Nbytes); udp_bytes = udp_bytes - 4; strncpy(SID , bufferHexa+6 , 4); SID[4]='\0'; strcat(RX_UDP_Frame, SID); strncpy(EID , bufferHexa+10, 4); EID[4]='\0'; strcat(RX_UDP_Frame, EID); strncpy(Data , bufferHexa+14, 16); Data[16]='\0'; strcat(RX_UDP_Frame, Data); RX_UDP_Frame[30]='\0'; strncpy(FCS , bufferHexa+30, 2); FCS[2]='\0'; if ((verbose) && (strcmp(UDP_CMD, "70")) && (strcmp(UDP_CMD, "50"))) { printf("\n\n>RX UDP: %s", bufferHexa); } // FCS OK? domocan_checksum(RX_UDP_Frame, calc_FCS); calc_FCS[2]='\0'; //printf("\nRX UDP: %s(UDP_CMD)-%s(PCID)-%d(DLC)-%s/%s(Header)-%s(Data)+%s(FCS)+CalcFCS=%s\nFull Frame= %s\n", UDP_CMD, PCID , udp_bytes, SID, EID, Data, FCS, calc_FCS, RX_UDP_Frame); if (!strcmp(FCS, calc_FCS)) { // Frame OK (FCS) // printf("...fcs OK...UDP_CMD=%s...", UDP_CMD); if (!strcmp(UDP_CMD, "60")) { // RX UDP_CMD=60 [Send CAN on Bus] if (verbose) printf("\nUDP_CMD=60 [Send CAN on BUS, "); // Convert SIDH and SIDL to binary (16 bits) DomoCANframe[0]='\0'; bin_str[0]='\0'; htoi(SID, bin_val, bin_str); // Convert SIDH and SIDL from UDP to CAN format bin_val[0]='\0'; UDPaddr_to_CAN(bin_str, bin_val); // Convert back SIDH and SIDL to HEX bintohex(bin_val, FCS); FCS[4]='\0'; DomoCANframe[0]='\0'; sprintf(DomoCANframe, "%s%s%s", "0x", FCS, EID); DomoCANframe[10]='\0'; if (verbose) printf("Destination=%s]", SID); frame.can_id = strtoul(DomoCANframe, NULL, 0); //*msg_out; frame.can_dlc = udp_bytes; Data[udp_bytes*2] = '\0'; msg_out[0] = '\0'; raw_len = convert_raw(Data, msg_out); // Convert to RAW msg to send; // Prepare CAN frame Data content for (i=0; i<=udp_bytes;i++) { frame.data[i] = msg_out[i]; } FCS[0]='\0'; if ((nbytes = write(sc, &frame, sizeof(frame))) != sizeof(frame)) { // Send NACK back to PC sprintf(FCS, "%s", "01"); FCS[2]='\0'; //perror("CAN write __"); } else { // Send ACK back to PC sprintf(FCS, "%s", "00"); FCS[2]='\0'; } // END IF // CAN_err = sendto(s, &frame, sizeof(struct can_frame), 0, (struct sockaddr*)&caddr, sizeof(caddr)); if (verbose) printf("\n=>TX CAN: Header=%s, Data=%s, DLC=%d, frame len= %d", DomoCANframe, Data, udp_bytes, sizeof(frame)); // Send UDP Ack back to PC with error code 0=OK, 1=NOK // Frame = 50 PCID Length=1 ACK/NACK DummyBytes FCS temp_msg[0]='\0'; sprintf(temp_msg, "%s%s%s", "50", PCID, "01", FCS); // Dummy bytes for (i=6; i<=29;i++) { temp_msg[i]='0'; } // END FOR temp_msg[30] = '\0'; // Calculating checksum (FCS) domocan_checksum(temp_msg, fcs); strcat(temp_msg, fcs); temp_msg[32]='\0'; raw_len = convert_raw(temp_msg, msg_out); printf("\n=>TX UDP CAN Send OK=Frame 0x50): %s",temp_msg); //s=sendto(sb, msg_out, 16, 0, (struct sockaddr *)&baddr, sizeof(baddr)); //FD_ZERO(&readfds); // zero out the read set // Prepare packet. // First part is an IPv4 header. memcpy (packet, &iphdr, IP4_HDRLEN); // UDP checksum (16 bits) udphdr.check = udp4_checksum (iphdr, udphdr, msg_out, datalen); // Next part of packet is upper layer protocol header. memcpy ((packet + IP4_HDRLEN), &udphdr, UDP_HDRLEN); // Finally, add the UDP data. memcpy (packet + IP4_HDRLEN + UDP_HDRLEN, msg_out, datalen); // Send packet. if (sendto (sd, packet, IP4_HDRLEN + UDP_HDRLEN + datalen, 0, (struct sockaddr *) &sin, sizeof (struct sockaddr)) < 0) { perror ("sendto() failed "); exit (EXIT_FAILURE); } //END IF sendto } else { // END IF UDP_CMD=60 if (!strcmp(UDP_CMD, "41")) { // RX UDP_CMD=41 [Stop Transmission] printf("\nRX UDP_CMD=41 [Stop Transmission]"); } else { // END IF UDP_CMD=41 if (!strcmp(UDP_CMD, "42")) { // RX UDP_CMD=42 [Change CAN Filter] printf("\nRX UDP_CMD=42 [Change CAN Filter]"); } else { // END IF UDP_CMD=42 if (!strcmp(UDP_CMD, "43")) { // RX UDP_CMD=43 [View CAN Filter] printf(" - RX UDP_CMD=43 [View CAN Filter ... ACK?]"); // Send back Empty Filter msg_out[0]='\0'; temp_msg[0]='\0'; sprintf(temp_msg, "%s%s%s", "52", PCID, "09"); // Dummy bytes for (i=6; i<=30;i++) { temp_msg[i]='0'; } // END FOR temp_msg[30] = '\0'; domocan_checksum(temp_msg, fcs); strcat(temp_msg, fcs); temp_msg[32]='\0'; raw_len = convert_raw(temp_msg, msg_out); printf("\n=>TX UDP ACK! %s",temp_msg); //s=sendto(sb, msg_out, 16, 0, (struct sockaddr *)&baddr, sizeof(baddr)); // Prepare packet. // First part is an IPv4 header. memcpy (packet, &iphdr, IP4_HDRLEN); // UDP checksum (16 bits) udphdr.check = udp4_checksum (iphdr, udphdr, msg_out, datalen); // Next part of packet is upper layer protocol header. memcpy ((packet + IP4_HDRLEN), &udphdr, UDP_HDRLEN); // Finally, add the UDP data. memcpy (packet + IP4_HDRLEN + UDP_HDRLEN, msg_out, datalen); // Send packet. if (sendto (sd, packet, IP4_HDRLEN + UDP_HDRLEN + datalen, 0, (struct sockaddr *) &sin, sizeof (struct sockaddr)) < 0) { perror ("sendto() failed "); exit (EXIT_FAILURE); } // END IF sendto //sendto(sock,msg_out,raw_len,0,(struct sockaddr*)&UDPclient,clsize); } else { // END IF UDP_CMD=43 if (!strcmp(UDP_CMD, "44")) { // RX UDP_CMD=44 [Change CAN Param] printf("\nRX UDP_CMD=44 [Change CAN Param]"); } else { // END IF UDP_CMD=44 if (!strcmp(UDP_CMD, "45")) { // RX UDP_CMD=45 [View CAN Param] printf("\nRX UDP_CMD=45 [View CAN Param]"); } // END IF RX UDP_CMD=45 } // END IF RX UDP_CMD=44 } // END IF RX UDP_CMD=43 } // END IF RX UDP_CMD=42 } // END IF RX UDP_CMD=41 } // END IF RX UDP_CMD=60 } else { if ((verbose) && (!strcmp(UDP_CMD, "70"))) printf("\nRX UDP FCS NOK! (%s-%s<>%s)\n", RX_UDP_Frame, FCS, calc_FCS); } // END IF FCS ok // } // END IF read(sa) } // END IF Received a UDP packet } // END WHILE close(sc); close(sa); close(sd); // Free allocated memory. free (data); free (packet); free (interface); free (target); free (src_ip); free (dst_ip); free (ip_flags); return 0; } // END main