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;
}
