Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1 | ingob | 1 | /*############################################################################ |
| 2 | ############################################################################*/ |
||
| 3 | |||
| 886 | killagreg | 4 | #include <avr/io.h> |
| 5 | #include <avr/interrupt.h> |
||
| 6 | |||
| 1 | ingob | 7 | #include "main.h" |
| 886 | killagreg | 8 | #include "twimaster.h" |
| 9 | #include "fc.h" |
||
| 936 | killagreg | 10 | #include "analog.h" |
| 1 | ingob | 11 | |
| 936 | killagreg | 12 | volatile uint8_t twi_state = 0; |
| 13 | volatile uint8_t motor_write = 0; |
||
| 14 | volatile uint8_t motor_read = 0; |
||
| 15 | volatile uint8_t dac_channel = 0; |
||
| 886 | killagreg | 16 | volatile uint8_t motor_rx[8]; |
| 936 | killagreg | 17 | volatile uint16_t I2CTimeout = 100; |
| 1 | ingob | 18 | |
| 936 | killagreg | 19 | |
| 20 | #define SCL_CLOCK 200000L |
||
| 21 | #define I2C_TIMEOUT 30000 |
||
| 22 | |||
| 23 | #define TWSR_STATUS_MASK 0xF8 |
||
| 24 | // for Master Transmitter Mode |
||
| 25 | |||
| 26 | #define I2C_STATUS_START 0x08 |
||
| 27 | #define I2C_STATUS_REPEATSTART 0x10 |
||
| 28 | #define I2C_STATUS_TX_SLA_ACK 0x18 |
||
| 29 | #define I2C_STATUS_SLAW_NOACK 0x20 |
||
| 30 | #define I2C_STATUS_TX_DATA_ACK 0x28 |
||
| 31 | #define I2C_STATUS_TX_DATA_NOTACK 0x30 |
||
| 32 | #define I2C_STATUS_RX_DATA_ACK 0x50 |
||
| 33 | #define I2C_STATUS_RX_DATA_NOTACK 0x58 |
||
| 34 | |||
| 886 | killagreg | 35 | /**************************************************/ |
| 36 | /* Initialize I2C (TWI) */ |
||
| 37 | /**************************************************/ |
||
| 38 | void I2C_Init(void) |
||
| 1 | ingob | 39 | { |
| 886 | killagreg | 40 | uint8_t sreg = SREG; |
| 41 | cli(); |
||
| 42 | |||
| 43 | // SDA is INPUT |
||
| 44 | DDRC &= ~(1<<DDC1); |
||
| 45 | // SCL is output |
||
| 46 | DDRC |= (1<<DDC0); |
||
| 47 | // pull up SDA |
||
| 48 | PORTC |= (1<<PORTC0)|(1<<PORTC1); |
||
| 49 | |||
| 50 | // TWI Status Register |
||
| 51 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
||
| 52 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
||
| 53 | |||
| 54 | // set TWI Bit Rate Register |
||
| 55 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
||
| 56 | |||
| 936 | killagreg | 57 | twi_state = 0; |
| 58 | motor_write = 0; |
||
| 59 | motor_read = 0; |
||
| 60 | |||
| 886 | killagreg | 61 | SREG = sreg; |
| 1 | ingob | 62 | } |
| 63 | |||
| 886 | killagreg | 64 | /****************************************/ |
| 65 | /* Start I2C */ |
||
| 66 | /****************************************/ |
||
| 67 | void I2C_Start(void) |
||
| 1 | ingob | 68 | { |
| 886 | killagreg | 69 | // TWI Control Register |
| 70 | // clear TWI interrupt flag (TWINT=1) |
||
| 71 | // disable TWI Acknowledge Bit (TWEA = 0) |
||
| 72 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
||
| 73 | // disable TWI STOP Condition Bit (TWSTO = 0) |
||
| 74 | // disable TWI Write Collision Flag (TWWC = 0) |
||
| 936 | killagreg | 75 | // enable i2c (TWEN = 1) |
| 886 | killagreg | 76 | // enable TWI Interrupt (TWIE = 1) |
| 77 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
||
| 1 | ingob | 78 | } |
| 79 | |||
| 886 | killagreg | 80 | /****************************************/ |
| 81 | /* Stop I2C */ |
||
| 82 | /****************************************/ |
||
| 83 | void I2C_Stop(void) |
||
| 1 | ingob | 84 | { |
| 886 | killagreg | 85 | // TWI Control Register |
| 86 | // clear TWI interrupt flag (TWINT=1) |
||
| 87 | // disable TWI Acknowledge Bit (TWEA = 0) |
||
| 88 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
||
| 89 | // enable TWI STOP Condition Bit (TWSTO = 1) |
||
| 90 | // disable TWI Write Collision Flag (TWWC = 0) |
||
| 936 | killagreg | 91 | // enable i2c (TWEN = 1) |
| 886 | killagreg | 92 | // disable TWI Interrupt (TWIE = 0) |
| 93 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
||
| 1 | ingob | 94 | } |
| 95 | |||
| 173 | holgerb | 96 | |
| 886 | killagreg | 97 | /****************************************/ |
| 98 | /* Write to I2C */ |
||
| 99 | /****************************************/ |
||
| 100 | void I2C_WriteByte(int8_t byte) |
||
| 101 | { |
||
| 102 | // move byte to send into TWI Data Register |
||
| 1 | ingob | 103 | TWDR = byte; |
| 886 | killagreg | 104 | // clear interrupt flag (TWINT = 1) |
| 105 | // enable i2c bus (TWEN = 1) |
||
| 936 | killagreg | 106 | // enable interrupt (TWIE = 1) |
| 1 | ingob | 107 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| 108 | } |
||
| 109 | |||
| 886 | killagreg | 110 | |
| 111 | /****************************************/ |
||
| 112 | /* Receive byte and send ACK */ |
||
| 113 | /****************************************/ |
||
| 114 | void I2C_ReceiveByte(void) |
||
| 1 | ingob | 115 | { |
| 886 | killagreg | 116 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
| 117 | } |
||
| 118 | |||
| 119 | /****************************************/ |
||
| 120 | /* I2C receive last byte and send no ACK*/ |
||
| 121 | /****************************************/ |
||
| 122 | void I2C_ReceiveLastByte(void) |
||
| 123 | { |
||
| 124 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
||
| 125 | } |
||
| 126 | |||
| 127 | |||
| 128 | /****************************************/ |
||
| 936 | killagreg | 129 | /* Reset I2C */ |
| 130 | /****************************************/ |
||
| 131 | void I2C_Reset(void) |
||
| 132 | { |
||
| 133 | // stop i2c bus |
||
| 134 | I2C_Stop(); |
||
| 135 | twi_state = 0; |
||
| 136 | motor_write = TWDR; |
||
| 137 | motor_write = 0; |
||
| 138 | motor_read = 0; |
||
| 139 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
||
| 140 | TWAMR = 0; |
||
| 141 | TWAR = 0; |
||
| 142 | TWDR = 0; |
||
| 143 | TWSR = 0; |
||
| 144 | TWBR = 0; |
||
| 145 | I2C_Init(); |
||
| 146 | I2C_Start(); |
||
| 147 | I2C_WriteByte(0); |
||
| 148 | } |
||
| 149 | |||
| 150 | /****************************************/ |
||
| 886 | killagreg | 151 | /* I2C ISR */ |
| 152 | /****************************************/ |
||
| 153 | ISR (TWI_vect) |
||
| 154 | |||
| 155 | { |
||
| 156 | |||
| 936 | killagreg | 157 | switch (twi_state++) // First i2c_start from SendMotorData() |
| 158 | { |
||
| 886 | killagreg | 159 | // Master Transmit |
| 160 | case 0: // Send SLA-W |
||
| 936 | killagreg | 161 | I2C_WriteByte(0x52 + (motor_write * 2) ); |
| 1 | ingob | 162 | break; |
| 936 | killagreg | 163 | case 1: // Send Data to Slave |
| 164 | switch(motor_write) |
||
| 165 | { |
||
| 1 | ingob | 166 | case 0: |
| 886 | killagreg | 167 | I2C_WriteByte(Motor_Front); |
| 1 | ingob | 168 | break; |
| 886 | killagreg | 169 | case 1: |
| 170 | I2C_WriteByte(Motor_Rear); |
||
| 1 | ingob | 171 | break; |
| 172 | case 2: |
||
| 886 | killagreg | 173 | I2C_WriteByte(Motor_Right); |
| 1 | ingob | 174 | break; |
| 175 | case 3: |
||
| 886 | killagreg | 176 | I2C_WriteByte(Motor_Left); |
| 1 | ingob | 177 | break; |
| 936 | killagreg | 178 | } |
| 1 | ingob | 179 | break; |
| 936 | killagreg | 180 | case 2: // repeat case 0+1 for all motors |
| 181 | I2C_Stop(); |
||
| 182 | if (motor_write < 3) |
||
| 183 | { |
||
| 184 | motor_write++; // jump to next motor |
||
| 185 | twi_state = 0; // and repeat from state 0 |
||
| 186 | } |
||
| 187 | else |
||
| 188 | { // data to last motor send |
||
| 189 | motor_write = 0; // reset motor write counter |
||
| 190 | } |
||
| 952 | killagreg | 191 | I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
| 1 | ingob | 192 | break; |
| 886 | killagreg | 193 | |
| 194 | // Master Receive |
||
| 195 | case 3: // Send SLA-R |
||
| 936 | killagreg | 196 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
| 886 | killagreg | 197 | break; |
| 1 | ingob | 198 | case 4: |
| 886 | killagreg | 199 | //Transmit 1st byte |
| 200 | I2C_ReceiveByte(); |
||
| 1 | ingob | 201 | break; |
| 886 | killagreg | 202 | case 5: //Read 1st byte and transmit 2nd Byte |
| 936 | killagreg | 203 | motor_rx[motor_read] = TWDR; |
| 886 | killagreg | 204 | I2C_ReceiveLastByte(); |
| 205 | break; |
||
| 206 | case 6: |
||
| 207 | //Read 2nd byte |
||
| 936 | killagreg | 208 | motor_rx[motor_read + 4] = TWDR; |
| 209 | motor_read++; |
||
| 210 | if (motor_read > 3) motor_read = 0; |
||
| 211 | I2C_Stop(); |
||
| 212 | twi_state = 0; |
||
| 213 | I2CTimeout = 10; |
||
| 214 | break; |
||
| 173 | holgerb | 215 | |
| 936 | killagreg | 216 | // Gyro-Offsets |
| 217 | case 7: |
||
| 218 | I2C_WriteByte(0x98); // Address the DAC |
||
| 219 | break; |
||
| 220 | |||
| 221 | case 8: |
||
| 222 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
||
| 223 | break; |
||
| 224 | |||
| 225 | case 9: |
||
| 226 | switch(dac_channel) |
||
| 227 | { |
||
| 228 | case 0: |
||
| 229 | I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A |
||
| 230 | break; |
||
| 231 | case 1: |
||
| 232 | I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B |
||
| 233 | break; |
||
| 234 | case 2: |
||
| 235 | I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C |
||
| 236 | break; |
||
| 237 | } |
||
| 238 | break; |
||
| 239 | |||
| 240 | case 10: |
||
| 241 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
||
| 242 | break; |
||
| 243 | |||
| 244 | case 11: |
||
| 245 | I2C_Stop(); |
||
| 246 | I2CTimeout = 10; |
||
| 247 | // repeat case 7...10 until all DAC Channels are updated |
||
| 248 | if(dac_channel < 2) |
||
| 249 | { |
||
| 250 | dac_channel ++; // jump to next channel |
||
| 251 | twi_state = 7; // and repeat from state 7 |
||
| 252 | I2C_Start(); // start transmission for next channel |
||
| 253 | } |
||
| 254 | else |
||
| 255 | { // data to last motor send |
||
| 256 | dac_channel = 0; // reset dac channel counter |
||
| 257 | twi_state = 0; // reset twi_state |
||
| 258 | } |
||
| 259 | break; |
||
| 260 | |||
| 886 | killagreg | 261 | default: |
| 262 | I2C_Stop(); |
||
| 263 | twi_state = 0; |
||
| 173 | holgerb | 264 | I2CTimeout = 10; |
| 936 | killagreg | 265 | motor_write = 0; |
| 266 | motor_read = 0; |
||
| 267 | } |
||
| 1 | ingob | 268 | } |