commit b018a107d2b8feb9b49cfe45959bd7d3378c85a5 Author: kevin.larke Date: Sat Jul 6 21:59:39 2019 -0400 Added picadae/ diff --git a/control/app/picadae_cmd.py b/control/app/picadae_cmd.py new file mode 100644 index 0000000..3ddc33b --- /dev/null +++ b/control/app/picadae_cmd.py @@ -0,0 +1,285 @@ +import os,sys,argparse,yaml,types,select,serial,logging,time + +from multiprocessing import Process, Pipe + +# Message header id's for messages passed between the application +# process and the microcontroller and video processes +QUIT_MSG = 0xffff +DATA_MSG = 0xfffe +ERROR_MSG = 0xfffd + + +def _reset_port(port): + port.reset_input_buffer() + port.reset_output_buffer() + port.send_break() + #self.reportResetFl = True + #self.reportStatusFl = True + +def _serial_process_func( serial_dev, baud, sensor_count, pipe ): + + reset_N = 0 + drop_N = 0 + noSync_N = 0 + + with serial.Serial(serial_dev, baud) as port: + + while True: + + # get the count of available bytes in the serial port buffer + bytes_waiting_N = port.in_waiting + + + # if no serial port bytes are available then sleep .... + if bytes_waiting_N == 0: + time.sleep(0.01) # ... for 10 ms + + else: # read the serial port ... + v = port.read(bytes_waiting_N) + + pipe.send((DATA_MSG,v)) # ... and send it to the parent + + + + msg = None + if pipe.poll(): # non-blocking check for parent process messages + try: + msg = pipe.recv() + except EOFError: + break + + # if an incoming message was received + if msg != None: + + # this is a shutdown msg + if msg[0] == QUIT_MSG: + pipe.send(msg) # ... send quit msg back + break + + # this is a data xmit msg + elif msg[0] == DATA_MSG: + port.write(msg[1]) + + + + + +class SessionProcess(Process): + def __init__(self,target,name,args=()): + self.parent_end, child_end = Pipe() + super(SessionProcess, self).__init__(target=target,name=name,args=args + (child_end,)) + self.doneFl = False + + def quit(self): + # send quit msg to the child process + self.parent_end.send((QUIT_MSG,0)) + + def send(self,msg_id,value): + # send a msg to the child process + self.parent_end.send((msg_id,value)) + + def recv(self): + x = None + if not self.doneFl and self.parent_end.poll(): + x = self.parent_end.recv() + + if x[0] == QUIT_MSG: + self.doneFl = True + + return x + + def isDone(self): + return self.doneFl + +class SerialProcess(SessionProcess): + def __init__(self,serial_device,baud,foo): + super(SerialProcess, self).__init__(_serial_process_func,"Serial",args=(serial_device,baud,foo)) + + + +class App: + def __init__( self, cfg ): + + self.cfg = cfg + self.serialProc = SerialProcess(cfg.serial_dev,cfg.serial_baud,0) + + + def _update( self, quittingFl=False ): + + if self.serialProc.isDone(): + return False + + while True: + msg = serialProc.recv() + + # no serial msg's were received + if msg is None: + break + + if msg[0] == DATA_MSG: + print("in:",msg[1]) + + + def _parse_error( self, msg ): + return (None,msg) + + def _parse_int( self, token, var_label, max_value ): + # convert the i2c destination address to an integer + try: + int_value = int(token) + except ValueError: + return self._parse_error("Synax error: '{}' is not a legal integer.".format(token)) + + # validate the i2c address value + if 0 > int_value or int_value > max_value: + return self._parse_error("Syntax error: '{}' {} out of range 0 to {}.".format(token,int_value,max_value)) + + return (int_value,None) + + def parse_cmd( self, cmd_str ): + + op_tok_idx = 0 + i2c_tok_idx = 1 + reg_tok_idx = 2 + rdn_tok_idx = 3 + + cmd_str.strip() + + # convert the command string to tokens + tokenL = cmd_str.split(' ') + + # no commands require fewer than 4 tokens + if len(tokenL) < 4: + return self._parse_error("Syntax error: Missing tokens.") + + # get the command opcode + op_code = tokenL[ op_tok_idx ] + + # validate the opcode + if op_code not in [ 'r','w']: + return self._parse_error("Unrecognized opcode: {}".format( op_code )) + + # validate the token count given the opcode + if op_code == 'r' and len(tokenL) != 4: + return self._parse_error("Syntax error: Illegal read syntax.") + + if op_code == 'w' and len(tokenL) == 4: + return self._parse_error("Syntax error: Illegal write command too short.") + + # convert the i2c destination address to an integer + i2c_addr, msg = self._parse_int( tokenL[i2c_tok_idx], "i2c address", 127 ) + + if i2c_addr is None: + return (None,msg) + + reg_addr, msg = self._parse_int( tokenL[reg_tok_idx], "reg address", 255 ) + + if reg_addr is None: + return (None,msg) + + dataL = [] + + # parse and validate the count of bytes to read + if op_code == 'r': + op_byteN, msg = self._parse_int( tokenL[ rdn_tok_idx ], "read byte count", 255 ) + + if op_byteN is None: + return (None,msg) + + + # parse and validate the values to write + elif op_code == 'w': + + for j,i in enumerate(range(reg_tok_idx+1,len(tokenL))): + value, msg = self._parse_int( tokenL[i], "write value: %i" % (j), 255 ) + + if value is None: + return (None,msg) + + dataL.append(value) + + op_byteN = len(dataL) + + + # form the command into a byte array + cmd_bV = bytearray( [ ord(op_code), i2c_addr, reg_addr, op_byteN ] + dataL ) + + return (cmd_bV,None) + + + def run( self ): + + self.serialProc.start() + + print("'quit' to exit") + time_out_secs = 1 + + while True: + + i, o, e = select.select( [sys.stdin], [], [], time_out_secs ) + + if (i): + s = sys.stdin.readline().strip() + + if s == 'quit': + break + + cmd_bV,err_msg = self.parse_cmd(s) + + if cmd_bV is None: + print(err_msg) + else: + self.serialProc.send( DATA_MSG, cmd_bV ) + + + else: + # wait timed out + msg = self.serialProc.recv() + + # if a serial msg was received + if msg is not None and msg[0] == DATA_MSG: + print("ser:",msg[1]) + + + self.serialProc.quit() + +def parse_args(): + """Parse the command line arguments.""" + + descStr = """Picadae auto-calibrate.""" + logL = ['debug','info','warning','error','critical'] + + ap = argparse.ArgumentParser(description=descStr) + + + ap.add_argument("-s","--setup", default="cfg/p_ac.yml", help="YAML configuration file.") + ap.add_argument("-c","--cmd", nargs="*", help="Give a command as multiple tokens") + ap.add_argument("-r","--run", help="Run a named command list from the setup file.") + ap.add_argument("-l","--log_level",choices=logL, default="warning", help="Set logging level: debug,info,warning,error,critical. Default:warning") + + return ap.parse_args() + + +def parse_yaml_cfg( fn ): + """Parse the YAML configuration file.""" + + cfg = None + + with open(fn,"r") as f: + cfgD = yaml.load(f, Loader=yaml.FullLoader) + + cfg = types.SimpleNamespace(**cfgD['picadae_cmd']) + + return cfg + + + +if __name__ == "__main__": + + args = parse_args() + + cfg = parse_yaml_cfg( args.setup ) + + app = App(cfg) + + app.run() diff --git a/control/app/picadae_cmd.yml b/control/app/picadae_cmd.yml new file mode 100644 index 0000000..7234f12 --- /dev/null +++ b/control/app/picadae_cmd.yml @@ -0,0 +1,8 @@ +{ + picadae_cmd: + { + serial_dev: "/dev/ttyACM0", + serial_baud: 38400, + + } +} diff --git a/control/ctrl/Makefile b/control/ctrl/Makefile new file mode 100644 index 0000000..09dc77a --- /dev/null +++ b/control/ctrl/Makefile @@ -0,0 +1,23 @@ +ifndef TTY +TTY=/dev/ttyACM0 +endif + +# compiler flags: +# -Os : optimize size +# -DF_CPU=16000000UL : define F_CPU as 16Mghz +# -mmcu=atmega328p : target MCU + +main.hex : main.c + # compile to object file (optimize for size) + avr-gcc -Os -DF_CPU=16000000UL -mmcu=atmega328p -o main.elf main.c twi.c + # link as ELF binary + #avr-gcc -mmcu=atmega328p main.o -o main.elf + # convert ELF format to an IHEX format as used by avrdude + avr-objcopy -O ihex -R .eeprom main.elf main.hex + + +burn: + avrdude -F -V -c arduino -p ATMEGA328P -P $(TTY) -b 115200 -U flash:w:main.hex + +clean : + rm main.o twi.o main.elf main.hex diff --git a/control/ctrl/main.c b/control/ctrl/main.c new file mode 100644 index 0000000..31ea277 --- /dev/null +++ b/control/ctrl/main.c @@ -0,0 +1,237 @@ +// Note this program is designed to pair with c_client. +// The i2c connection may be made directly between the two Arduino SDA and SCL pins. +// No i2c pullup resistors are require.d +#define F_CPU 16000000UL +#define BAUD 38400 + +#include +#include +#include + +#include "twi.h" + +//------------------------------------------------------------------------------ + +#define SER_BUF_N (16) // size of receive buffer + +// Note that 'ser_buf_i_idx' must be declared volatile or the +// the compare in the main loop will not work. +volatile int ser_buf_i_idx = 0; // receive buffer input index +int ser_buf_o_idx = 0; // receive buffer output index + +// Receive buffer +char buf[ SER_BUF_N ]; + + +void uart_init(void) +{ + UBRR0H = UBRRH_VALUE; + UBRR0L = UBRRL_VALUE; + +#if USE_2X + UCSR0A |= _BV(U2X0); +#else + UCSR0A &= ~(_BV(U2X0)); +#endif + + UCSR0C = _BV(UCSZ01) | _BV(UCSZ00); // 8-bit data + UCSR0B = _BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0); // Enable RX and TX and RX intertupt enable +} + +void uart_putchar(char c) +{ + loop_until_bit_is_set(UCSR0A, UDRE0); // Wait until data register empty. + UDR0 = c; +} + +void uart_putchar_alt(char c) +{ + UDR0 = c; + loop_until_bit_is_set(UCSR0A, TXC0); // Wait until transmission ready. +} + +char uart_getchar(void) +{ + loop_until_bit_is_set(UCSR0A, RXC0); // Wait until data exists. + return UDR0; +} + +//------------------------------------------------------------------------------ + +#define I2C_LOCAL_ADDR (9) +#define I2C_REMOTE_ADDR (8) + +#define I2C_BUF_N (16) +static uint8_t i2c_buf[ I2C_BUF_N ]; +static volatile uint8_t i2c_buf_i_idx = 0; +static uint8_t i2c_buf_o_idx = 0; + +static uint8_t last_char = '0'; + +void i2c_read_from( uint8_t i2c_addr, uint8_t dev_reg_addr, uint8_t read_byte_cnt ) +{ + uint8_t recv_char = '0'; + const uint8_t kWaitFl = 1; + const uint8_t kSendStopFl = 1; + const uint8_t kNoSendStopFl = 0; + + // Request to read from the client. Note that 'sendStop'==0. + // Use this call to tell the client what data should be sent + // during the subsequent twi_readFrom(). + twi_writeTo(I2C_REMOTE_ADDR, &dev_reg_addr, 1, kWaitFl, kNoSendStopFl); + + + // Blocking waiting and wait to read the client's response. + for( uint8_t i=0; i i2c_read_from() + // 'w', reg-idx, cnt, value0, ... valueN -> i2c_xmit() + + switch(state) + { + case kWait_for_cmd: + if(c == 'w' || c == 'r') + { + cmd = c; + state = kWait_for_i2c; + } + else + uart_putchar('E'); + break; + + case kWait_for_i2c: + i2c_addr = (uint8_t)c; + state = kWait_for_reg; + break; + + case kWait_for_reg: + dev_reg_addr = (uint8_t)c; + state = kWait_for_cnt; + break; + + case kWait_for_cnt: + op_byte_cnt = (uint8_t)c; + + if( cmd == 'r' ) + { + i2c_read_from( i2c_addr, dev_reg_addr, op_byte_cnt ); + state = kWait_for_cmd; + } + else + { + state = kWait_for_value; + } + break; + + case kWait_for_value: + { + uint8_t buf[] = { dev_reg_addr, c }; + + i2c_xmit( I2C_REMOTE_ADDR, buf, 2, kSendStopFl); + state = kWait_for_cmd; + } + break; + + } + } + } +} + + + + diff --git a/control/ctrl/twi.c b/control/ctrl/twi.c new file mode 100644 index 0000000..94c75be --- /dev/null +++ b/control/ctrl/twi.c @@ -0,0 +1,572 @@ +/* + twi.c - TWI/I2C library for Wiring & Arduino + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts +*/ + +#include +#include +#include +#include +#include +#include +// kpl #include "Arduino.h" // for digitalWrite + +#define true (1) // kpl +#define false (0) // kpl + +#ifndef cbi +#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif + +#ifndef sbi +#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +//#include "pins_arduino.h" +#include "twi.h" + +static volatile uint8_t twi_state; +static volatile uint8_t twi_slarw; +static volatile uint8_t twi_sendStop; // should the transaction end with a stop +static volatile uint8_t twi_inRepStart; // in the middle of a repeated start + +static void (*twi_onSlaveTransmit)(void); +static void (*twi_onSlaveReceive)(uint8_t*, int); + +static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_masterBufferIndex; +static volatile uint8_t twi_masterBufferLength; + +static uint8_t twi_txBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_txBufferIndex; +static volatile uint8_t twi_txBufferLength; + +static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_rxBufferIndex; + +static volatile uint8_t twi_error; + +/* + * Function twi_init + * Desc readys twi pins and sets twi bitrate + * Input none + * Output none + */ +void twi_init(void) +{ + // initialize state + twi_state = TWI_READY; + twi_sendStop = true; // default value + twi_inRepStart = false; + + // activate internal pullups for twi. + //kpl digitalWrite(SDA, 1); + //kpl digitalWrite(SCL, 1); + DDRC |= _BV(DDC4) + _BV(DDC5); // kpl for atmega328 TODO: replace with sbi() macro and make cross-compilable + PORTC |= _BV(PORTC4) + _BV(PORTC5); + + + + // initialize twi prescaler and bit rate + cbi(TWSR, TWPS0); + cbi(TWSR, TWPS1); + TWBR = ((F_CPU / TWI_FREQ) - 16) / 2; + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ + + // enable twi module, acks, and twi interrupt + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); +} + +/* + * Function twi_disable + * Desc disables twi pins + * Input none + * Output none + */ +void twi_disable(void) +{ + // disable twi module, acks, and twi interrupt + TWCR &= ~(_BV(TWEN) | _BV(TWIE) | _BV(TWEA)); + + // deactivate internal pullups for twi. + // kpl digitalWrite(SDA, 0); + // kpl digitalWrite(SCL, 0); + DDRC &= ~(_BV(DDC4) + _BV(DDC5)); // kpl for atmega328 TODO: replace with cbi() macro and make cross-compilable + PORTC &= ~(_BV(PORTC4) + _BV(PORTC5)); + + +} + +/* + * Function twi_slaveInit + * Desc sets slave address and enables interrupt + * Input none + * Output none + */ +void twi_setAddress(uint8_t address) +{ + // set twi slave address (skip over TWGCE bit) + TWAR = address << 1; +} + +/* + * Function twi_setClock + * Desc sets twi bit rate + * Input Clock Frequency + * Output none + */ +void twi_setFrequency(uint32_t frequency) +{ + TWBR = ((F_CPU / frequency) - 16) / 2; + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ +} + +/* + * Function twi_readFrom + * Desc attempts to become twi bus master and read a + * series of bytes from a device on the bus + * Input address: 7bit i2c device address + * data: pointer to byte array + * length: number of bytes to read into array + * sendStop: Boolean indicating whether to send a stop at the end + * Output number of bytes read + */ +uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < length){ + return 0; + } + + // wait until twi is ready, become master receiver + while(TWI_READY != twi_state){ + continue; + } + twi_state = TWI_MRX; + twi_sendStop = sendStop; + // reset error state (0xFF.. no error occured) + twi_error = 0xFF; + + // initialize buffer iteration vars + twi_masterBufferIndex = 0; + twi_masterBufferLength = length-1; // This is not intuitive, read on... + // On receive, the previously configured ACK/NACK setting is transmitted in + // response to the received byte before the interrupt is signalled. + // Therefor we must actually set NACK when the _next_ to last byte is + // received, causing that NACK to be sent in response to receiving the last + // expected byte of data. + + // build sla+w, slave device address + w bit + twi_slarw = TW_READ; + twi_slarw |= address << 1; + + if (true == twi_inRepStart) { + // if we're in the repeated start state, then we've already sent the start, + // (@@@ we hope), and the TWI statemachine is just waiting for the address byte. + // We need to remove ourselves from the repeated start state before we enable interrupts, + // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning + // up. Also, don't enable the START interrupt. There may be one pending from the + // repeated start that we sent ourselves, and that would really confuse things. + twi_inRepStart = false; // remember, we're dealing with an ASYNC ISR + do { + TWDR = twi_slarw; + } while(TWCR & _BV(TWWC)); + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE); // enable INTs, but not START + } + else + // send start condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + + // wait for read operation to complete + while(TWI_MRX == twi_state){ + continue; + } + + if (twi_masterBufferIndex < length) + length = twi_masterBufferIndex; + + // copy twi buffer to data + for(i = 0; i < length; ++i){ + data[i] = twi_masterBuffer[i]; + } + + return length; +} + +/* + * Function twi_writeTo + * Desc attempts to become twi bus master and write a + * series of bytes to a device on the bus + * Input address: 7bit i2c device address + * data: pointer to byte array + * length: number of bytes in array + * wait: boolean indicating to wait for write or not + * sendStop: boolean indicating whether or not to send a stop at the end + * Output 0 .. success + * 1 .. length to long for buffer + * 2 .. address send, NACK received + * 3 .. data send, NACK received + * 4 .. other twi error (lost bus arbitration, bus error, ..) + */ +uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < length){ + return 1; + } + + // wait until twi is ready, become master transmitter + while(TWI_READY != twi_state){ + continue; + } + twi_state = TWI_MTX; + twi_sendStop = sendStop; + // reset error state (0xFF.. no error occured) + twi_error = 0xFF; + + // initialize buffer iteration vars + twi_masterBufferIndex = 0; + twi_masterBufferLength = length; + + // copy data to twi buffer + for(i = 0; i < length; ++i){ + twi_masterBuffer[i] = data[i]; + } + + // build sla+w, slave device address + w bit + twi_slarw = TW_WRITE; + twi_slarw |= address << 1; + + // if we're in a repeated start, then we've already sent the START + // in the ISR. Don't do it again. + // + if (true == twi_inRepStart) { + // if we're in the repeated start state, then we've already sent the start, + // (@@@ we hope), and the TWI statemachine is just waiting for the address byte. + // We need to remove ourselves from the repeated start state before we enable interrupts, + // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning + // up. Also, don't enable the START interrupt. There may be one pending from the + // repeated start that we sent outselves, and that would really confuse things. + twi_inRepStart = false; // remember, we're dealing with an ASYNC ISR + do { + TWDR = twi_slarw; + } while(TWCR & _BV(TWWC)); + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE); // enable INTs, but not START + } + else + // send start condition + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA); // enable INTs + + // wait for write operation to complete + while(wait && (TWI_MTX == twi_state)){ + continue; + } + + if (twi_error == 0xFF) + return 0; // success + else if (twi_error == TW_MT_SLA_NACK) + return 2; // error: address send, nack received + else if (twi_error == TW_MT_DATA_NACK) + return 3; // error: data send, nack received + else + return 4; // other twi error +} + +/* + * Function twi_transmit + * Desc fills slave tx buffer with data + * must be called in slave tx event callback + * Input data: pointer to byte array + * length: number of bytes in array + * Output 1 length too long for buffer + * 2 not slave transmitter + * 0 ok + */ +uint8_t twi_transmit(const uint8_t* data, uint8_t length) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < (twi_txBufferLength+length)){ + return 1; + } + + // ensure we are currently a slave transmitter + if(TWI_STX != twi_state){ + return 2; + } + + // set length and copy data into tx buffer + for(i = 0; i < length; ++i){ + twi_txBuffer[twi_txBufferLength+i] = data[i]; + } + twi_txBufferLength += length; + + return 0; +} + +/* + * Function twi_attachSlaveRxEvent + * Desc sets function called before a slave read operation + * Input function: callback function to use + * Output none + */ +void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) ) +{ + twi_onSlaveReceive = function; +} + +/* + * Function twi_attachSlaveTxEvent + * Desc sets function called before a slave write operation + * Input function: callback function to use + * Output none + */ +void twi_attachSlaveTxEvent( void (*function)(void) ) +{ + twi_onSlaveTransmit = function; +} + +/* + * Function twi_reply + * Desc sends byte or readys receive line + * Input ack: byte indicating to ack or to nack + * Output none + */ +void twi_reply(uint8_t ack) +{ + // transmit master read ready signal, with or without ack + if(ack){ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + }else{ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + } +} + +/* + * Function twi_stop + * Desc relinquishes bus master status + * Input none + * Output none + */ +void twi_stop(void) +{ + // send stop condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); + + // wait for stop condition to be exectued on bus + // TWINT is not set after a stop condition! + while(TWCR & _BV(TWSTO)){ + continue; + } + + // update twi state + twi_state = TWI_READY; +} + +/* + * Function twi_releaseBus + * Desc releases bus control + * Input none + * Output none + */ +void twi_releaseBus(void) +{ + // release bus + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); + + // update twi state + twi_state = TWI_READY; +} + +ISR(TWI_vect) +{ + switch(TW_STATUS){ + // All Master + case TW_START: // sent start condition + case TW_REP_START: // sent repeated start condition + // copy device address and r/w bit to output register and ack + TWDR = twi_slarw; + twi_reply(1); + break; + + // Master Transmitter + case TW_MT_SLA_ACK: // slave receiver acked address + case TW_MT_DATA_ACK: // slave receiver acked data + // if there is data to send, send it, otherwise stop + if(twi_masterBufferIndex < twi_masterBufferLength){ + // copy data to output register and ack + TWDR = twi_masterBuffer[twi_masterBufferIndex++]; + twi_reply(1); + }else{ + if (twi_sendStop) + twi_stop(); + else { + twi_inRepStart = true; // we're gonna send the START + // don't enable the interrupt. We'll generate the start, but we + // avoid handling the interrupt until we're in the next transaction, + // at the point where we would normally issue the start. + TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ; + twi_state = TWI_READY; + } + } + break; + case TW_MT_SLA_NACK: // address sent, nack received + twi_error = TW_MT_SLA_NACK; + twi_stop(); + break; + case TW_MT_DATA_NACK: // data sent, nack received + twi_error = TW_MT_DATA_NACK; + twi_stop(); + break; + case TW_MT_ARB_LOST: // lost bus arbitration + twi_error = TW_MT_ARB_LOST; + twi_releaseBus(); + break; + + // Master Receiver + case TW_MR_DATA_ACK: // data received, ack sent + // put byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + case TW_MR_SLA_ACK: // address sent, ack received + // ack if more bytes are expected, otherwise nack + if(twi_masterBufferIndex < twi_masterBufferLength){ + twi_reply(1); + }else{ + twi_reply(0); + } + break; + case TW_MR_DATA_NACK: // data received, nack sent + // put final byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + if (twi_sendStop) + twi_stop(); + else { + twi_inRepStart = true; // we're gonna send the START + // don't enable the interrupt. We'll generate the start, but we + // avoid handling the interrupt until we're in the next transaction, + // at the point where we would normally issue the start. + TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ; + twi_state = TWI_READY; + } + break; + case TW_MR_SLA_NACK: // address sent, nack received + twi_stop(); + break; + // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case + + // Slave Receiver + case TW_SR_SLA_ACK: // addressed, returned ack + case TW_SR_GCALL_ACK: // addressed generally, returned ack + case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack + case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack + // enter slave receiver mode + twi_state = TWI_SRX; + // indicate that rx buffer can be overwritten and ack + twi_rxBufferIndex = 0; + twi_reply(1); + break; + case TW_SR_DATA_ACK: // data received, returned ack + case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack + // if there is still room in the rx buffer + if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){ + // put byte in buffer and ack + twi_rxBuffer[twi_rxBufferIndex++] = TWDR; + twi_reply(1); + }else{ + // otherwise nack + twi_reply(0); + } + break; + case TW_SR_STOP: // stop or repeated start condition received + // ack future responses and leave slave receiver state + twi_releaseBus(); + // put a null char after data if there's room + if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){ + twi_rxBuffer[twi_rxBufferIndex] = '\0'; + } + // callback to user defined callback + twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); + // since we submit rx buffer to "wire" library, we can reset it + twi_rxBufferIndex = 0; + break; + case TW_SR_DATA_NACK: // data received, returned nack + case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack + // nack back at master + twi_reply(0); + break; + + // Slave Transmitter + case TW_ST_SLA_ACK: // addressed, returned ack + case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack + // enter slave transmitter mode + twi_state = TWI_STX; + // ready the tx buffer index for iteration + twi_txBufferIndex = 0; + // set tx buffer length to be zero, to verify if user changes it + twi_txBufferLength = 0; + // request for txBuffer to be filled and length to be set + // note: user must call twi_transmit(bytes, length) to do this + twi_onSlaveTransmit(); + // if they didn't change buffer & length, initialize it + if(0 == twi_txBufferLength){ + twi_txBufferLength = 1; + twi_txBuffer[0] = 0x00; + } + // transmit first byte from buffer, fall + case TW_ST_DATA_ACK: // byte sent, ack returned + // copy data to output register + TWDR = twi_txBuffer[twi_txBufferIndex++]; + // if there is more to send, ack, otherwise nack + if(twi_txBufferIndex < twi_txBufferLength){ + twi_reply(1); + }else{ + twi_reply(0); + } + break; + case TW_ST_DATA_NACK: // received nack, we are done + case TW_ST_LAST_DATA: // received ack, but we are done already! + // ack future responses + twi_reply(1); + // leave slave receiver state + twi_state = TWI_READY; + break; + + // All + case TW_NO_INFO: // no state information + break; + case TW_BUS_ERROR: // bus error, illegal stop/start + twi_error = TW_BUS_ERROR; + twi_stop(); + break; + } +} + diff --git a/control/ctrl/twi.h b/control/ctrl/twi.h new file mode 100644 index 0000000..d27325e --- /dev/null +++ b/control/ctrl/twi.h @@ -0,0 +1,55 @@ +/* + twi.h - TWI/I2C library for Wiring & Arduino + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef twi_h +#define twi_h + + #include + + //#define ATMEGA8 + + #ifndef TWI_FREQ + #define TWI_FREQ 100000L + #endif + + #ifndef TWI_BUFFER_LENGTH + #define TWI_BUFFER_LENGTH 32 + #endif + + #define TWI_READY 0 + #define TWI_MRX 1 + #define TWI_MTX 2 + #define TWI_SRX 3 + #define TWI_STX 4 + + void twi_init(void); + void twi_disable(void); + void twi_setAddress(uint8_t); + void twi_setFrequency(uint32_t); + uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t); + uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t); + uint8_t twi_transmit(const uint8_t*, uint8_t); + void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) ); + void twi_attachSlaveTxEvent( void (*)(void) ); + void twi_reply(uint8_t); + void twi_stop(void); + void twi_releaseBus(void); + +#endif + diff --git a/control/tiny/Makefile b/control/tiny/Makefile new file mode 100644 index 0000000..8ba677b --- /dev/null +++ b/control/tiny/Makefile @@ -0,0 +1,28 @@ +ifndef TTY +TTY=/dev/ttyACM0 +endif + +ifndef TARGET +TARGET=blink +endif + +MCU=attiny85 +AVRDUDEMCU=t85 +CC=/usr/bin/avr-gcc +CFLAGS=-g -Os -Wall -mcall-prologues -mmcu=$(MCU) +OBJ2HEX=/usr/bin/avr-objcopy +AVRDUDE=avrdude + +# See http://www.engbedded.com/fusecalc for fuse settings +# /usr/bin/avrdude -C/etc/avrdude/avrdude.conf -v -pattiny85 -cstk500v1 -P/dev/ttyACM0 -b19200 -Uflash:w:/tmp/arduino_build_108059/i2c.ino.hex:i +# + +all: + $(CC) $(CFLAGS) $(TARGET).c usiTwiSlave.c -o$(TARGET) + $(OBJ2HEX) -R .eeprom -O ihex $(TARGET) $(TARGET).hex + +burn: + $(AVRDUDE) -p $(MCU) -P $(TTY) -C/etc/avrdude/avrdude.conf -v -c avrisp -b 19200 -U flash:w:$(TARGET).hex -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -U efuse:w:0xff:m +clean: + rm -f *.hex *.obj *.o + diff --git a/control/tiny/i2c_timer_pwm.c b/control/tiny/i2c_timer_pwm.c new file mode 100644 index 0000000..689d0af --- /dev/null +++ b/control/tiny/i2c_timer_pwm.c @@ -0,0 +1,177 @@ +// This program acts as the device (slave) for the control program i2c/a2a/c_ctl +#define F_CPU 8000000L +#include +#include +#include +#include + +#include "usiTwiSlave.h" + +#define I2C_SLAVE_ADDRESS 0x8 // the 7-bit address (remember to change this when adapting this example) + +enum +{ + kCS13_10_idx = 0, // Timer 1 Prescalar (CS13,CS12,CS11,CS10) from Table 12-5 pg 89 + kOCR1C_idx = 1, // OCR1C timer match value + +}; + +volatile uint8_t ctl_regs[] = +{ + 0x0f, // clk/16384 + 244, // OCR1C +}; + +// Tracks the current register pointer position +volatile uint8_t reg_position = 0; +const uint8_t reg_size = sizeof(ctl_regs); + +ISR(TIMER1_OVF_vect) +{ + PINB = _BV(PINB4); // writes to PINB toggle the pins +} + +void timer_init() +{ + TIMSK &= ~_BV(TOIE1); // Disable interrupt TIMER1_OVF + OCR1A = 255; // Set to anything greater than OCR1C (the counter never gets here.) + TCCR1 |= _BV(CTC1); // Reset TCNT1 to 0 when TCNT1==OCR1C + TCCR1 |= _BV(PWM1A); // Enable PWM A + TCCR1 |= ctl_regs[kCS13_10_idx] & 0x0f; + OCR1C = ctl_regs[kOCR1C_idx]; + TIMSK |= _BV(TOIE1); // Enable interrupt TIMER1_OVF +} + + + +/** + * This is called for each read request we receive, never put more + * than one byte of data (with TinyWireS.send) to the send-buffer when + * using this callback + */ +void on_request() +{ + // read and transmit the requestd position + usiTwiTransmitByte(ctl_regs[reg_position]); + + + // Increment the reg position on each read, and loop back to zero + reg_position++; + if (reg_position >= reg_size) + { + reg_position = 0; + } + +} + + +/** + * The I2C data received -handler + * + * This needs to complete before the next incoming transaction (start, + * data, restart/stop) on the bus does so be quick, set flags for long + * running tasks to be called from the mainloop instead of running + * them directly, + */ + +void on_receive( uint8_t howMany ) +{ + if (howMany < 1) + { + // Sanity-check + return; + } + if (howMany > TWI_RX_BUFFER_SIZE) + { + // Also insane number + return; + } + + // get the register index to read/write + reg_position = usiTwiReceiveByte(); + + howMany--; + + // If only one byte was received then this was a read request + // and the buffer pointer (reg_position) is now set to return the byte + // at this location on the subsequent call to on_request() ... + if (!howMany) + { + return; + } + + // ... otherwise this was a write request and the buffer + // pointer is now pointing to the first byte to write to + while(howMany--) + { + ctl_regs[reg_position] = usiTwiReceiveByte(); + + if(reg_position == kCS13_10_idx || reg_position == kOCR1C_idx ) + timer_init(); + + reg_position++; + if (reg_position >= reg_size) + { + reg_position = 0; + } + } + +} + + + +int main(void) +{ + cli(); // mask all interupts + + DDRB |= _BV(DDB4); // setup PB4 as output + PORTB &= ~_BV(PINB4); + + timer_init(); + + // setup i2c library + usi_onReceiverPtr = on_receive; //on_receive; + usi_onRequestPtr = on_request; + usiTwiSlaveInit(I2C_SLAVE_ADDRESS); + + sei(); + + PINB = _BV(PINB4); // writes to PINB toggle the pins + _delay_ms(1000); + PINB = _BV(PINB4); // writes to PINB toggle the pins + + + while(1) + { + //_delay_ms(1000); + + if (!usi_onReceiverPtr) + { + // no onReceive callback, nothing to do... + continue; + } + + if (!(USISR & ( 1 << USIPF ))) + { + // Stop not detected + continue; + } + + + uint8_t amount = usiTwiAmountDataInReceiveBuffer(); + if (amount == 0) + { + // no data in buffer + continue; + } + + + usi_onReceiverPtr(amount); + + + } + return 0; +} + + + diff --git a/control/tiny/usiTwiSlave.c b/control/tiny/usiTwiSlave.c new file mode 100644 index 0000000..b26f697 --- /dev/null +++ b/control/tiny/usiTwiSlave.c @@ -0,0 +1,660 @@ +/******************************************************************************** + +USI TWI Slave driver. + +Created by Donald R. Blake. donblake at worldnet.att.net +Adapted by Jochen Toppe, jochen.toppe at jtoee.com + +--------------------------------------------------------------------------------- + +Created from Atmel source files for Application Note AVR312: Using the USI Module +as an I2C slave. + +This program is free software; you can redistribute it and/or modify it under the +terms of the GNU General Public License as published by the Free Software +Foundation; either version 2 of the License, or (at your option) any later +version. + +This program is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A +PARTICULAR PURPOSE. See the GNU General Public License for more details. + +--------------------------------------------------------------------------------- + +Change Activity: + + Date Description + ------ ------------- + 16 Mar 2007 Created. + 27 Mar 2007 Added support for ATtiny261, 461 and 861. + 26 Apr 2007 Fixed ACK of slave address on a read. + 04 Jul 2007 Fixed USISIF in ATtiny45 def + 12 Dev 2009 Added callback functions for data requests + 06 Feb 2015 Minor change to allow mutli-byte requestFrom() from master. + 10 Feb 2015 Simplied RX/TX buffer code and allowed use of full buffer. + 12 Dec 2016 Added support for ATtiny167 + +********************************************************************************/ + + +/******************************************************************************** + includes +********************************************************************************/ + +#include +#include + +#include "usiTwiSlave.h" +//#include "../common/util.h" + + +/******************************************************************************** + device dependent defines +********************************************************************************/ + +#if defined( __AVR_ATtiny167__ ) +# define DDR_USI DDRB +# define PORT_USI PORTB +# define PIN_USI PINB +# define PORT_USI_SDA PB0 +# define PORT_USI_SCL PB2 +# define PIN_USI_SDA PINB0 +# define PIN_USI_SCL PINB2 +# define USI_START_COND_INT USISIF +# define USI_START_VECTOR USI_START_vect +# define USI_OVERFLOW_VECTOR USI_OVERFLOW_vect +#endif + +#if defined( __AVR_ATtiny2313__ ) +# define DDR_USI DDRB +# define PORT_USI PORTB +# define PIN_USI PINB +# define PORT_USI_SDA PB5 +# define PORT_USI_SCL PB7 +# define PIN_USI_SDA PINB5 +# define PIN_USI_SCL PINB7 +# define USI_START_COND_INT USISIF +# define USI_START_VECTOR USI_START_vect +# define USI_OVERFLOW_VECTOR USI_OVERFLOW_vect +#endif + +#if defined(__AVR_ATtiny84__) | \ + defined(__AVR_ATtiny44__) +# define DDR_USI DDRA +# define PORT_USI PORTA +# define PIN_USI PINA +# define PORT_USI_SDA PORTA6 +# define PORT_USI_SCL PORTA4 +# define PIN_USI_SDA PINA6 +# define PIN_USI_SCL PINA4 +# define USI_START_COND_INT USISIF +# define USI_START_VECTOR USI_START_vect +# define USI_OVERFLOW_VECTOR USI_OVF_vect +#endif + +#if defined( __AVR_ATtiny25__ ) | \ + defined( __AVR_ATtiny45__ ) | \ + defined( __AVR_ATtiny85__ ) +# define DDR_USI DDRB +# define PORT_USI PORTB +# define PIN_USI PINB +# define PORT_USI_SDA PB0 +# define PORT_USI_SCL PB2 +# define PIN_USI_SDA PINB0 +# define PIN_USI_SCL PINB2 +# define USI_START_COND_INT USISIF +# define USI_START_VECTOR USI_START_vect +# define USI_OVERFLOW_VECTOR USI_OVF_vect +#endif + +#if defined( __AVR_ATtiny26__ ) +# define DDR_USI DDRB +# define PORT_USI PORTB +# define PIN_USI PINB +# define PORT_USI_SDA PB0 +# define PORT_USI_SCL PB2 +# define PIN_USI_SDA PINB0 +# define PIN_USI_SCL PINB2 +# define USI_START_COND_INT USISIF +# define USI_START_VECTOR USI_STRT_vect +# define USI_OVERFLOW_VECTOR USI_OVF_vect +#endif + +#if defined( __AVR_ATtiny261__ ) | \ + defined( __AVR_ATtiny461__ ) | \ + defined( __AVR_ATtiny861__ ) +# define DDR_USI DDRB +# define PORT_USI PORTB +# define PIN_USI PINB +# define PORT_USI_SDA PB0 +# define PORT_USI_SCL PB2 +# define PIN_USI_SDA PINB0 +# define PIN_USI_SCL PINB2 +# define USI_START_COND_INT USISIF +# define USI_START_VECTOR USI_START_vect +# define USI_OVERFLOW_VECTOR USI_OVF_vect +#endif + +#if defined( __AVR_ATmega165__ ) | \ + defined( __AVR_ATmega325__ ) | \ + defined( __AVR_ATmega3250__ ) | \ + defined( __AVR_ATmega645__ ) | \ + defined( __AVR_ATmega6450__ ) | \ + defined( __AVR_ATmega329__ ) | \ + defined( __AVR_ATmega3290__ ) +# define DDR_USI DDRE +# define PORT_USI PORTE +# define PIN_USI PINE +# define PORT_USI_SDA PE5 +# define PORT_USI_SCL PE4 +# define PIN_USI_SDA PINE5 +# define PIN_USI_SCL PINE4 +# define USI_START_COND_INT USISIF +# define USI_START_VECTOR USI_START_vect +# define USI_OVERFLOW_VECTOR USI_OVERFLOW_vect +#endif + +#if defined( __AVR_ATmega169__ ) +# define DDR_USI DDRE +# define PORT_USI PORTE +# define PIN_USI PINE +# define PORT_USI_SDA PE5 +# define PORT_USI_SCL PE4 +# define PIN_USI_SDA PINE5 +# define PIN_USI_SCL PINE4 +# define USI_START_COND_INT USISIF +# define USI_START_VECTOR USI_START_vect +# define USI_OVERFLOW_VECTOR USI_OVERFLOW_vect +#endif + + + +/******************************************************************************** + + functions implemented as macros + +********************************************************************************/ + +#define SET_USI_TO_SEND_ACK( ) \ +{ \ + /* prepare ACK */ \ + USIDR = 0; \ + /* set SDA as output */ \ + DDR_USI |= ( 1 << PORT_USI_SDA ); \ + /* clear all interrupt flags, except Start Cond */ \ + USISR = \ + ( 0 << USI_START_COND_INT ) | \ + ( 1 << USIOIF ) | ( 1 << USIPF ) | \ + ( 1 << USIDC )| \ + /* set USI counter to shift 1 bit */ \ + ( 0x0E << USICNT0 ); \ +} + +#define SET_USI_TO_READ_ACK( ) \ +{ \ + /* set SDA as input */ \ + DDR_USI &= ~( 1 << PORT_USI_SDA ); \ + /* prepare ACK */ \ + USIDR = 0; \ + /* clear all interrupt flags, except Start Cond */ \ + USISR = \ + ( 0 << USI_START_COND_INT ) | \ + ( 1 << USIOIF ) | \ + ( 1 << USIPF ) | \ + ( 1 << USIDC ) | \ + /* set USI counter to shift 1 bit */ \ + ( 0x0E << USICNT0 ); \ +} + +#define SET_USI_TO_TWI_START_CONDITION_MODE( ) \ +{ \ + USICR = \ + /* enable Start Condition Interrupt, disable Overflow Interrupt */ \ + ( 1 << USISIE ) | ( 0 << USIOIE ) | \ + /* set USI in Two-wire mode, no USI Counter overflow hold */ \ + ( 1 << USIWM1 ) | ( 0 << USIWM0 ) | \ + /* Shift Register Clock Source = External, positive edge */ \ + /* 4-Bit Counter Source = external, both edges */ \ + ( 1 << USICS1 ) | ( 0 << USICS0 ) | ( 0 << USICLK ) | \ + /* no toggle clock-port pin */ \ + ( 0 << USITC ); \ + USISR = \ + /* clear all interrupt flags, except Start Cond */ \ + ( 0 << USI_START_COND_INT ) | ( 1 << USIOIF ) | ( 1 << USIPF ) | \ + ( 1 << USIDC ) | ( 0x0 << USICNT0 ); \ +} + +#define SET_USI_TO_SEND_DATA( ) \ +{ \ + /* set SDA as output */ \ + DDR_USI |= ( 1 << PORT_USI_SDA ); \ + /* clear all interrupt flags, except Start Cond */ \ + USISR = \ + ( 0 << USI_START_COND_INT ) | ( 1 << USIOIF ) | ( 1 << USIPF ) | \ + ( 1 << USIDC) | \ + /* set USI to shift out 8 bits */ \ + ( 0x0 << USICNT0 ); \ +} + +#define SET_USI_TO_READ_DATA( ) \ +{ \ + /* set SDA as input */ \ + DDR_USI &= ~( 1 << PORT_USI_SDA ); \ + /* clear all interrupt flags, except Start Cond */ \ + USISR = \ + ( 0 << USI_START_COND_INT ) | ( 1 << USIOIF ) | \ + ( 1 << USIPF ) | ( 1 << USIDC ) | \ + /* set USI to shift out 8 bits */ \ + ( 0x0 << USICNT0 ); \ +} + +#define USI_RECEIVE_CALLBACK() \ +{ \ + if (usi_onReceiverPtr) \ + { \ + if (usiTwiAmountDataInReceiveBuffer()) \ + { \ + usi_onReceiverPtr(usiTwiAmountDataInReceiveBuffer()); \ + } \ + } \ +} + +#define ONSTOP_USI_RECEIVE_CALLBACK() \ +{ \ + if (USISR & ( 1 << USIPF )) \ + { \ + USI_RECEIVE_CALLBACK(); \ + } \ +} + + +#define USI_REQUEST_CALLBACK() \ +{ \ + USI_RECEIVE_CALLBACK(); \ + if(usi_onRequestPtr) usi_onRequestPtr(); \ +} + +/******************************************************************************** + + typedef's + +********************************************************************************/ + +typedef enum +{ + USI_SLAVE_CHECK_ADDRESS = 0x00, + USI_SLAVE_SEND_DATA = 0x01, + USI_SLAVE_REQUEST_REPLY_FROM_SEND_DATA = 0x02, + USI_SLAVE_CHECK_REPLY_FROM_SEND_DATA = 0x03, + USI_SLAVE_REQUEST_DATA = 0x04, + USI_SLAVE_GET_DATA_AND_SEND_ACK = 0x05 +} overflowState_t; + + + +/******************************************************************************** + + local variables + +********************************************************************************/ + +static uint8_t slaveAddress; +static volatile overflowState_t overflowState; + + +static uint8_t rxBuf[ TWI_RX_BUFFER_SIZE ]; +static volatile uint8_t rxHead; +static volatile uint8_t rxTail; +static volatile uint8_t rxCount; + +static uint8_t txBuf[ TWI_TX_BUFFER_SIZE ]; +static volatile uint8_t txHead; +static volatile uint8_t txTail; +static volatile uint8_t txCount; + + + +/******************************************************************************** + + local functions + +********************************************************************************/ + + + +// flushes the TWI buffers + +static +void +flushTwiBuffers( + void +) +{ + rxTail = 0; + rxHead = 0; + rxCount = 0; + txTail = 0; + txHead = 0; + txCount = 0; +} // end flushTwiBuffers + + + +/******************************************************************************** + + public functions + +********************************************************************************/ + + + +// initialise USI for TWI slave mode + +void +usiTwiSlaveInit( + uint8_t ownAddress +) +{ + + flushTwiBuffers( ); + + slaveAddress = ownAddress; + + // In Two Wire mode (USIWM1, USIWM0 = 1X), the slave USI will pull SCL + // low when a start condition is detected or a counter overflow (only + // for USIWM1, USIWM0 = 11). This inserts a wait state. SCL is released + // by the ISRs (USI_START_vect and USI_OVERFLOW_vect). + + // Set SCL and SDA as output + DDR_USI |= ( 1 << PORT_USI_SCL ) | ( 1 << PORT_USI_SDA ); + + // set SCL high + PORT_USI |= ( 1 << PORT_USI_SCL ); + + // set SDA high + PORT_USI |= ( 1 << PORT_USI_SDA ); + + // Set SDA as input + DDR_USI &= ~( 1 << PORT_USI_SDA ); + + USICR = + // enable Start Condition Interrupt + ( 1 << USISIE ) | + // disable Overflow Interrupt + ( 0 << USIOIE ) | + // set USI in Two-wire mode, no USI Counter overflow hold + ( 1 << USIWM1 ) | ( 0 << USIWM0 ) | + // Shift Register Clock Source = external, positive edge + // 4-Bit Counter Source = external, both edges + ( 1 << USICS1 ) | ( 0 << USICS0 ) | ( 0 << USICLK ) | + // no toggle clock-port pin + ( 0 << USITC ); + + // clear all interrupt flags and reset overflow counter + + USISR = ( 1 << USI_START_COND_INT ) | ( 1 << USIOIF ) | ( 1 << USIPF ) | ( 1 << USIDC ); + +} // end usiTwiSlaveInit + + +bool usiTwiDataInTransmitBuffer(void) +{ + + // return 0 (false) if the receive buffer is empty + return txCount; + +} // end usiTwiDataInTransmitBuffer + + +// put data in the transmission buffer, wait if buffer is full + +void +usiTwiTransmitByte( + uint8_t data +) +{ + + // kpl uint8_t tmphead; + + // wait for free space in buffer + while ( txCount == TWI_TX_BUFFER_SIZE) ; + + // store data in buffer + txBuf[ txHead ] = data; + txHead = ( txHead + 1 ) & TWI_TX_BUFFER_MASK; + txCount++; + +} // end usiTwiTransmitByte + + + + + +// return a byte from the receive buffer, wait if buffer is empty + +uint8_t +usiTwiReceiveByte( + void +) +{ + uint8_t rtn_byte; + + // wait for Rx data + while ( !rxCount ); + + rtn_byte = rxBuf [ rxTail ]; + // calculate buffer index + rxTail = ( rxTail + 1 ) & TWI_RX_BUFFER_MASK; + rxCount--; + + // return data from the buffer. + return rtn_byte; + +} // end usiTwiReceiveByte + + + +uint8_t usiTwiAmountDataInReceiveBuffer(void) +{ + return rxCount; +} + + + + +/******************************************************************************** + + USI Start Condition ISR + +********************************************************************************/ + +ISR( USI_START_VECTOR ) +{ + + /* + // This triggers on second write, but claims to the callback there is only *one* byte in buffer + ONSTOP_USI_RECEIVE_CALLBACK(); + */ + /* + // This triggers on second write, but claims to the callback there is only *one* byte in buffer + USI_RECEIVE_CALLBACK(); + */ + + + // set default starting conditions for new TWI package + overflowState = USI_SLAVE_CHECK_ADDRESS; + + // set SDA as input + DDR_USI &= ~( 1 << PORT_USI_SDA ); + + // wait for SCL to go low to ensure the Start Condition has completed (the + // start detector will hold SCL low ) - if a Stop Condition arises then leave + // the interrupt to prevent waiting forever - don't use USISR to test for Stop + // Condition as in Application Note AVR312 because the Stop Condition Flag is + // going to be set from the last TWI sequence + while ( + // SCL his high + ( PIN_USI & ( 1 << PIN_USI_SCL ) ) && + // and SDA is low + !( ( PIN_USI & ( 1 << PIN_USI_SDA ) ) ) + ); + + + if ( !( PIN_USI & ( 1 << PIN_USI_SDA ) ) ) + { + + // a Stop Condition did not occur + + USICR = + // keep Start Condition Interrupt enabled to detect RESTART + ( 1 << USISIE ) | + // enable Overflow Interrupt + ( 1 << USIOIE ) | + // set USI in Two-wire mode, hold SCL low on USI Counter overflow + ( 1 << USIWM1 ) | ( 1 << USIWM0 ) | + // Shift Register Clock Source = External, positive edge + // 4-Bit Counter Source = external, both edges + ( 1 << USICS1 ) | ( 0 << USICS0 ) | ( 0 << USICLK ) | + // no toggle clock-port pin + ( 0 << USITC ); + + } + else + { + // a Stop Condition did occur + + USICR = + // enable Start Condition Interrupt + ( 1 << USISIE ) | + // disable Overflow Interrupt + ( 0 << USIOIE ) | + // set USI in Two-wire mode, no USI Counter overflow hold + ( 1 << USIWM1 ) | ( 0 << USIWM0 ) | + // Shift Register Clock Source = external, positive edge + // 4-Bit Counter Source = external, both edges + ( 1 << USICS1 ) | ( 0 << USICS0 ) | ( 0 << USICLK ) | + // no toggle clock-port pin + ( 0 << USITC ); + + } // end if + + USISR = + // clear interrupt flags - resetting the Start Condition Flag will + // release SCL + ( 1 << USI_START_COND_INT ) | ( 1 << USIOIF ) | + ( 1 << USIPF ) |( 1 << USIDC ) | + // set USI to sample 8 bits (count 16 external SCL pin toggles) + ( 0x0 << USICNT0); + + +} // end ISR( USI_START_VECTOR ) + + + +/******************************************************************************** + + USI Overflow ISR + +Handles all the communication. + +Only disabled when waiting for a new Start Condition. + +********************************************************************************/ + +ISR( USI_OVERFLOW_VECTOR ) +{ + + switch ( overflowState ) + { + + // Address mode: check address and send ACK (and next USI_SLAVE_SEND_DATA) if OK, + // else reset USI + case USI_SLAVE_CHECK_ADDRESS: + if ( ( USIDR == 0 ) || ( ( USIDR >> 1 ) == slaveAddress) ) + { + if ( USIDR & 0x01 ) + { + USI_REQUEST_CALLBACK(); + overflowState = USI_SLAVE_SEND_DATA; + } + else + { + overflowState = USI_SLAVE_REQUEST_DATA; + } // end if + SET_USI_TO_SEND_ACK( ); + } + else + { + SET_USI_TO_TWI_START_CONDITION_MODE( ); + } + break; + + // Master write data mode: check reply and goto USI_SLAVE_SEND_DATA if OK, + // else reset USI + case USI_SLAVE_CHECK_REPLY_FROM_SEND_DATA: + if ( USIDR ) + { + // if NACK, the master does not want more data + SET_USI_TO_TWI_START_CONDITION_MODE( ); + return; + } + // from here we just drop straight into USI_SLAVE_SEND_DATA if the + // master sent an ACK + + // copy data from buffer to USIDR and set USI to shift byte + // next USI_SLAVE_REQUEST_REPLY_FROM_SEND_DATA + case USI_SLAVE_SEND_DATA: + // Get data from Buffer + if ( txCount ) + { + USIDR = txBuf[ txTail ]; + txTail = ( txTail + 1 ) & TWI_TX_BUFFER_MASK; + txCount--; + } + else + { + // the buffer is empty + SET_USI_TO_READ_ACK( ); // This might be neccessary sometimes see http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=805227#805227 + SET_USI_TO_TWI_START_CONDITION_MODE( ); + return; + } // end if + overflowState = USI_SLAVE_REQUEST_REPLY_FROM_SEND_DATA; + SET_USI_TO_SEND_DATA( ); + break; + + // set USI to sample reply from master + // next USI_SLAVE_CHECK_REPLY_FROM_SEND_DATA + case USI_SLAVE_REQUEST_REPLY_FROM_SEND_DATA: + overflowState = USI_SLAVE_CHECK_REPLY_FROM_SEND_DATA; + SET_USI_TO_READ_ACK( ); + break; + + // Master read data mode: set USI to sample data from master, next + // USI_SLAVE_GET_DATA_AND_SEND_ACK + case USI_SLAVE_REQUEST_DATA: + overflowState = USI_SLAVE_GET_DATA_AND_SEND_ACK; + SET_USI_TO_READ_DATA( ); + break; + + // copy data from USIDR and send ACK + // next USI_SLAVE_REQUEST_DATA + case USI_SLAVE_GET_DATA_AND_SEND_ACK: + // put data into buffer + // check buffer size + if ( rxCount < TWI_RX_BUFFER_SIZE ) + { + rxBuf[ rxHead ] = USIDR; + rxHead = ( rxHead + 1 ) & TWI_RX_BUFFER_MASK; + rxCount++; + } else { + // overrun + // drop data + } + // next USI_SLAVE_REQUEST_DATA + overflowState = USI_SLAVE_REQUEST_DATA; + SET_USI_TO_SEND_ACK( ); + break; + + } // end switch + +} // end ISR( USI_OVERFLOW_VECTOR ) diff --git a/control/tiny/usiTwiSlave.h b/control/tiny/usiTwiSlave.h new file mode 100644 index 0000000..8ffdda2 --- /dev/null +++ b/control/tiny/usiTwiSlave.h @@ -0,0 +1,97 @@ +/******************************************************************************** + +Header file for the USI TWI Slave driver. + +Created by Donald R. Blake +donblake at worldnet.att.net + +--------------------------------------------------------------------------------- + +Created from Atmel source files for Application Note AVR312: Using the USI Module +as an I2C slave. + +This program is free software; you can redistribute it and/or modify it under the +terms of the GNU General Public License as published by the Free Software +Foundation; either version 2 of the License, or (at your option) any later +version. + +This program is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A +PARTICULAR PURPOSE. See the GNU General Public License for more details. + +--------------------------------------------------------------------------------- + +Change Activity: + + Date Description + ------ ------------- + 15 Mar 2007 Created. + +********************************************************************************/ + + + +#ifndef _USI_TWI_SLAVE_H_ +#define _USI_TWI_SLAVE_H_ + + + +/******************************************************************************** + + includes + +********************************************************************************/ + +#include + + + +/******************************************************************************** + + prototypes + +********************************************************************************/ + +void usiTwiSlaveInit( uint8_t ); +void usiTwiTransmitByte( uint8_t ); +uint8_t usiTwiReceiveByte( void ); +bool usiTwiDataInReceiveBuffer( void ); +void (*_onTwiDataRequest)(void); +bool usiTwiDataInTransmitBuffer(void); +uint8_t usiTwiAmountDataInReceiveBuffer(void); +// on_XXX handler pointers +void (*usi_onRequestPtr)(void); +void (*usi_onReceiverPtr)(uint8_t); + + +/******************************************************************************** + + driver buffer definitions + +********************************************************************************/ + +// permitted RX buffer sizes: 1, 2, 4, 8, 16, 32, 64, 128 or 256 + +#ifndef TWI_RX_BUFFER_SIZE +#define TWI_RX_BUFFER_SIZE ( 16 ) +#endif +#define TWI_RX_BUFFER_MASK ( TWI_RX_BUFFER_SIZE - 1 ) + +#if ( TWI_RX_BUFFER_SIZE & TWI_RX_BUFFER_MASK ) +# error TWI RX buffer size is not a power of 2 +#endif + +// permitted TX buffer sizes: 1, 2, 4, 8, 16, 32, 64, 128 or 256 + +#ifndef TWI_TX_BUFFER_SIZE +#define TWI_TX_BUFFER_SIZE ( 16 ) +#endif +#define TWI_TX_BUFFER_MASK ( TWI_TX_BUFFER_SIZE - 1 ) + +#if ( TWI_TX_BUFFER_SIZE & TWI_TX_BUFFER_MASK ) +# error TWI TX buffer size is not a power of 2 +#endif + + + +#endif // ifndef _USI_TWI_SLAVE_H_