static int cmd_init(int argc, char **argv) { int uart = -1; int baud = -1; uint32_t timeout = -1; if (argc != 3 && argc != 4) { printf("usage; %s <uart> <baudrate> [<timeout_us>]\n", argv[0]); puts("available baudrates :"); for (size_t i = 0 ; i < ARRAY_LEN(baudrates) ; i++) { printf("\t%ld\n", (long int)baudrates[i]); } return 1; } /* parse parameters */ uart = parse_uart(argv[1]); if (uart < 0) { return -1; } baud = parse_baud(argv[2]); if (baud < 0) { return -1; } if (argc == 4) { timeout = (uint32_t)atol(argv[3]); if (timeout == 0) { printf("Error : Invalid timeout (%s)", argv[3]); return -1; } } /* init */ uart_half_duplex_params_t params = { .uart = uart, .baudrate = baud, .dir = UART_HALF_DUPLEX_DIR_NONE, }; int ret = uart_half_duplex_init(&stream, feetech_buffer, ARRAY_LEN(feetech_buffer), ¶ms); if (argc == 4) { stream.timeout_us = timeout; } if (ret == UART_HALF_DUPLEX_NODEV) { puts("Error: invalid UART device given"); return -1; } if (ret == UART_HALF_DUPLEX_NOBAUD) { puts("Error: given baudrate is not applicable"); return -1; } if (ret == UART_HALF_DUPLEX_INTERR) { puts("Error: internal error"); return -1; } if (ret == UART_HALF_DUPLEX_NOMODE) { puts("Error: given mode is not applicable"); return -1; } if (ret == UART_HALF_DUPLEX_NOBUFF) { puts("Error: invalid buffer given"); return -1; } printf("Successfully initialized Feetech TTL bus UART_DEV(%i)\n", uart); return 0; }
int main(int argc, char*argv[]){ int baudrate = 0; //Check if a baudrate was given if(argc >= 1) if (sscanf (argv[1], "%i", &baudrate)!=1) {} baudrate = parse_baud(baudrate); if(baudrate == 0) { baudrate = parse_baud(9600); printf("Invalid baud rate, defaulting to 9600."); } char buffer[20]; //Check if there's a config file specifying this node's name if(access("/mnt/data/config/nodename",F_OK) != -1){ int input_fd = open("/mnt/data/config/nodename", O_RDONLY); char nameBuffer[50]; char inputBuffer; int nameLength = 0; while(read(input_fd, &inputBuffer, 1)>0 && inputBuffer >= 32 && inputBuffer <= 126) nameBuffer[nameLength++] = inputBuffer; nodeNameLength = nameLength-1; nodeName = nameBuffer; } int length, tx_length = 2; int count, value_count, transfered; char *pointer; int output_end = 0; char *portname = "/dev/ttyAMA0"; memcpy(&buffer, nodeName, sizeof(nodeName)); int fd = open (portname, O_RDONLY | O_NOCTTY | O_SYNC); if (fd < 0) { printf ("error %d opening %s: %s", errno, portname, strerror (errno)); return -20; } set_interface_attribs (fd, baudrate); //Seems to be required to flush everything when the interface settings are changed //otherwise we don't get any data when the Pi is first booted and this is run //as a daemon close(fd); sleep(2); fd = open (portname, O_RDONLY | O_NOCTTY | O_SYNC); tcflush(fd,TCIFLUSH); uint8_t buff; rI2CRX_begin(); rI2CRX_recvDecParamCB = &recvParam; rI2CRX_frameRXBeginCB = &gotAFrame; rI2CRX_frameRXEndCB = &endFrame; context = zmq_ctx_new(); pubTelemetry = zmq_socket (context, ZMQ_PUB); rc = zmq_bind (pubTelemetry, "tcp://*:3000"); assert (rc == 0); while(1){ if((length = read(fd, buffer, 20)) == -1){ printf("unable to read!\n"); } else if (length < 0) { printf("Error from read: %d: %s\n", length, strerror(errno)); } for(count = 0; count < length; count++){ buff = buffer[count]; fflush(stdout); rI2CRX_receiveBytes(&buff,1); } } zmq_close(pubTelemetry); zmq_ctx_destroy(context); close(fd); return 0; }