Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- component hal_mydro;
- option userspace;
- option singleton yes;
- option userinit yes;
- option extra_cleanup;
- pin in float xpos;
- pin in float ypos;
- pin in float zpos;
- license "GPL";
- author "c@rstenpresser.de";
- ;;
- #include <termios.h>
- #include <fcntl.h>
- #include <stdio.h>
- #include <unistd.h>
- #include <math.h>
- #define B_Z0 5
- #define B_Y0 11
- #define B_X0 16
- // 0 1 2 3 4 5 6 7 8 9 - E FULL
- unsigned char decode7seg[13] = {0xEE, 0x82, 0xDC, 0xD6, 0xB2, 0x76, 0x7E, 0xC2, 0xFE, 0xF6, 0x10, 0x7C, 0xff};
- int fd;
- unsigned char buffer[64];
- struct termios options;
- int i,j,l;
- double x_val, y_val, z_val, k;
- double t;
- int ioerror = 0;
- char* dev="/dev/ttyACM0";
- static void aaauser_init(int argc, char **argv) {
- printf ("lol rofl\n");
- printf ("lol =%d\n",argc);
- // printf ("0 =%c\n",argv[0]);
- printf ("lol =%d\n",argc);
- }
- static void user_init(int argc, char **argv) {
- printf("hal_mydro: Trying to open (%s)\n",dev);
- fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
- if (fd <0) {
- printf("hal_mydro: Error opening (%s)\n",dev);
- exit(-EBUSY);
- } else {
- printf("hal_mydro: Open successfull (%s)\n",dev);
- // open successful
- fcntl(fd, F_SETFL, 0);
- /* get the current options */
- tcgetattr(fd, &options);
- cfsetispeed(&options, B57600);
- cfsetospeed(&options, B57600);
- /* set raw input, 1 second timeout */
- options.c_cflag |= (CLOCAL | CREAD);
- options.c_oflag &= ~OPOST;
- options.c_cc[VMIN] = 0;
- options.c_cc[VTIME] = 10;
- options.c_cflag &= ~PARENB; /* Clear parity enable */
- options.c_iflag &= ~INPCK; /* Enable parity checking */
- options.c_cflag &= ~CSTOPB;
- options.c_cflag &= ~CSIZE;
- options.c_cflag |= CS8;
- options.c_cflag &= ~CRTSCTS;
- // options.c_iflag &= ~(IXON | IXOFF | IXANY); /* no flow control */
- options.c_oflag &= ~(IXON | IXOFF | IXANY); /* no flow control */
- options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
- options.c_oflag &= ~OPOST; /* No output processing */
- options.c_oflag &= ~ONLCR; /* Don't convert linefeeds */
- /* Miscellaneous stuff */
- options.c_cflag |= (CLOCAL | CREAD); /* Enable receiver, set local */
- /* Linux seems to have problem with the following ??!! */
- options.c_cflag |= (IXON | IXOFF); /* Software flow control */
- options.c_lflag = 0; /* no local flags */
- options.c_cflag |= HUPCL; /* Drop DTR on close */
- /* Setup non blocking, return on 1 character */
- options.c_cc[VMIN] = 0;
- options.c_cc[VTIME] = 1;
- /* Clear the line */
- tcflush(fd,TCIFLUSH);
- /* Update the options and do it NOW */
- if (tcsetattr(fd,TCSANOW,&options) != 0) {
- printf("configure port failed\n");
- exit(-EBUSY);
- }
- // get version schreiben
- buffer[0] = 0xAA;
- buffer[1] = 0x01;
- buffer[3] = 0x00;
- buffer[2] = buffer[0] ^ buffer[1] ^ buffer[3];
- write(fd,buffer,4);
- usleep(10000);
- // antwort lesen
- i = read(fd,buffer,64);
- printf("hal_mydro: VERSION: Got %d bytes :",i);
- for (j = 0; j<i; j++) {
- printf(" 0x%02x ",buffer[j]);
- }
- printf("\n");
- // count bytes schreiben
- buffer[0] = 0xAA;
- buffer[1] = 0x13;
- buffer[3] = 0x00;
- buffer[2] = buffer[0] ^ buffer[1] ^ buffer[3];
- write(fd,buffer,4);
- usleep(10000);
- i = read(fd,buffer,64);
- printf("hal_mydro: NBYTES: Got %d bytes :",i);
- for (j = 0; j<i; j++) {
- printf(" 0x%02x ",buffer[j]);
- }
- printf("\n");
- }
- }
- void user_mainloop() {
- printf("hal_mydro: entered mainloop...\n");
- while (ioerror == 0) {
- /*x_val = (double)xpos;
- y_val = (double)ypos;
- z_val = (double)zpos;*/
- x_val += M_PI;
- y_val = 100-x_val;
- z_val = y_val/3;
- // Header
- buffer[0] = 0xAA;
- buffer[1] = 0x10;
- buffer[2] = 0x00;
- buffer[3] = 0x12;
- // Z
- l=B_Z0;
- k = fabs(z_val);
- for(i=2; i>-3;i--) {
- t = k;
- j = (int)(k/pow(10,i));
- k = k - j* pow(10,i);
- //printf("Z: i=%d j=%d, t=%4.2f k=%4.2f \n",i,j,t,k);
- if (j > 9) { j = 11;}
- buffer[l++] = decode7seg[j];
- }
- // Minus-Zeichen?
- if (z_val < 0) {
- if (buffer[B_Z0] == decode7seg[0]) {
- buffer[B_Z0] = decode7seg[10];
- } else {
- buffer[B_Z0] = decode7seg[11]; // Error
- }
- }
- buffer[7] += 1; //Dezimalpunkt
- buffer[4] = 0xff; //LEDs
- // Y
- l=B_Y0;
- k = fabs(y_val);
- for(i=2; i>-3;i--) {
- j = (int)(k/pow(10,i));
- k = k - j* pow(10,i);
- //printf("Y: i=%d j=%d, k=%4.2f \n",i,j,k);
- if (j > 9) { j = 11;}
- buffer[l++] = decode7seg[j];
- }
- // Minus-Zeichen?
- if (y_val < 0) {
- if (buffer[B_Y0] == decode7seg[0]) {
- buffer[B_Y0] = decode7seg[10];
- } else {
- buffer[B_Y0] = decode7seg[11]; // Error
- }
- }
- buffer[13] += 1; //Dezimalpunkt
- buffer[10] = 0; //Unbenutzt
- // X
- l=B_X0;
- k = fabs(x_val);
- for(i=3; i>-3;i--) {
- j = (int)(k/pow(10,i));
- k = k - j* pow(10,i);
- if (j > 9) { j = 11;}
- //printf("X: i=%d j=%d, k=%4.2f \n",i,j,k);
- buffer[l++] = decode7seg[j];
- }
- // Minus-Zeichen?
- if (x_val < 0) {
- if (buffer[B_X0] == decode7seg[0]) {
- buffer[B_X0] = decode7seg[10];
- } else {
- buffer[B_X0] = decode7seg[11]; // Error
- }
- }
- //Dezimalpunkt
- buffer[19] += 1;
- // calculate checksum
- buffer[2] = buffer[0] ^ buffer[1];
- for (j=3; j < 22; j++) {
- buffer[2] ^= buffer[j];
- }
- // schreiben
- write(fd,buffer,22);
- // antwort lesen
- i = read(fd,buffer,64);
- if (i != 4) {
- printf("hal_mydro: Got %d bytes :",i);
- for (j = 0; j<i; j++) {
- printf(" 0x%02x ",buffer[j]);
- }
- printf("\n");
- ioerror = 1;
- }
- //Delay
- usleep(50*1000);
- } // end while
- printf("hal_mydro: exit mainloop\n");
- } // end main_loop
- EXTRA_CLEANUP() {
- if (fd >=0) {
- close(fd);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement