Subversion Repositories FlightCtrl

/*############################################################################
############################################################################*/

#include "main.h"

volatile unsigned char twi_state = 0;
unsigned char motor = 0,TransmitBlConfig = 1;
unsigned char motorread = 0,MissingMotor = 0;
unsigned char ReadTemperature = 0;

MotorData_t Motor[MAX_MOTORS];

unsigned int I2CError = 0;

//############################################################################
//Initzialisieren der I2C (TWI) Schnittstelle
void i2c_init(void)
//############################################################################
{
  TWSR = 0;
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
}

void i2c_reset(void)
//############################################################################
{
                 I2C_Stop();
                 twi_state = 0;
                 motor = TWDR;
                 motor = 0;
                 TWCR = 0x80;
                 TWAMR = 0;
                 TWAR = 0;
                 TWDR = 0;
                 TWSR = 0;
                 TWBR = 0;
                 i2c_init();
                 I2C_Start();
                 i2c_write_byte(0);
}

/*
//############################################################################
SIGNAL (TWI_vect)
//############################################################################
{
 static unsigned char missing_motor;
     switch(twi_state++)
        {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Writing the Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 0:
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
                                if(motor == MAX_MOTORS)  // writing finished -> now read
                                {
                                 motor = 0;
                                 twi_state = 3;
                                 i2c_write_byte(0x53+(motorread*2));
                                 }
                                else i2c_write_byte(0x52+(motor*2));
                break;
        case 1:
                i2c_write_byte(Motor[motor++].SetPoint);
                break;
        case 2:
                if(TWSR == 0x30)
                                 {
                                  if(!missing_motor) missing_motor = motor;
                                  if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
                                 }
                I2C_Stop();
                I2CTimeout = 10;
                twi_state = 0;
                I2C_Start();
                break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Reading Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 3:
               //Transmit 1st byte for reading
                if(TWSR != 0x40)  // Error?
                 {
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
                  motorread++;
                  if(motorread >= MAX_MOTORS) motorread = 0;
                  I2C_Stop();
                  twi_state = 0;
                 }
                 else
                 {
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
                  I2C_ReceiveByte();
                 }
                MissingMotor = missing_motor;
                missing_motor = 0;
                break;
        case 4: //Read 1st byte and transmit 2nd Byte
                Motor[motorread].Current = TWDR;
                I2C_ReceiveLastByte(); //nack
                break;
        case 5:
                //Read 2nd byte
                Motor[motorread].MaxPWM = TWDR;
                                motorread++; // next motor
                if(motorread >= MAX_MOTORS) motorread = 0;
                I2C_Stop();
                twi_state = 0;
                break;

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// writing Gyro-Offset
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 8:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 9:
                i2c_write_byte(0x10); // Update Channel A
                break;
        case 10:
                i2c_write_byte(AnalogOffsetNick); // Value
                break;
        case 11:
                i2c_write_byte(0x80); // Value
                break;
        case 12:
                I2C_Stop();
                I2CTimeout = 10;
                I2C_Start();
                break;
        case 13:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 14:
                i2c_write_byte(0x12); // Update Channel B
                break;
        case 15:
                i2c_write_byte(AnalogOffsetRoll); // Value
                break;
        case 16:
                i2c_write_byte(0x80); // Value
                break;
        case 17:
                I2C_Stop();
                I2CTimeout = 10;
                I2C_Start();
                break;
        case 18:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 19:
                i2c_write_byte(0x14); // Update Channel C
                break;
        case 20:
                i2c_write_byte(AnalogOffsetGier); // Value
                break;
        case 21:
                i2c_write_byte(0x80); // Value
                break;
        case 22:
                I2C_Stop();
                I2CTimeout = 10;
                twi_state = 0;
                break;
        default: twi_state = 0;
                break;
                }
 TWCR |= 0x80;
}
*/

/*
                if(DataArray[3] & 0x01) Parameter.PwmScaling            = DataArray[4];
                if(DataArray[3] & 0x02) Parameter.MaxStrom                      = DataArray[5];
                if(DataArray[3] & 0x04) Parameter.TemperaturLimiter = DataArray[6];
                if(DataArray[3] & 0x08) Parameter.SkaliereStrom         = DataArray[7];
                if(DataArray[3] & 0x10) Parameter.Bitconfig                     = DataArray[8];
*/

//############################################################################
SIGNAL (TWI_vect)
//############################################################################
{                           // 2    3   4   5   6   7   8  9
 unsigned char test[] = {0,0,'#',0x1F,255,30,99,49,0x00,7,8,9,10};
 static unsigned char missing_motor,send = 0,crc = 0,read_more = 0;

     switch(twi_state++)
        {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Writing the Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 0:
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
                                if(motor == MAX_MOTORS)  // writing finished -> now read
                                {
                                 motor = 0;
                                 twi_state = 4;
                                 i2c_write_byte(0x53+(motorread*2));
                                 }
                                else i2c_write_byte(0x52+(motor*2));
                                send = 0;
                break;
        case 1:
                i2c_write_byte(Motor[motor].SetPoint);
//              if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) /*|| !Motor[motor].SetPointLowerBits*/)
                if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) || !Motor[motor].SetPointLowerBits && !TransmitBlConfig)
                                  twi_state++; // skip
                break;
        case 2:
                if(!send++)
                                 {
                                  i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07);
                                  crc = 0xAA;
                                 }
                                else
                if(send == 9) i2c_write_byte(crc)
                                else
                                 {
                                  crc += test[send];
                                  i2c_write_byte(test[send]);
                                }
                if(TransmitBlConfig && !MotorenEin && motor == motorread && (Motor[motorread].Version & MOTOR_STATE_NEW_PROTOCOL_MASK))
                                 {
                                  if(send <= 10) twi_state--;
                                 }

                break;
        case 3:
                        motor++;
                if(TWSR == 0x30)
                                 {
                                  if(!missing_motor) missing_motor = motor;
                                  if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
                                 }
                I2C_Stop();
                I2CTimeout = 10;
                twi_state = 0;
                I2C_Start();
                break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Reading Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 4:
               //Transmit 1st byte for reading
                if(TWSR != 0x40)  // Error?
                 {
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
                  motorread++;
                  if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; ReadTemperature = ++ReadTemperature & 0x0f;}
                  I2C_Stop();
                  twi_state = 0;
                 }
                 else
                 {
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
                  if(motorread == ReadTemperature || TransmitBlConfig)                         
                                  {
                   read_more = 1;
                   I2C_ReceiveByte();
                                  }
                                  else
                                   {
                                    I2C_ReceiveLastByte();                               
                                        read_more = 0;
                                   }   
                 }
                MissingMotor = missing_motor;
                missing_motor = 0;
                break;
        case 5: //Read 1st byte and transmit 2nd Byte
                Motor[motorread].Current = TWDR;
                if(read_more) I2C_ReceiveByte() //nack
                                else
                                 {
                                        motorread++; // next motor
                                        if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; ReadTemperature = ++ReadTemperature & 0x0f;}
                                        I2C_Stop();
                                        twi_state = 0;
                                 }
                break;
        case 6:
                       //Read 2nd byte and transmit 3rd Byte
                Motor[motorread].MaxPWM = TWDR;
                                if(TWDR == 250)
                                 {
                                  if(!MotorenEin) Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
                                 }
                                 else if(TransmitBlConfig) Motor[motorread].Version = 0;
                                I2C_ReceiveLastByte(); //nack
                break;
        case 7: // read next
                                Motor[motorread].Temperature = TWDR;
                                motorread++; // next motor
                if(motorread >= MAX_MOTORS) { TransmitBlConfig = 0; motorread = 0; }
                I2C_Stop();
                twi_state = 0;
                                break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// writing Gyro-Offset
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 18:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 19:
                i2c_write_byte(0x10); // Update Channel A
                break;
        case 20:
                i2c_write_byte(AnalogOffsetNick); // Value
                break;
        case 21:
                i2c_write_byte(0x80); // Value
                break;
        case 22:
                I2C_Stop();
                I2CTimeout = 10;
                I2C_Start();
                break;
        case 23:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 24:
                i2c_write_byte(0x12); // Update Channel B
                break;
        case 25:
                i2c_write_byte(AnalogOffsetRoll); // Value
                break;
        case 26:
                i2c_write_byte(0x80); // Value
                break;
        case 27:
                I2C_Stop();
                I2CTimeout = 10;
                I2C_Start();
                break;
        case 28:
                i2c_write_byte(0x98); // Address of the DAC
                break;
        case 29:
                i2c_write_byte(0x14); // Update Channel C
                break;
        case 30:
                i2c_write_byte(AnalogOffsetGier); // Value
                break;
        case 31:
                i2c_write_byte(0x80); // Value
                break;
        case 32:
                I2C_Stop();
                I2CTimeout = 10;
                twi_state = 0;
                break;
        default: twi_state = 0;
                break;
                }
 TWCR |= 0x80;
}