

#include    /* Standard input/output definitions */
#include   /* String function definitions */
#include   /* UNIX standard function definitions */
#include    /* File control definitions */
#include    /* Error number definitions */
#include  /* POSIX terminal control definitions */
#include 


int i, fd, len;

char str[3];
static unsigned char servo_buf[8]; // holds the current RC servo data block - 8 channels worth of values

int open_serial_port(void)
{
    
    fd = open("/dev/ttyS1", O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd == -1)
    {
        /*
         * Could not open the port.
         */
        
        perror("open_port: Unable to open /dev/ttyS1 - ");
    }
    else
        fcntl(fd, F_SETFL, 0);
    
    return (fd);
}

int close_serial_port(void)
{
    close (fd);
}


void WriteServo(unsigned char uc)
{
    //   WriteUART2(uc); // writes to the USART serving the servo controller

    int n = write(fd, (void *)&uc, 1); // Write one raw byte to USART
    printf("%d\n", n); // should be 1
}

// ouput the servo data block to the servo controller
void SendServo(void)
{
    unsigned char Servo_CRC;
    
    WriteServo(0x5A); // Write Header
    Servo_CRC = 0x5A; 
    
    for(i = 0;i < sizeof(servo_buf); i++)
    {
        WriteServo(servo_buf[i]);
        Servo_CRC ^= servo_buf[i];
    }
    
    WriteServo(Servo_CRC); // Write checksum

    printf("Written\n");
}

int main() 
{
    open_serial_port();
    printf("Serial port open\n");
    
    for (i = 0; i < sizeof(servo_buf); i++)
    {
        servo_buf[i] = 0x5A; // set all servos to 90 (results in a string of 10 Zs)
    }
    printf("Filled servo_buf\n");
    
    SendServo();
    
    close_serial_port();
    printf("Serial port closed\n");

}