Subversion Repositories FlightCtrl

Rev

Rev 1322 | Rev 1638 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1322 Rev 1479
Line 4... Line 4...
4 
#include "main.h"
 4 
#include "main.h"
Line 5... Line 5...
5 
 
 5 
 
6 
volatile unsigned char twi_state = 0;
 6 
volatile unsigned char twi_state = 0;
7 
unsigned char motor = 0;
 7 
unsigned char motor = 0;
8 
unsigned char motorread = 0,MissingMotor = 0;
 - 
 
9 
unsigned char motor_rx[16],motor_rx2[16];
 - 
 
- 
 
 8 
unsigned char motorread = 0,MissingMotor = 0;
10 
unsigned char MotorPresent[MAX_MOTORS];
 9 
 
- 
 
 10 
MotorData_t Motor[MAX_MOTORS];
11 
unsigned char MotorError[MAX_MOTORS];
 11 
 
Line 12... Line 12...
12 
unsigned int I2CError = 0;
 12 
unsigned int I2CError = 0;
13 
 
 13 
 
14 
//############################################################################
 14 
//############################################################################
Line 112... Line 112...
112 
                                 i2c_write_byte(0x53+(motorread*2));
 112 
                                 i2c_write_byte(0x53+(motorread*2));
113 
                                 }
 113 
                                 }
114 
                                else i2c_write_byte(0x52+(motor*2));
 114 
                                else i2c_write_byte(0x52+(motor*2));
115 
                break;
 115 
                break;
116 
        case 1:
 116 
        case 1:
117 
                i2c_write_byte(Motor[motor++]);
 117 
                i2c_write_byte(Motor[motor++].SetPoint);
118 
                break;
 118 
                break;
119 
        case 2:
 119 
        case 2:
120 
                if(TWSR == 0x30)
 120 
                if(TWSR == 0x30)
121 
                                 {
 121 
                                 {
122 
                                  if(!missing_motor) missing_motor = motor;
 122 
                                  if(!missing_motor) missing_motor = motor;
123 
                                  if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255;
 123 
                                  if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
124 
                                 }
 124 
                                 }
125 
                i2c_stop();
 125 
                i2c_stop();
126 
                I2CTimeout = 10;
 126 
                I2CTimeout = 10;
127 
                twi_state = 0;
 127 
                twi_state = 0;
128 
                i2c_start();  
 128 
                i2c_start();
Line 132... Line 132...
132 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 132 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
133 
        case 3:
 133 
        case 3:
134 
               //Transmit 1st byte for reading
 134 
               //Transmit 1st byte for reading
135 
                if(TWSR != 0x40)  // Error?
 135 
                if(TWSR != 0x40)  // Error?
136 
                 {
 136 
                 {
137 
                  MotorPresent[motorread] = 0;
 137 
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
138 
                  motorread++;
 138 
                  motorread++;
139 
                  if(motorread >= MAX_MOTORS) motorread = 0;
 139 
                  if(motorread >= MAX_MOTORS) motorread = 0;
140 
                  i2c_stop();
 140 
                  i2c_stop();
141 
                  twi_state = 0;
 141 
                  twi_state = 0;
142 
                 }
 142 
                 }
143 
                 else
 143 
                 else
144 
                 {
 144 
                 {
145 
                  MotorPresent[motorread] = ('1' - '-') + motorread;
 145 
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
146 
                  I2C_ReceiveByte();
 146 
                  I2C_ReceiveByte();
147 
                 }
 147 
                 }
148 
                MissingMotor = missing_motor;
 148 
                MissingMotor = missing_motor;
149 
                missing_motor = 0;
 149 
                missing_motor = 0;
150 
                break;
 150 
                break;
151 
        case 4: //Read 1st byte and transmit 2nd Byte
 151 
        case 4: //Read 1st byte and transmit 2nd Byte
152 
                motor_rx[motorread] = TWDR;
 152 
                Motor[motorread].Current = TWDR;
153 
                I2C_ReceiveLastByte(); //nack
 153 
                I2C_ReceiveLastByte(); //nack
154 
                break;
 154 
                break;
155 
        case 5:
 155 
        case 5:
156 
                //Read 2nd byte
 156 
                //Read 2nd byte
157 
                motor_rx2[motorread++] = TWDR;
 157 
                Motor[motorread].MaxPWM = TWDR;
- 
 
 158 
                                motorread++; // next motor
158 
                if(motorread >= MAX_MOTORS) motorread = 0;
 159 
                if(motorread >= MAX_MOTORS) motorread = 0;
159 
                i2c_stop();
 160 
                i2c_stop();
160 
                twi_state = 0;
 161 
                twi_state = 0;
161 
                break;
 162 
                break;