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
}