Posts Tagged ‘ i2c ’

LPC176x UART Driver

In my last post (here), I claimed that FIFOs are often used in UART drivers. Here I will show a UART driver that utilizes dual FIFOs, one for transmit and one for receive. A universal asynchronous receiver/transmitter (UART) is a device that receives and transmits data without a known clock relationship to the connecting device. This allows each device to send data whenever it wants. This is in stark contrast to the SPI and I2C buses where the slave device can’t send data without the master first initiating a bus transfer. UARTs are very versatile and are in wide use. They are most commonly found in RS-232 ports on PCs.

The basic structure behind a UART driver is a negotiation process between the asynchronous hardware and the user’s code. FIFOs are used to aide this process. For transmitting data, it is desirable for the user to drop the data off at any time and forget about the actual serial transmission. This is where the FIFO comes in. The UART driver just takes the data and puts it in a FIFO and returns to the user. In another thread (driven by interrupts) the driver sends all the data in the FIFO as fast as it can. The receive path is very similar. The driver, again in an interrupt driven thread, transfers all received data into a FIFO. The user periodically checks if there is any new data and pulls it out at its own speed.

UARTs are often used for printing ASCII to a debug console. Most of the UARTs I have made have only been used for this purpose. For this reason it is very important to have a good method for converting numbers (integer and floating-point) to a sequence of ASCII characters. Of course, you could use a sprintf-like function, however, these are very slow. Even the embedded versions of these libraries produce terribly inefficient code (I dare you to follow the call stack of a printf function). I’m not a big fan of Arduinos, but I must say that the Arduino serial printing functions are very nice. There are no format strings to parse. Instead, the user just calls a sequence of print functions to produce the desired ASCII. My UART driver has an integrated printing library similar to the functions found in the Arduino library. This may be better off separated from the actual driver, however, I feel it fits fine into this code. You’ll notice a lot of similarity between my print functions and the Arduino serial library.

Header File

/************************************************************************
Copyright (c) 2011, Nic McDonald
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:

1. Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above
   copyright notice, this list of conditions and the following
   disclaimer in the documentation and/or other materials provided
   with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*************************************************************************

 Information:
   File Name  :  uart3.h
   Author(s)  :  Nic McDonald
   Hardware   :  LPCXpresso LPC1768
   Purpose    :  UART 3 Driver

*************************************************************************
 Modification History:
   Revision   Date         Author    Description of Revision
   1.00       05/30/2011   NGM       initial

*************************************************************************
 Assumptions:
   All print functions assume the UART is enabled.  Calling these
   functions while the UART is disabled produced undefined behavior.

************************************************************************/

#ifndef _UART3_H_
#define _UART3_H_

/* includes */
#include <stdint.h>

/* defines */
#define SW_FIFO_SIZE            512
#define UART3_DISABLED          0x00
#define UART3_OPERATIONAL       0x01
#define UART3_OVERFLOW          0x02
#define UART3_PARITY_ERROR      0x03
#define UART3_FRAMING_ERROR     0x04
#define UART3_BREAK_DETECTED    0x05
#define UART3_CHAR_TIMEOUT      0x06

/* typedefs */

/* functions */
void uart3_enable(uint32_t baudrate);
void uart3_disable(void);
void uart3_printByte(uint8_t c);
void uart3_printBytes(uint8_t* buf, uint32_t len);
void uart3_printString(char* buf); // must be null terminated
void uart3_printInt32(int32_t n, uint8_t base);
void uart3_printUint32(uint32_t n, uint8_t base);
void uart3_printDouble(double n, uint8_t frac_digits);
uint32_t uart3_available(void);
uint8_t uart3_peek(void);
uint8_t uart3_read(void);
uint8_t uart3_txStatus(void);
uint8_t uart3_rxStatus(void);

#endif /* _UART3_H_ */

Source File

/************************************************************************
Copyright (c) 2011, Nic McDonald
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:

1. Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above
   copyright notice, this list of conditions and the following
   disclaimer in the documentation and/or other materials provided
   with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*************************************************************************

 Information:
   File Name  :  uart3.c
   Author(s)  :  Nic McDonald
   Hardware   :  LPCXpresso LPC1768
   Purpose    :  UART 3 Driver

*************************************************************************
 Modification History:
   Revision   Date         Author    Description of Revision
   1.00       05/30/2011   NGM       initial

*************************************************************************
 Theory of Operation:
   This provides a simple UART driver with accompanying print functions 
   for converting integer and floating point numbers to bytes.

************************************************************************/

#include "uart3.h"
#include "fifo.h"
#include "LPC17xx.h"

/* local defines */
#define RX_TRIGGER_ONE          0x0
#define RX_TRIGGER_FOUR         0x1
#define RX_TRIGGER_EIGHT        0x2
#define RX_TRIGGER_FOURTEEN     0x3
#define RX_TRIGGER_LEVEL        RX_TRIGGER_FOURTEEN
#define RLS_INTERRUPT           0x03
#define RDA_INTERRUPT           0x02
#define CTI_INTERRUPT           0x06
#define THRE_INTERRUPT          0x01
#define LSR_RDR                 (1<<0)
#define LSR_OE                  (1<<1)
#define LSR_PE                  (1<<2)
#define LSR_FE                  (1<<3)
#define LSR_BI                  (1<<4)
#define LSR_THRE                (1<<5)
#define LSR_TEMT                (1<<6)
#define LSR_RXFE                (1<<7)

/* local persistent variables */
static uint8_t uart3_tx_sts = UART3_DISABLED;
static uint8_t uart3_rx_sts = UART3_DISABLED;
static uint8_t uart3_txBuffer[SW_FIFO_SIZE];
static uint8_t uart3_rxBuffer[SW_FIFO_SIZE];
static FIFO txFifo;
static FIFO rxFifo;

/* private function declarations */
static inline void uart3_interruptsOn(void);
static inline void uart3_interruptsOff(void);

uint32_t rdaInterrupts = 0;
uint32_t ctiInterrupts = 0;

/* public functions */
void uart3_enable(uint32_t baudrate) {
    uint32_t fdiv, pclk;

    // initial the SW FIFOs
    fifo_init(&txFifo, SW_FIFO_SIZE, (uint8_t*)uart3_txBuffer);
    fifo_init(&rxFifo, SW_FIFO_SIZE, (uint8_t*)uart3_rxBuffer);

    // set pin function to RxD3 and TxD3
    LPC_PINCON->PINSEL0 &= ~0x0000000F;
    LPC_PINCON->PINSEL0 |=  0x0000000A;

    // give power to PCUART3
    LPC_SC->PCONP |= (1 << 25);

    // set peripheral clock selection for UART3
    LPC_SC->PCLKSEL1 &= ~(3 << 18); // clear bits
    LPC_SC->PCLKSEL1 |=  (1 << 18); // set to "01" (full speed)
    pclk = SystemCoreClock;

    // set to 8 databits, no parity, and 1 stop bit
    LPC_UART3->LCR = 0x03;

    // enable 'Divisor Latch Access" (must disable later)
    LPC_UART3->LCR |= (1 << 7);

    // do baudrate calculation
    fdiv = (pclk / (16 * baudrate));
    LPC_UART3->DLM = (fdiv >> 8) & 0xFF;
    LPC_UART3->DLL = (fdiv) & 0xFF;

    // disable 'Divisor Latch Access"
    LPC_UART3->LCR &= ~(1 << 7);

    // set the number of bytes received before a RDA interrupt
    LPC_UART3->FCR |= (RX_TRIGGER_LEVEL << 6);

    // enable Rx and Tx FIFOs and clear FIFOs
    LPC_UART3->FCR |= 0x01;

    // clear Rx and Tx FIFOs
    LPC_UART3->FCR |= 0x06;

    // add the interrupt handler into the interrupt vector
    NVIC_EnableIRQ(UART3_IRQn);

    // set the priority of the interrupt
    NVIC_SetPriority(UART3_IRQn, 30); // '0' is highest

    // turn on UART3 interrupts
    uart3_interruptsOn();

    // set to operational status
    uart3_tx_sts = UART3_OPERATIONAL;
    uart3_rx_sts = UART3_OPERATIONAL;
}

void uart3_disable(void) {
    // disable interrupt
    NVIC_DisableIRQ(UART3_IRQn);

    // turn off all interrupt sources
    uart3_interruptsOff();

    // clear software FIFOs
    fifo_clear(&txFifo);
    fifo_clear(&rxFifo);

    // set to disabled status
    uart3_tx_sts = UART3_DISABLED;
    uart3_rx_sts = UART3_DISABLED;
}

void uart3_printByte(uint8_t b) {
    uint8_t thr_empty;

    // turn off UART3 interrupts while accessing shared resources
    uart3_interruptsOff();

    // determine if the THR register is empty
    thr_empty = (LPC_UART3->LSR & LSR_THRE);

    // both checks MUST be here.  there is a slight chance that
    //  the THR is empty but chars still reside in the SW Tx FIFO
    if (thr_empty && fifo_isEmpty(&txFifo)) {
        LPC_UART3->THR = b;
    }
    else {
        // turn UART3 interrupts back on to allow Sw Tx FIFO emptying
        uart3_interruptsOn();

        // wait for one slot available in the SW Tx FIFO
        while (fifo_isFull(&txFifo));

        // turn interrupts back off
        uart3_interruptsOff();

        // add character to SW Tx FIFO
        fifo_put(&txFifo, b); // <- this is the only case of txFifo putting
    }

    // turn UART3 interrupts back on
    uart3_interruptsOn();
}

void uart3_printBytes(uint8_t* buf, uint32_t len) {
    // transfer all bytes to HW Tx FIFO
    while ( len != 0 ) {
        // send next byte
        uart3_printByte(*buf);

        // update the buf ptr and length
        buf++;
        len--;
    }
}

void uart3_printString(char* buf) {
    while ( *buf != '\0' ) {
        // send next byte
        uart3_printByte((uint8_t)*buf);

        // update the buf ptr
        buf++;
    }
}

void uart3_printInt32(int32_t n, uint8_t base) {
    uint32_t i = 0;

    // print '-' for negative numbers, also negate
    if (n < 0) {
        uart3_printByte((uint8_t)'-');
        n = ((~n) + 1);
    }

    // cast to unsigned and print using uint32_t printer
    i = n;
    uart3_printUint32(i, base);
}

void uart3_printUint32(uint32_t n, uint8_t base) {
    uint32_t i = 0;
    uint8_t buf[8 * sizeof(uint32_t)]; // binary is the largest

    // check for zero case, print and bail out if so
    if (n == 0) {
        uart3_printByte((uint8_t)'0');
        return;
    }

    while (n > 0) {
        buf[i] = n % base;
        i++;
        n /= base;
    }

    for (; i > 0; i--) {
        if (buf[i - 1] < 10)
            uart3_printByte((uint8_t)('0' + buf[i - 1]));
        else
            uart3_printByte((uint8_t)('A' + buf[i - 1] - 10));
    }
}

void uart3_printDouble(double n, uint8_t frac_digits) {
    uint8_t i;
    uint32_t i32;
    double rounding, remainder;

    // test for negatives
    if (n < 0.0) {
        uart3_printByte((uint8_t)'-');
        n = -n;
    }

    // round correctly so that print(1.999, 2) prints as "2.00"
    rounding = 0.5;
    for (i=0; i<frac_digits; i++)
        rounding /= 10.0;
    n += rounding;

    // extract the integer part of the number and print it
    i32 = (uint32_t)n;
    remainder = n - (double)i32;
    uart3_printUint32(i32, 10);

    // print the decimal point, but only if there are digits beyond
    if (frac_digits > 0)
        uart3_printByte((uint8_t)'.');

    // extract digits from the remainder one at a time
    while (frac_digits-- > 0) {
        remainder *= 10.0;
        i32 = (uint32_t)remainder;
        uart3_printUint32(i32, 10);
        remainder -= i32;
    }
}

uint32_t uart3_available(void) {
    uint32_t avail;
    uart3_interruptsOff();
    avail = fifo_available(&rxFifo);
    uart3_interruptsOn();
    return avail;
}

uint8_t uart3_peek(void) {
    uint8_t ret;
    uart3_interruptsOff();
    ret = fifo_peek(&rxFifo);
    uart3_interruptsOn();
    return ret;
}

uint8_t uart3_read(void) {
    uint8_t ret;
    uart3_interruptsOff();
    ret = fifo_get(&rxFifo);
    uart3_interruptsOn();
    return ret;
}

uint8_t uart3_txStatus(void) {
    return uart3_tx_sts;
}

uint8_t uart3_rxStatus(void) {
    return uart3_rx_sts;
}

/* private functions */
void UART3_IRQHandler(void) {
    uint8_t intId;  // interrupt identification
    uint8_t lsrReg; // line status register

    // get the interrupt identification from the IIR register
    intId = ((LPC_UART3->IIR) >> 1) & 0x7;

    // RLS (receive line status) interrupt
    if ( intId == RLS_INTERRUPT ) {
        // get line status register value (clears interrupt)
        lsrReg = LPC_UART3->LSR;

        // determine type of error and set Rx status accordingly
        if (lsrReg & LSR_OE)
            uart3_rx_sts = UART3_OVERFLOW; // won't happen when using SW fifo
        else if (lsrReg & LSR_PE)
            uart3_rx_sts = UART3_PARITY_ERROR;
        else if (lsrReg & LSR_FE)
            uart3_rx_sts = UART3_FRAMING_ERROR;
        else if (lsrReg & LSR_BI)
            uart3_rx_sts = UART3_BREAK_DETECTED;
    }
    // RDA (receive data available) interrupt
    else if ( intId == RDA_INTERRUPT )      {
        // this interrupt occurs when the number of bytes in the
        //  HW Rx FIFO are greater than or equal to the trigger level 
        // (FCR[7:6])

        // read out bytes
        // clears interrupt when HW Rx FIFO is below trigger level FCR[7:6]
        // the number of loops should be the trigger level (or +1)
        while ((LPC_UART3->LSR) & 0x1)
            fifo_put(&rxFifo, LPC_UART3->RBR);
        rdaInterrupts++;
    }
    // CTI (character timeout indicator) interrupt
    else if ( intId == CTI_INTERRUPT )      {
        // this interrupt occurs when the HW Rx FIFO contains at least one
        //  char and nothing has been received in 3.5 to 4.5 char times.
        // read out all remaining bytes
        while ((LPC_UART3->LSR) & 0x1)
            fifo_put(&rxFifo, LPC_UART3->RBR);
        ctiInterrupts++;
    }
    // THRE (transmit holding register empty) interrupt
    else if ( intId == THRE_INTERRUPT ) {
        uint8_t i;
        // transfer 16 bytes if available, if not, transfer all you can
        for (i=0; ((i<16) && (!fifo_isEmpty(&txFifo))); i++)
            LPC_UART3->THR = fifo_get(&txFifo);
    }
}

static inline void uart3_interruptsOn(void) {
    LPC_UART3->IER = 0x07; // RBR, THRE, RLS
}

static inline void uart3_interruptsOff(void) {
    LPC_UART3->IER = 0x00; // !RBR, !THRE, !RLS
}

Handling FIFOs


The LPC176x UART design has hardware FIFOs built-in. Having these hardware FIFOs makes the UART hardware very efficient. However, handling the data flow between the hardware FIFOs, the software FIFOs, and the user can be very tricky. There are many situations that must be considered. The main issue is synchronization (the lack of such will cause data corruption). A correct UART driver design must always send the data in-order. Issues will occur if the driver mistakenly assumes that the software FIFO is empty and adds data directly to the hardware FIFO. If you look at the ‘print_byte()’ function, it has a lot of checks to ensure this does not happen. Throughout the code, the driver is constantly turning on and off the UART interrupts. This is because the interrupts can trigger at any time. While accessing shared memory, the interrupt code must be stalled. This is a tricky concept and is the basis for many embedded system software errors.

LPC176x I2C Driver

I’ve had a few requests for a LPC176x I2C driver.  During my development process on the LPC1768 LPCXpresso board, I wanted to design a simple I2C driver but I couldn’t find any simple examples.  Most of the drivers out there are complex and don’t have easy functionality for those who need a simple master only send/receive interface. I believe this one is simple enough to learn from.

Header File

Before writing a driver, you first need to make a specification of the interface.  I wanted my driver to be a basic send/receive interface where the slave is specified by address and the buffer is pre-allocated.  Here is the header file to my driver.

/*****************************************************************************
Copyright (c) 2011, Nic McDonald
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials provided
with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************
Copyright 2011
All Rights Reserved

Information:
File Name : i2c0.h
Author(s) : Nic McDonald
Project : Quadrotor
Hardware : LPCXpresso LPC1768
Purpose : I2C Driver

******************************************************************************
Modification History:
Revision Date Author Description of Revision
1.00 03/04/2011 NGM initial

******************************************************************************
Warning:
This I2C implementation is only for master mode. It also only
gives one transfer per transaction. This means that this driver
only does 'send' or 'receive' per function call. The user
functions 'receive' and 'send' are NOT thread safe.

*****************************************************************************/
#ifndef _I2C0_H_
#define _I2C0_H_

/* includes */
#include &lt;stdlib.h&gt;
#include &lt;stdint.h&gt;
#include "LPC17xx.h"

/* defines */
#define MODE_100kbps 100000
#define MODE_400kbps 400000
#define MODE_1Mbps 1000000

/* typedefs */

/* functions */

// Initialize the I2C hardware.
// see 'readme'
void i2c0_init(uint32_t i2c_freq, uint8_t int_pri);

// Performs a I2C master send function.
// Returns the number of bytes sent successfully.
// Returns 0xFFFFFFFF if slave did not response on bus.
// This is NOT thread safe.
uint32_t i2c0_send(uint8_t address, uint8_t* buffer, uint32_t length);

// Performs a I2C master receive function.
// Returns the number of bytes received successfully.
// Returns 0xFFFFFFFF if slave did not response on bus.
// This is NOT thread safe.
uint32_t i2c0_receive(uint8_t address, uint8_t* buffer, uint32_t length);

/*** DEBUG ***/uint8_t* i2c_buf(void);
/*** DEBUG ***/uint32_t i2c_pos(void);

#endif /* _I2C0_H_ */

Source File

Now that we have a good interface, let’s see what we need to implement.

/*****************************************************************************
Copyright (c) 2011, Nic McDonald
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above
   copyright notice, this list of conditions and the following
   disclaimer in the documentation and/or other materials provided
   with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************
                                Copyright 2011
                             All Rights Reserved

 Information:
   File Name  :  i2c0.c
   Author(s)  :  Nic McDonald
   Project    :  Quadrotor
   Hardware   :  LPCXpresso LPC1768
   Purpose    :  I2C Driver

******************************************************************************
 Modification History:
   Revision   Date         Author    Description of Revision
   1.00       03/04/2011   NGM       initial

*****************************************************************************/
#include "i2c0.h"

// IC2 control bits
#define AA      (1 << 2)
#define SI      (1 << 3)
#define STO     (1 << 4)
#define STA     (1 << 5)
#define I2EN    (1 << 6)

// pointers setup by users functions
static volatile uint8_t  slave_address; // formatted by send or receive
static volatile uint8_t* buf;
static volatile uint32_t buf_len;
static volatile uint32_t num_transferred;
static volatile uint32_t i2c0_busy;

static inline uint8_t to_read_address(uint8_t address);
static inline uint8_t to_write_address(uint8_t address);

/*************DEBUG**************************************************************************************/
uint8_t i2c_status_buf[100];
uint32_t i2c_status_pos;
uint8_t* i2c_buf(void) {return i2c_status_buf;}
uint32_t i2c_pos(void) {return i2c_status_pos;}
/*************DEBUG**************************************************************************************/

LPC_I2C_TypeDef*  regs;
IRQn_Type         irqn;
uint32_t ignore_data_nack = 1;


void i2c0_init(uint32_t i2c_freq, uint8_t int_pri) {
    uint32_t pclk, fdiv;

    regs = LPC_I2C0;
    irqn = I2C0_IRQn;

    // setup initial state
    i2c0_busy = 0;
    buf = NULL;
    buf_len = 0;
    num_transferred = 0;

    // give power to the I2C hardware
    LPC_SC->PCONP |= (1 << 7);

    // set PIO0.27 and PIO0.28 to I2C0 SDA and SCK
    LPC_PINCON->PINSEL1 &= ~0x03C00000;
    LPC_PINCON->PINSEL1 |=  0x01400000;

    // set peripheral clock selection for I2C0
    LPC_SC->PCLKSEL0 &= ~(3 << 14); // clear bits
    LPC_SC->PCLKSEL0 |=  (1 << 14); // set to "01" (full speed)
    pclk = SystemCoreClock;

    // clear all flags
    regs->I2CONCLR = AA | SI | STO | STA | I2EN;

    // determine the frequency divider and set corresponding registers
    //  this makes a 50% duty cycle
    fdiv = pclk / i2c_freq;
    regs->I2SCLL = fdiv >> 1; // fdiv / 2
    regs->I2SCLH = fdiv - (fdiv >> 1); // compensate for odd dividers

    // install interrupt handler
    NVIC_EnableIRQ(irqn);

    // set the priority of the interrupt
    NVIC_SetPriority(irqn, int_pri); // '0' is highest

    // enable the I2C (master only)
    regs->I2CONSET = I2EN;
}

uint32_t i2c0_send(uint8_t address, uint8_t* buffer, uint32_t length) {
    // check software FSM
    if (i2c0_busy)
        //error_led_trap(0x11000001, i2c0_busy, 0, 0, 0, 0, 0, 0, 0);
        return 0;

    // set to status to 'busy'
    i2c0_busy = 1;

    // setup pointers
    slave_address = to_write_address(address);
    buf = buffer;
    buf_len = length;
    num_transferred = 0;

    // trigger a start condition
    regs->I2CONSET = STA;

    // wait for completion
    while (i2c0_busy);

    // get how many bytes were transferred
    return num_transferred;
}

uint32_t i2c0_receive(uint8_t address, uint8_t* buffer, uint32_t length) {
    // check software FSM
    if (i2c0_busy)
        //error_led_trap(0x11000002, i2c0_busy, 0, 0, 0, 0, 0, 0, 0);
        return 0;

    // set to status to 'busy'
    i2c0_busy = 1;

    // setup pointers
    slave_address = to_read_address(address);
    buf = buffer;
    buf_len = length;
    num_transferred = 0;

    // trigger a start condition
    regs->I2CONSET = STA;

    // wait for completion
    while (i2c0_busy);

    // get how many bytes were transferred
    return num_transferred;
}

void I2C0_IRQHandler(void) {
    // get reason for interrupt
    uint8_t status = regs->I2STAT;

    // ignore data nack when control is true
    if ((status == 0x30) && (ignore_data_nack))
            status = 0x28;

    // LPC17xx User Manual page 443:
    //      "...read and write to [I2DAT] only while ... the SI bit is set"
    //      "Data in I2DAT remains stable as long as the SI bit is set."


    /**************************************DEBUG************************************************************/
    i2c_status_buf[i2c_status_pos] = status;
    i2c_status_pos++;
    if (i2c_status_pos > 99)
        i2c_status_pos = 0;
    /**************************************DEBUG************************************************************/


    switch(status) {

    // Int: start condition has been transmitted
    // Do:  send SLA+R or SLA+W
    case 0x08:
        regs->I2DAT = slave_address; // formatted by send or receive
        regs->I2CONCLR = STA | SI;
        break;

    // Int: repeated start condition has been transmitted
    // Do:  send SLA+R or SLA+W
    //case 0x10:
    //    regs->I2DAT = slave_address;
    //    regs->I2CONCLR = STA | SI;
    //    break;

    // Int: SLA+W has been transmitted, ACK received
    // Do:  send first byte of buffer if available
    case 0x18:
        if (num_transferred < buf_len) {
            regs->I2DAT = buf[0];
            regs->I2CONCLR = STO | STA | SI;
        }
        else {
            regs->I2CONCLR = STA | SI;
            regs->I2CONSET = STO;
        }
        break;

    // Int: SLA+W has been transmitted, NACK received
    // Do:  stop!
    case 0x20:
        regs->I2CONCLR = STA | SI;
        regs->I2CONSET = STO;
        num_transferred = 0xFFFFFFFF;
        i2c0_busy = 0;
        break;

    // Int: data byte has been transmitted, ACK received
    // Do:  load next byte if available, else stop
    case 0x28:
        num_transferred++;
        if (num_transferred < buf_len) {
            regs->I2DAT = buf[num_transferred];
            regs->I2CONCLR = STO | STA | SI;
        }
        else {
            regs->I2CONCLR = STA | SI;
            regs->I2CONSET = STO;
            i2c0_busy = 0;
        }
        break;

    // Int: data byte has been transmitted, NACK received
    // Do:  stop!
    case 0x30:
        regs->I2CONCLR = STA | SI;
        regs->I2CONSET = STO;
        i2c0_busy = 0;
        break;

    // Int: arbitration lost in SLA+R/W or Data bytes
    // Do:  release bus
    case 0x38:
        regs->I2CONCLR = STO | STA | SI;
        i2c0_busy = 0;
        break;

    // Int: SLA+R has been transmitted, ACK received
    // Do:  determine if byte is to be received
    case 0x40:
        if (num_transferred < buf_len) {
            regs->I2CONCLR = STO | STA | SI;
            regs->I2CONSET = AA;
        }
        else {
            regs->I2CONCLR = AA | STO | STA | SI;
        }
        break;

    // Int: SLA+R has been transmitted, NACK received
    // Do:  stop!
    case 0x48:
        regs->I2CONCLR = STA | SI;
        regs->I2CONSET = STO;
        num_transferred = 0xFFFFFFFF;
        i2c0_busy = 0;
        break;

    // Int: data byte has been received, ACK has been returned
    // Do:  read byte, determine if another byte is needed
    case 0x50:
        buf[num_transferred] = regs->I2DAT;
        num_transferred++;
        if (num_transferred < buf_len) {
            regs->I2CONCLR = STO | STA | SI;
            regs->I2CONSET = AA;
        }
        else {
            regs->I2CONCLR = AA | STO | STA | SI;
        }
        break;

    // Int: data byte has been received, NACK has been returned
    // Do:  transfer is done, stop.
    case 0x58:
        regs->I2CONCLR = STA | SI;
        regs->I2CONSET = STO;
        i2c0_busy = 0;
        break;

    // something went wrong, trap error
    default:
        while (1); // flash a LED or something 😦
        break;

    }
}

static inline uint8_t to_read_address(uint8_t address) {
    return (address << 1) | 0x01;
}
static inline uint8_t to_write_address(uint8_t address) {
    return (address << 1);
}

As you can see, the implementation is fairly simple except for the interrupt handler. Fortunately, NXP is a great vendor when it comes to documentation. The user manual (found here) explains everything in detail. In fact, the state machine implemented in my driver’s interrupt handler is taken directly from the instructions in the manual. Each time an I2C event occurs, the I2C interrupter reports a status code. The user manual tells you exactly what to do for each status code. Using a large switch/case statement as I have done, leads to very short interrupt handling time.

I left some debugging code in there as I found it was extremely useful. The ‘i2c_buf’ and ‘i2c_pos’ functions allow me to retrieve information about the i2c transfer. The ‘i2c0_send’ and ‘i2c0_recv’ functions are mostly unconnected with the interrupts so there is no good way to figure what is going wrong when it does. Using a small buffer lets me see the order in which the interrupt status codes come. This allows me to determine what went wrong and why. This debug buffer isn’t flawless. I only used it to see one transaction length. I suggest removing it from the code once you verified that the driver works for you.

Conclusion

I hope that no one takes this code and uses it.  Instead, I’d hope that you’d take this code, verify it works in your system, then use it to start working on your own driver!  Making an I2C driver is a lot of fun and allows you to write code that heavily interacts with the hardware.  Making a finite state machine around the I2C status codes will really help you learn driver development. I2C is one of the more complicated protocols. UART, SPI, etc. are much easier and are a better starting point for a beginner. USB, Ethernet, CAN, etc. are more complicated than I2C. I2C presents a nice bridge between the extremely easy and the extremely hard.

Sample Usage:

#include <stdio.h>
#include "i2c0.h"
void main() {
    i2c0_init(MODE_400kbps, 3);
    char buf[100] = "hello";
    uint8_t slave = 0xEE;
    uint32_t res;
    if ((res = i2c0_send(slave, buf, sizeof(buf))) == 0xFFFFFFFF)
        /* slave did not response on bus */;
    if ((res = i2c0_recv(slave, buf, sizeof(buf))) == 0xFFFFFFFF)
        /* slave did not response on bus */;
    else {
        buf[res] = '\0';
        printf("Slave responded: %s\n", buf);
    }
}

Quadcopter Software Design

Before I begin discussing the quadcopter design, here is a diagram that shows the high-level software structure for my design:

All future software design posts will reference to this diagram.  Here is a short explanation of the necessity and function of each block:

PWM Decoder Driver:
In order to interpret flight commands from a standard RC Transmitter/Receiver, any multi-rotor design must have some sort of PWM decoder.  My decoder will be expecting a single signal containing all PWM channels (details here).  The decoder driver will translate the several channels of pulse widths into values to be interpreted by the command translator.

Command Translator:
Depending on the flight mode, the PWM values given by the transmitter are interpreted differently.  The command translator determines which mode the user is flying in and translates the commands accordingly.

I2C Master:
The inertial sensors I’ve chosen to use all run on the I2C protocol.  This high-speed protocol will be a great interface to use because of its addressing scheme and its interrupt time efficiency.  The I2C protocol is more complicated that most serial protocols but the LPC1768 has built-in hardware to handle the physical layer.  The I2C interrupt scheme for this hardware is very efficient with each interrupt needing only a few lines of code.

Sensor Fusion:
As mentioned in an earlier post, I will be implementing several sensor fusion algorithms to find out which has the best performance.  The first one I will implement is the Extended Kalman Filter (EKF).  The EKF algorithm is widely known to be one of the best methods for state (in my case aircraft attitude) estimation.  This filter will take the sensor readings from the various sensors and output an estimation of the current aircraft attitude.  For my initial design it will be important to measure:

  • Roll – Side to side angle relative to the horizon
  • Pitch – Front to back angle relative to the horizon
  • Roll Rate – Side to side rotational rate relative to the horizon
  • Pitch Rate – Front to back rotational rate relative to the horizon
  • Yaw Rate – Horizontal rotational rate relative to magnetic heading
  • Magnetic Heading – Direction relative to magnetic north

Proportional-Integral-Derivative (PID) Stabilization:
Once the desired aircraft attitude is determined from the transmitter commands and the actual aircraft attitude is determined from the sensor fusion algorithm, PID controllers are used to determine the fastest way to make the desired attitude become the actual attitude.  PID controllers are very efficient for control of a system in which an accurate physical model is unknown.  Using calculus to determine error slopes and areas, PID controllers compensate for environmental noise and disturbances while overcoming steady state error and oscillations.  PID controllers are VERY simple to design and code but are VERY hard to tune and calibrate.  (see this post)

Multi-Rotor Throttle Mixing (MRTM):
Standard helicopters use Cyclic Collective Pitch Mixing (CCPM) to adjust the aircraft attitude.  It works by adjusting the pitch of the blades depending on their current angular position.  I came up with term MRTM, as I haven’t found a term defined for multi-rotor helicopters.  MRTM works by adjusting the speed of several propellers in such a way that results in the same aircraft control and a typical helicopter (a post will soon follow that explains this in detail).  MRTM can be used to control the attitude of many styles of multi-rotor helicopters.

PWM Encoder Driver:
Once everything has been computed, the PWM Encoder takes the control values and generates a pulse width modulated (PWM) output for each motor.  This is the exact opposite process of the PWM Decoder.  The LPC1768 has good support for PWM output.  I’ll be able to output all channels of PWM without using any interrupts or processing time.  Can’t beat that!