Subversion Repositories FlightCtrl

Rev

Rev 1654 | Rev 1662 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

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


#include "main.h"

volatile unsigned char twi_state = 0;
volatile unsigned char motor = 0;
volatile unsigned char motorread = 0,MissingMotor = 0;
volatile unsigned char BLFlags = 0;

MotorData_t Motor[MAX_MOTORS];


unsigned int I2CError = 0;

//############################################################################
//Initzialisieren der I2C (TWI) Schnittstelle
void i2c_init(void)
//############################################################################
{
        unsigned char i;

        TWSR = 0;
        TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;

        for(i=0; i < MAX_MOTORS; i++)
        {
                Motor[i].Version        = 0;
                Motor[i].SetPoint       = 0;
                Motor[i].SetPointLowerBits      = 0;
                Motor[i].State          = 0;
                Motor[i].Current        = 0;
                Motor[i].MaxPWM         = 0;
                BLConfig[i].SetMask =   MASK_SET_PWM_SCALING|MASK_SET_CURRENT_LIMIT|MASK_SET_TEMP_LIMIT|MASK_SET_CURRENT_SCALING|MASK_SET_BITCONFIG;
                BLConfig[i].PwmScaling = 255;           // MaxPWM
                BLConfig[i].CurrentLimit = 30;          // Current Limit in A
                BLConfig[i].TempLimit = 99;                     // Temperature Limit in °C
                BLConfig[i].CurrentScaling = 64;        // Current Scaling
                BLConfig[i].BitConfig = 0;                      // BitConfig
        }
}

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 = 0, byte_counter = 0, read_more = 0, motorread_temperature = 0;
        static unsigned char *pTxBuff;
J4High;
        switch(twi_state++)
        {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Writing the Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                case 0:
                        while(Mixer.Motor[motor][MIX_GAS] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
                        if(motor == MAX_MOTORS)  // writing finished -> now read
                        {
                                motor = 0;
                                twi_state = 5;
                                i2c_write_byte(0x53+(motorread*2));
                        }
                                else i2c_write_byte(0x52+(motor*2));
                        break;
                case 1:
                        i2c_write_byte(Motor[motor].SetPoint);
                        if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK))
                        {
                                twi_state = 4; //jump over sending more data
                        }
                        else if(!( (Motor[motor].SetPointLowerBits && RequiredMotors < 7) || (BLFlags & BLFLAG_SEND_CONFIG)))
                        {
                                twi_state = 4; // skip state
                        }
                        break;
                case 2: // lower bits of setpoint (higher resolution)
                        i2c_write_byte((Motor[motor].SetPointLowerBits << 1) & 0x07); // send the lower bits of setpoint
                        // transmit config only on demand and the motors are not running and only for one motor per round trip
                        if( (BLFlags & BLFLAG_SEND_CONFIG) && (motor == motorread))
                        {       // prepare sending of configuration
                                byte_counter = 0;       // reset send byte counter
                        }
                        else
                        {       // jump to state for end of transmission for that motor
                                twi_state = 4;
                        }
                        break;
                case 3:
                        if(!byte_counter) // first byte?
                        {
                                pTxBuff = (uint8_t*)&BLConfig[motor]; // select configuration for motor
                                i2c_write_byte(pTxBuff[byte_counter]);
                                twi_state = 3; // keep state 3
                        }
                        else if(byte_counter >= sizeof(BLConfig_t))
                        {
                                i2c_write_byte(0);
                                // jump to case 4
                        }
            else // transmit configuration to BLs
                        {
                                i2c_write_byte(pTxBuff[byte_counter]);  // submit next byte
                                twi_state = 3;// keep state 3
                        }
                        byte_counter++; // next byte
                        break;
        case 4:

                        if(TWSR == 0x30)
                        {
                                if(!missing_motor) missing_motor = motor + 1;
                                if((Motor[motor].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor].State++; // increment error counter and handle overflow
                        }
                        I2C_Stop();
                        I2CTimeout = 10;
                        motor++;
                        twi_state = 0;
                        I2C_Start();
                        break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Reading Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        case 5:
                        //Transmit 1st byte for reading
                        if(TWSR != 0x40)  // Error?
                        {
                                Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
                                if(++motorread >= MAX_MOTORS)
                                {
                                        BLFlags &= ~BLFLAG_SEND_CONFIG;
                                        motorread = 0;
                                        if(++motorread_temperature >= MAX_MOTORS)
                                        {
                                                motorread_temperature = 0;
                                                BLFlags &= ~BLFLAG_READ_VERSION;
                                        }
                                }
                                I2C_Stop();
                                twi_state = 0;
                                BLFlags |= BLFLAG_TX_COMPLETE;
                        }
                        else
                        {
                                Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
                                if( (motorread == motorread_temperature) || (BLFlags & BLFLAG_READ_VERSION) )
                                {
                                        read_more = 1;
                                        I2C_ReceiveByte();
                                }
                                else
                                {
                                        read_more = 0;
                                        I2C_ReceiveLastByte();
                                }
                        }
                        MissingMotor = missing_motor;
                        missing_motor = 0;
                        break;
        case 6: //Read 1st byte and transmit 2nd Byte
                Motor[motorread].Current = TWDR;
                if(read_more)
                {
                                        I2C_ReceiveByte() //ack

                                }
                                else
                                {
                                        if(++motorread >= MAX_MOTORS)
                                        {
                                                motorread = 0;          // restart from beginning
                                                BLFlags &= ~BLFLAG_SEND_CONFIG;
                                                if(++motorread_temperature >= MAX_MOTORS)
                                                {
                                                        motorread_temperature = 0;
                                                        BLFlags &= ~BLFLAG_READ_VERSION;
                                                }
                                        }
                                        I2C_Stop();
                                        twi_state = 0;
                                        BLFlags |= BLFLAG_TX_COMPLETE;
                                }
                break;
        case 7:
                       //Read 2nd byte and transmit 3rd Byte
                Motor[motorread].MaxPWM = TWDR;
                if(BLFlags & BLFLAG_READ_VERSION)
                                {
                                        if(TWDR == 250)
                                        {
                                                if(!MotorenEin) Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
                                        }
                                        else
                                        {
                                                Motor[motorread].Version = 0;
                                        }
                                }
                                I2C_ReceiveLastByte(); //nack
                break;
        case 8: // read next
                                Motor[motorread].Temperature = TWDR;
                                if(++motorread >= MAX_MOTORS)
                                {
                                        motorread = 0; // restart reading of first motor
                                        BLFlags &= ~BLFLAG_SEND_CONFIG;
                                        if(++motorread_temperature >= MAX_MOTORS)
                                        {
                                                motorread_temperature = 0;
                                                BLFlags &= ~BLFLAG_READ_VERSION;
                                        }
                                }
                I2C_Stop();
                BLFlags |= BLFLAG_TX_COMPLETE;
                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;
        J4Low;
}

void I2C_SendBLConfig(void)
{
        unsigned char i;
        if(MotorenEin) return;
        while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
        BLFlags |= BLFLAG_SEND_CONFIG; // enable sending of BL config
        // setpoints should be zero
        for(i = 0; i < MAX_MOTORS; i++)
        {
                Motor[i].SetPoint = 0;
                Motor[i].SetPointLowerBits = 0;
        }
        motorread = 0;
        // needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms)
        do
        {
                twi_state = 0;
                I2C_Start(); // start an i2c transmission
                while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
        }while(BLFlags & BLFLAG_SEND_CONFIG); // repeat until the BL config has been send to all motors
}