UKHAS Wiki

#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <stdlib.h>
 
 
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");
 
}